commit 36a0da7c292b21b0f6bc5e974d04a88995d9bc74 Author: logzhan <719901725@qq.com> Date: Mon Dec 11 11:51:46 2023 +0800 上传第一版代码 diff --git a/carto_ws/src/CMakeLists.txt b/carto_ws/src/CMakeLists.txt new file mode 100644 index 0000000..cd58121 --- /dev/null +++ b/carto_ws/src/CMakeLists.txt @@ -0,0 +1,69 @@ +# toplevel CMakeLists.txt for a catkin workspace +# catkin/cmake/toplevel.cmake + +cmake_minimum_required(VERSION 3.0.2) + +project(Project) + +set(CATKIN_TOPLEVEL TRUE) + +# search for catkin within the workspace +set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") +execute_process(COMMAND ${_cmd} + RESULT_VARIABLE _res + OUTPUT_VARIABLE _out + ERROR_VARIABLE _err + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_STRIP_TRAILING_WHITESPACE +) +if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) + # searching fot catkin resulted in an error + string(REPLACE ";" " " _cmd_str "${_cmd}") + message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") +endif() + +# include catkin from workspace or via find_package() +if(_res EQUAL 0) + set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") + # include all.cmake without add_subdirectory to let it operate in same scope + include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) + add_subdirectory("${_out}") + +else() + # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument + # or CMAKE_PREFIX_PATH from the environment + if(NOT DEFINED CMAKE_PREFIX_PATH) + if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") + if(NOT WIN32) + string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + else() + set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + endif() + endif() + endif() + + # list of catkin workspaces + set(catkin_search_path "") + foreach(path ${CMAKE_PREFIX_PATH}) + if(EXISTS "${path}/.catkin") + list(FIND catkin_search_path ${path} _index) + if(_index EQUAL -1) + list(APPEND catkin_search_path ${path}) + endif() + endif() + endforeach() + + # search for catkin in all workspaces + set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) + find_package(catkin QUIET + NO_POLICY_SCOPE + PATHS ${catkin_search_path} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + unset(CATKIN_TOPLEVEL_FIND_PACKAGE) + + if(NOT catkin_FOUND) + message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") + endif() +endif() + +catkin_workspace() diff --git a/carto_ws/src/cartographer-master/.bazelci/presubmit.yml b/carto_ws/src/cartographer-master/.bazelci/presubmit.yml new file mode 100644 index 0000000..b7a02ef --- /dev/null +++ b/carto_ws/src/cartographer-master/.bazelci/presubmit.yml @@ -0,0 +1,7 @@ +--- +platforms: + ubuntu1604: + build_targets: + - "..." + test_targets: + - "..." diff --git a/carto_ws/src/cartographer-master/.bazelrc b/carto_ws/src/cartographer-master/.bazelrc new file mode 100644 index 0000000..eafe5c8 --- /dev/null +++ b/carto_ws/src/cartographer-master/.bazelrc @@ -0,0 +1,23 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Do an optimized build by default, or otherwise Cartographer cannot run +# real-time and certain tests will timeout. +build -c opt + +# By default, an optimized C++ build with Bazel will build each library twice, +# with and without -fPIC. --force_pic avoids the unnecessary actions and +# reduces build time. +build --force_pic + diff --git a/carto_ws/src/cartographer-master/.dockerignore b/carto_ws/src/cartographer-master/.dockerignore new file mode 100644 index 0000000..6647fc9 --- /dev/null +++ b/carto_ws/src/cartographer-master/.dockerignore @@ -0,0 +1,4 @@ +**/Dockerfile* +**/.dockerignore +**/.git +**/.travis.yml diff --git a/carto_ws/src/cartographer-master/.github/PULL_REQUEST_TEMPLATE.md b/carto_ws/src/cartographer-master/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000..ce73cbd --- /dev/null +++ b/carto_ws/src/cartographer-master/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,2 @@ +Want to contribute? Great! Make sure you've read and understood +[CONTRIBUTING.md](https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md). diff --git a/carto_ws/src/cartographer-master/.gitignore b/carto_ws/src/cartographer-master/.gitignore new file mode 100644 index 0000000..d1f961a --- /dev/null +++ b/carto_ws/src/cartographer-master/.gitignore @@ -0,0 +1,2 @@ +build +bazel-* diff --git a/carto_ws/src/cartographer-master/.travis.yml b/carto_ws/src/cartographer-master/.travis.yml new file mode 100644 index 0000000..8dc1031 --- /dev/null +++ b/carto_ws/src/cartographer-master/.travis.yml @@ -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. + +sudo: required +services: docker + +# Cache intermediate Docker layers. For a description of how this works, see: +# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html +cache: + directories: + - /home/travis/docker/ + +env: + - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ + - LSB_RELEASE=focal DOCKER_CACHE_FILE=/home/travis/docker/focal-cache.tar.gz CC=gcc CXX=g++ + - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ + - LSB_RELEASE=buster DOCKER_CACHE_FILE=/home/travis/docker/buster-cache.tar.gz CC=gcc CXX=g++ + +before_install: scripts/load_docker_cache.sh + +install: true +script: + - docker build ${TRAVIS_BUILD_DIR} -t cartographer:${LSB_RELEASE} -f Dockerfile.${LSB_RELEASE} + --build-arg cc=$CC --build-arg cxx=$CXX + - scripts/save_docker_cache.sh diff --git a/carto_ws/src/cartographer-master/AUTHORS b/carto_ws/src/cartographer-master/AUTHORS new file mode 100644 index 0000000..663aac4 --- /dev/null +++ b/carto_ws/src/cartographer-master/AUTHORS @@ -0,0 +1,7 @@ +# This is the list of Cartographer authors for copyright purposes. +# +# This does not necessarily list everyone who has contributed code, since in +# some cases, their employer may be the copyright holder. To see the full list +# of contributors, see the revision history in source control. +Google Inc. +and other contributors diff --git a/carto_ws/src/cartographer-master/BUILD.bazel b/carto_ws/src/cartographer-master/BUILD.bazel new file mode 100644 index 0000000..00ec4ef --- /dev/null +++ b/carto_ws/src/cartographer-master/BUILD.bazel @@ -0,0 +1,30 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cartographer is a system that provides real-time simultaneous localization +# and mapping (SLAM) in 2D and 3D across multiple platforms and sensor +# configurations. + +licenses(["notice"]) # Apache 2.0 + +package(default_visibility = ["//visibility:public"]) + +exports_files(["LICENSE"]) + +filegroup( + name = "configuration_files", + srcs = glob([ + "configuration_files/*.lua", + ]), +) diff --git a/carto_ws/src/cartographer-master/CHANGELOG.rst b/carto_ws/src/cartographer-master/CHANGELOG.rst new file mode 100644 index 0000000..05e6da5 --- /dev/null +++ b/carto_ws/src/cartographer-master/CHANGELOG.rst @@ -0,0 +1,23 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cartographer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2021-03-09) +------------------ +https://github.com/cartographer-project/cartographer/compare/1.0.0...2.0.0 + +1.0.0 (2018-06-01) +------------------ +https://github.com/googlecartographer/cartographer/compare/0.3.0...1.0.0 + +0.3.0 (2017-11-23) +------------------ +https://github.com/googlecartographer/cartographer/compare/0.2.0...0.3.0 + +0.2.0 (2017-06-19) +------------------ +https://github.com/googlecartographer/cartographer/compare/0.1.0...0.2.0 + +0.1.0 (2017-05-18) +------------------ +* First unstable development release diff --git a/carto_ws/src/cartographer-master/CMakeLists.txt b/carto_ws/src/cartographer-master/CMakeLists.txt new file mode 100644 index 0000000..ef2fcb3 --- /dev/null +++ b/carto_ws/src/cartographer-master/CMakeLists.txt @@ -0,0 +1,389 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT 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 3.2) + +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}) +option(BUILD_GRPC "build Cartographer gRPC support" false) +set(CARTOGRAPHER_HAS_GRPC ${BUILD_GRPC}) +option(BUILD_PROMETHEUS "build Prometheus monitoring support" false) + +include("${PROJECT_SOURCE_DIR}/cmake/functions.cmake") +google_initialize_cartographer_project() +google_enable_testing() + +find_package(absl REQUIRED) +set(BOOST_COMPONENTS iostreams) +if(WIN32) + list(APPEND BOOST_COMPONENTS zlib) + set(Boost_USE_STATIC_LIBS FALSE) +endif() +find_package(Boost REQUIRED COMPONENTS ${BOOST_COMPONENTS}) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) +find_package(Eigen3 REQUIRED) +find_package(LuaGoogle REQUIRED) +if(WIN32) + # On Windows, Protobuf is incorrectly found by the bundled CMake module, so prefer native CMake config. + set(protobuf_MODULE_COMPATIBLE TRUE CACHE INTERNAL "") + find_package(Protobuf 3.0.0 CONFIG) +else() + find_package(Protobuf 3.0.0 REQUIRED) +endif() + +if (${BUILD_GRPC}) + find_package(async_grpc REQUIRED) +endif() + +if(${BUILD_PROMETHEUS}) + find_package( ZLIB REQUIRED ) +endif() + +include(FindPkgConfig) +if (NOT WIN32) + PKG_SEARCH_MODULE(CAIRO REQUIRED cairo>=1.12.16) +else() + find_library(CAIRO_LIBRARIES cairo) +endif() + +# Only build the documentation if we can find Sphinx. +find_package(Sphinx) +if(SPHINX_FOUND) + add_subdirectory("docs") +endif() + +# Install catkin package.xml +install(FILES package.xml DESTINATION share/cartographer) + +set(CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY ${CMAKE_INSTALL_PREFIX}/share/cartographer/configuration_files + CACHE PATH ".lua configuration files directory") + +install(DIRECTORY configuration_files DESTINATION share/cartographer/) + +install(DIRECTORY cmake DESTINATION share/cartographer/) + +file(GLOB_RECURSE ALL_LIBRARY_HDRS "cartographer/*.h") +file(GLOB_RECURSE ALL_LIBRARY_SRCS "cartographer/*.cc") +file(GLOB_RECURSE TEST_LIBRARY_HDRS "cartographer/fake_*.h" "cartographer/*test_helpers*.h" "cartographer/mock_*.h") +file(GLOB_RECURSE TEST_LIBRARY_SRCS "cartographer/fake_*.cc" "cartographer/*test_helpers*.cc" "cartographer/mock_*.cc") +file(GLOB_RECURSE ALL_TESTS "cartographer/*_test.cc") +file(GLOB_RECURSE ALL_EXECUTABLES "cartographer/*_main.cc") + +# Remove dotfiles/-folders that could potentially pollute the build. +file(GLOB_RECURSE ALL_DOTFILES ".*/*") +if (ALL_DOTFILES) + list(REMOVE_ITEM ALL_LIBRARY_HDRS ${ALL_DOTFILES}) + list(REMOVE_ITEM ALL_LIBRARY_SRCS ${ALL_DOTFILES}) + list(REMOVE_ITEM TEST_LIBRARY_HDRS ${ALL_DOTFILES}) + list(REMOVE_ITEM TEST_LIBRARY_SRCS ${ALL_DOTFILES}) + list(REMOVE_ITEM ALL_TESTS ${ALL_DOTFILES}) + list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_DOTFILES}) +endif() +list(REMOVE_ITEM ALL_LIBRARY_SRCS ${ALL_EXECUTABLES}) +list(REMOVE_ITEM ALL_LIBRARY_SRCS ${ALL_TESTS}) +list(REMOVE_ITEM ALL_LIBRARY_HDRS ${TEST_LIBRARY_HDRS}) +list(REMOVE_ITEM ALL_LIBRARY_SRCS ${TEST_LIBRARY_SRCS}) +file(GLOB_RECURSE ALL_GRPC_FILES "cartographer/cloud/*") +file(GLOB_RECURSE ALL_PROMETHEUS_FILES "cartographer/cloud/metrics/prometheus/*") +list(REMOVE_ITEM ALL_GRPC_FILES ${ALL_PROMETHEUS_FILES}) +if (NOT ${BUILD_GRPC}) + list(REMOVE_ITEM ALL_LIBRARY_HDRS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM ALL_LIBRARY_SRCS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM TEST_LIBRARY_HDRS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM TEST_LIBRARY_SRCS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES}) +endif() +if (NOT ${BUILD_PROMETHEUS}) + list(REMOVE_ITEM ALL_LIBRARY_HDRS ${ALL_PROMETHEUS_FILES}) + list(REMOVE_ITEM ALL_LIBRARY_SRCS ${ALL_PROMETHEUS_FILES}) + list(REMOVE_ITEM TEST_LIBRARY_HDRS ${ALL_PROMETHEUS_FILES}) + list(REMOVE_ITEM TEST_LIBRARY_SRCS ${ALL_PROMETHEUS_FILES}) + list(REMOVE_ITEM ALL_TESTS ${ALL_PROMETHEUS_FILES}) + list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_PROMETHEUS_FILES}) +endif() +set(INSTALL_SOURCE_HDRS ${ALL_LIBRARY_HDRS} ${TEST_LIBRARY_HDRS}) +file(GLOB_RECURSE INTERNAL_HDRS "cartographer/*/internal/*.h") +list(REMOVE_ITEM INSTALL_SOURCE_HDRS ${INTERNAL_HDRS}) + +file(GLOB_RECURSE ALL_PROTOS "cartographer/*.proto") +file(GLOB_RECURSE ALL_GRPC_SERVICES "cartographer/*_service.proto") +list(REMOVE_ITEM ALL_PROTOS ALL_GRPC_SERVICES) +if (NOT ${BUILD_GRPC}) + list(REMOVE_ITEM ALL_PROTOS ${ALL_GRPC_FILES}) +endif() + +# TODO(cschuet): Move proto compilation to separate function. +set(ALL_PROTO_SRCS) +set(ALL_PROTO_HDRS) +foreach(ABS_FIL ${ALL_PROTOS}) + file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${ABS_FIL}) + get_filename_component(DIR ${REL_FIL} DIRECTORY) + get_filename_component(FIL_WE ${REL_FIL} NAME_WE) + + list(APPEND ALL_PROTO_SRCS "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.cc") + list(APPEND ALL_PROTO_HDRS "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.h") + + add_custom_command( + OUTPUT "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.cc" + "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${PROJECT_BINARY_DIR} -I + ${PROJECT_SOURCE_DIR} ${ABS_FIL} + DEPENDS ${ABS_FIL} + COMMENT "Running C++ protocol buffer compiler on ${ABS_FIL}" + VERBATIM + ) +endforeach() +set_source_files_properties(${ALL_PROTO_SRCS} ${ALL_PROTO_HDRS} PROPERTIES GENERATED TRUE) +list(APPEND ALL_LIBRARY_HDRS ${ALL_PROTO_HDRS}) +list(APPEND ALL_LIBRARY_SRCS ${ALL_PROTO_SRCS}) + +if(${BUILD_GRPC}) + set(ALL_GRPC_SERVICE_SRCS) + set(ALL_GRPC_SERVICE_HDRS) + foreach(ABS_FIL ${ALL_GRPC_SERVICES}) + file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${ABS_FIL}) + get_filename_component(DIR ${REL_FIL} DIRECTORY) + get_filename_component(FIL_WE ${REL_FIL} NAME_WE) + + list(APPEND ALL_GRPC_SERVICE_SRCS "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.cc") + list(APPEND ALL_GRPC_SERVICE_HDRS "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.h") + + add_custom_command( + OUTPUT "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.cc" + "${PROJECT_BINARY_DIR}/${DIR}/${FIL_WE}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${PROJECT_BINARY_DIR} + -I ${PROJECT_SOURCE_DIR} + ${ABS_FIL} + DEPENDS ${ABS_FIL} + COMMENT "Running C++ protocol buffer compiler on ${ABS_FIL}" + VERBATIM + ) + endforeach() + set_source_files_properties(${ALL_GRPC_SERVICE_SRCS} ${ALL_GRPC_SERVICE_HDRS} PROPERTIES GENERATED TRUE) + list(APPEND ALL_LIBRARY_HDRS ${ALL_GRPC_SERVICE_HDRS}) + list(APPEND ALL_LIBRARY_SRCS ${ALL_GRPC_SERVICE_SRCS}) +endif() +set(INSTALL_GENERATED_HDRS ${ALL_PROTO_HDRS} ${ALL_GRPC_SERVICE_HDRS}) + +add_library(${PROJECT_NAME} STATIC ${ALL_LIBRARY_HDRS} ${ALL_LIBRARY_SRCS}) + +configure_file( + ${PROJECT_SOURCE_DIR}/cartographer/common/config.h.cmake + ${PROJECT_BINARY_DIR}/cartographer/common/config.h) + +google_binary(cartographer_autogenerate_ground_truth + SRCS + cartographer/ground_truth/autogenerate_ground_truth_main.cc +) + +google_binary(cartographer_compute_relations_metrics + SRCS + cartographer/ground_truth/compute_relations_metrics_main.cc +) + +google_binary(cartographer_pbstream + SRCS + cartographer/io/pbstream_main.cc +) + +google_binary(cartographer_print_configuration + SRCS + cartographer/common/print_configuration_main.cc +) + +if(${BUILD_GRPC}) + google_binary(cartographer_grpc_server + SRCS + cartographer/cloud/map_builder_server_main.cc + ) + target_link_libraries(cartographer_grpc_server PUBLIC grpc++) + target_link_libraries(cartographer_grpc_server PUBLIC async_grpc) + if(${BUILD_PROMETHEUS}) + target_link_libraries(cartographer_grpc_server PUBLIC ${ZLIB_LIBRARIES}) + target_link_libraries(cartographer_grpc_server PUBLIC prometheus-cpp-core) + target_link_libraries(cartographer_grpc_server PUBLIC prometheus-cpp-pull) + endif() +endif() + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${EIGEN3_INCLUDE_DIR}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${CERES_INCLUDE_DIRS}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${CERES_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${LUA_INCLUDE_DIR}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${LUA_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${Boost_INCLUDE_DIRS}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) + +if (WIN32) + find_package(glog REQUIRED) + set(GLOG_LIBRARY glog::glog) +else() + set(GLOG_LIBRARY glog) +endif() + +target_link_libraries(${PROJECT_NAME} PUBLIC ${GLOG_LIBRARY}) +target_link_libraries(${PROJECT_NAME} PUBLIC gflags) +if(WIN32) + # Needed to fix conflict with MSVC's error macro. + target_compile_definitions(${PROJECT_NAME} PUBLIC -DGLOG_NO_ABBREVIATED_SEVERITIES) +endif() +if(MSVC) + # Needed for VS 2017 5.8 + target_compile_definitions(${PROJECT_NAME} PUBLIC -D_ENABLE_EXTENDED_ALIGNED_STORAGE -D_USE_MATH_DEFINES) +endif() + +if("${CAIRO_INCLUDE_DIRS}") + target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${CAIRO_INCLUDE_DIRS}") +endif() +target_link_libraries(${PROJECT_NAME} PUBLIC ${CAIRO_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + ${PROTOBUF_INCLUDE_DIR}) +# TODO(hrapp): This should not explicitly list pthread and use +# PROTOBUF_LIBRARIES, but that failed on first try. +target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} + absl::algorithm + absl::base + absl::debugging + absl::flat_hash_map + absl::memory + absl::meta + absl::numeric + absl::str_format + absl::strings + absl::synchronization + absl::time + absl::utility +) +if (NOT WIN32) + target_link_libraries(${PROJECT_NAME} PUBLIC pthread) +endif() +if(${BUILD_GRPC}) + target_link_libraries(${PROJECT_NAME} PUBLIC grpc++) + target_link_libraries(${PROJECT_NAME} PUBLIC async_grpc) +endif() +if(${BUILD_PROMETHEUS}) + target_link_libraries(${PROJECT_NAME} PUBLIC ${ZLIB_LIBRARIES}) + target_link_libraries(${PROJECT_NAME} PUBLIC prometheus-cpp-core) + target_link_libraries(${PROJECT_NAME} PUBLIC prometheus-cpp-pull) + target_compile_definitions(${PROJECT_NAME} PUBLIC USE_PROMETHEUS=1) +endif() + +set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") +set_target_properties(${PROJECT_NAME} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + +set(TEST_LIB + cartographer_test_library +) +add_library(${TEST_LIB} ${TEST_LIBRARY_HDRS} ${TEST_LIBRARY_SRCS}) +target_include_directories(${TEST_LIB} SYSTEM PRIVATE + "${GMOCK_INCLUDE_DIRS}") +# Needed for dynamically linked GTest on Windows. +if (WIN32) + target_compile_definitions(${TEST_LIB} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY) +endif() +target_link_libraries(${TEST_LIB} PUBLIC ${GMOCK_LIBRARY}) +target_link_libraries(${TEST_LIB} PUBLIC ${PROJECT_NAME}) +set_target_properties(${TEST_LIB} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + +foreach(ABS_FIL ${ALL_TESTS}) + file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${ABS_FIL}) + get_filename_component(DIR ${REL_FIL} DIRECTORY) + get_filename_component(FIL_WE ${REL_FIL} NAME_WE) + # Replace slashes as required for CMP0037. + string(REPLACE "/" "." TEST_TARGET_NAME "${DIR}/${FIL_WE}") + google_test("${TEST_TARGET_NAME}" ${ABS_FIL}) + if(${BUILD_GRPC}) + target_link_libraries("${TEST_TARGET_NAME}" PUBLIC grpc++) + target_link_libraries("${TEST_TARGET_NAME}" PUBLIC async_grpc) + endif() + if(${BUILD_PROMETHEUS}) + target_link_libraries("${TEST_TARGET_NAME}" PUBLIC ${ZLIB_LIBRARIES}) + target_link_libraries("${TEST_TARGET_NAME}" PUBLIC prometheus-cpp-core) + target_link_libraries("${TEST_TARGET_NAME}" PUBLIC prometheus-cpp-pull) + endif() + target_link_libraries("${TEST_TARGET_NAME}" PUBLIC ${TEST_LIB}) +endforeach() + +# Add the binary directory first, so that port.h is included after it has +# been generated. +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + $ +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT CartographerExport + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +foreach(HDR ${INSTALL_SOURCE_HDRS}) + file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR}) + get_filename_component(DIR ${REL_FIL} DIRECTORY) + install( + FILES ${HDR} + DESTINATION include/${DIR} + ) +endforeach() + +foreach(HDR ${INSTALL_GENERATED_HDRS}) + file(RELATIVE_PATH REL_FIL ${PROJECT_BINARY_DIR} ${HDR}) + get_filename_component(DIR ${REL_FIL} DIRECTORY) + install( + FILES ${HDR} + DESTINATION include/${DIR} + ) +endforeach() + +set(CARTOGRAPHER_CMAKE_DIR share/cartographer/cmake) +include(CMakePackageConfigHelpers) +configure_package_config_file( + cartographer-config.cmake.in + ${PROJECT_BINARY_DIR}/cmake/cartographer/cartographer-config.cmake + PATH_VARS CARTOGRAPHER_CMAKE_DIR + INSTALL_DESTINATION ${CMAKE_INSTALL_PREFIX}/share/cartographer +) + +install( + EXPORT CartographerExport + DESTINATION share/cartographer/cmake/ + FILE CartographerTargets.cmake +) + +install( + FILES ${PROJECT_BINARY_DIR}/cmake/cartographer/cartographer-config.cmake + DESTINATION share/cartographer/ +) diff --git a/carto_ws/src/cartographer-master/CONTRIBUTING.md b/carto_ws/src/cartographer-master/CONTRIBUTING.md new file mode 100644 index 0000000..8495b52 --- /dev/null +++ b/carto_ws/src/cartographer-master/CONTRIBUTING.md @@ -0,0 +1,51 @@ +Want to contribute? Great! First, read this page. + +### Before you contribute + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0): + +``` +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. +``` + +### Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). +You can sign-off a commit via `git commit -s`. + +### Code reviews + +All submissions, including submissions by project members, require review. +We use GitHub pull requests for this purpose. Make sure you've read, +understood and considered all the points below before creating your PR. + +#### Style guide + +C++ code should adhere to the +[Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). +You can handle the formatting part of the style guide via `git clang-format`. + +#### Best practices + +When preparing your PR and also during the code review make sure to follow +[best practices](https://google.github.io/eng-practices/review/developer/). +Most importantly, keep your PR under 200 lines of code and address a single +concern. + +#### Testing + +- Add unit tests and documentation (these do not count toward your 200 lines). +- Run `ninja test` or `catkin_make_isolated --install --use-ninja --pkg cartographer --make-args test` as appropriate. +- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis + runs for every update which blocks merging of other changes. diff --git a/carto_ws/src/cartographer-master/Dockerfile.bionic b/carto_ws/src/cartographer-master/Dockerfile.bionic new file mode 100644 index 0000000..7066e6a --- /dev/null +++ b/carto_ws/src/cartographer-master/Dockerfile.bionic @@ -0,0 +1,41 @@ +# Copyright 2020 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM ubuntu:bionic + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_proto3.sh cartographer/scripts/ +RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf +COPY scripts/install_grpc.sh cartographer/scripts/ +RUN cartographer/scripts/install_grpc.sh && rm -rf grpc +COPY scripts/install_async_grpc.sh cartographer/scripts/ +RUN cartographer/scripts/install_async_grpc.sh && rm -rf async_grpc +COPY scripts/install_prometheus_cpp.sh cartographer/scripts/ +RUN cartographer/scripts/install_prometheus_cpp.sh && rm -rf prometheus-cpp +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake_with_grpc.sh && rm -rf cartographer diff --git a/carto_ws/src/cartographer-master/Dockerfile.buster b/carto_ws/src/cartographer-master/Dockerfile.buster new file mode 100644 index 0000000..f587e9e --- /dev/null +++ b/carto_ws/src/cartographer-master/Dockerfile.buster @@ -0,0 +1,32 @@ +# Copyright 2020 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM debian:buster + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo. +RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/carto_ws/src/cartographer-master/Dockerfile.focal b/carto_ws/src/cartographer-master/Dockerfile.focal new file mode 100644 index 0000000..4abbe56 --- /dev/null +++ b/carto_ws/src/cartographer-master/Dockerfile.focal @@ -0,0 +1,33 @@ +# Copyright 2020 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM ubuntu:focal + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/carto_ws/src/cartographer-master/Dockerfile.stretch b/carto_ws/src/cartographer-master/Dockerfile.stretch new file mode 100644 index 0000000..b3f346e --- /dev/null +++ b/carto_ws/src/cartographer-master/Dockerfile.stretch @@ -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. + +FROM debian:stretch + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo. +RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_ceres.sh cartographer/scripts/ +RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver +COPY scripts/install_proto3.sh cartographer/scripts/ +RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/carto_ws/src/cartographer-master/Dockerfile.trusty.bazel b/carto_ws/src/cartographer-master/Dockerfile.trusty.bazel new file mode 100644 index 0000000..9e296e8 --- /dev/null +++ b/carto_ws/src/cartographer-master/Dockerfile.trusty.bazel @@ -0,0 +1,27 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM ubuntu:trusty + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +COPY scripts/install_debs_bazel.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_bazel.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_bazel.sh && rm -rf cartographer diff --git a/carto_ws/src/cartographer-master/LICENSE b/carto_ws/src/cartographer-master/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/carto_ws/src/cartographer-master/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/carto_ws/src/cartographer-master/README.rst b/carto_ws/src/cartographer-master/README.rst new file mode 100644 index 0000000..1b216cb --- /dev/null +++ b/carto_ws/src/cartographer-master/README.rst @@ -0,0 +1,101 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +============ +Cartographer +============ + +|build| |docs| |license| + +Purpose +======= + +`Cartographer`_ is a system that provides real-time simultaneous localization +and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor +configurations. + +|video| + +.. _Cartographer: https://github.com/cartographer-project/cartographer +.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping + +Getting started +=============== + +* Learn to use Cartographer at `our Read the Docs site`_. +* You can ask a question by `creating an issue`_. + +.. _our Read the Docs site: https://google-cartographer.readthedocs.io +.. _creating an issue: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question + +Contributing +============ + +You can find information about contributing to Cartographer at `our Contribution +page`_. + +.. _our Contribution page: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md + +Open house slide archive +======================== + +In the past there had been regular open-for-all meetings to discuss progress and plans for Cartographer. +Slides of these Cartographer Open House meetings are listed below. + +- March 14, 2019: `Slides `_ +- February 21, 2019: `Slides `_ +- January 17, 2019: `Slides `_ +- November 22, 2018: `Slides `_ +- October 25, 2018: `Slides `_ +- September 13, 2018: `Slides `_ +- August 16, 2018: `Slides `_ +- August 2, 2018: `Slides `_ +- July 5, 2018: `Slides `_ +- June 21, 2018: `Slides `_ +- June 7, 2018: `Slides `_ +- May 24, 2018: `Slides `_ +- May 3, 2018: `Slides `_ +- March 29, 2018: `Slides `_ +- February 22, 2018: `Slides `_ +- February 8, 2018: `Slides `_ +- January 18, 2018: `Slides `_ +- January 11, 2018: `Slides `_ +- December 7, 2017: `Slides `_ +- November 23, 2017: `Slides `_ +- November 9, 2017: `Slides `_ +- October 26, 2017: `Slides `_ +- October 12, 2017: `Slides `_ +- September 14, 2017: `Slides `_ +- August 17, 2017: `Slides `_ +- July 20, 2017: `Slides `_ +- July 6, 2017: `Slides `_ +- June 22, 2017: `Slides `_ +- June 8, 2017: `Slides `_ + +.. |build| image:: https://travis-ci.org/cartographer-project/cartographer.svg?branch=master + :alt: Build Status + :scale: 100% + :target: https://travis-ci.org/cartographer-project/cartographer +.. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest + :alt: Documentation Status + :scale: 100% + :target: https://google-cartographer.readthedocs.io/en/latest/?badge=latest +.. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg + :alt: Apache 2 license. + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/blob/master/LICENSE +.. |video| image:: https://j.gifs.com/wp3BJM.gif + :alt: Cartographer 3D SLAM Demo + :scale: 100% + :target: https://youtu.be/DM0dpHLhtX0 diff --git a/carto_ws/src/cartographer-master/RELEASING.rst b/carto_ws/src/cartographer-master/RELEASING.rst new file mode 100644 index 0000000..19a8726 --- /dev/null +++ b/carto_ws/src/cartographer-master/RELEASING.rst @@ -0,0 +1,15 @@ +Steps for Releasing +------------------- + +.. code-block:: bash + catkin_generate_changelog + +* Update changelog to point to GitHub release log (e.g. + https://github.com/cartographer-project/cartographer/compare/0.1.0...0.2.0) + +.. code-block:: bash + git commit -am"Update changelog for release" + catkin_prepare_release --bump minor --no-push + +* Create PR +* Add release via GitHub web UI diff --git a/carto_ws/src/cartographer-master/WORKSPACE b/carto_ws/src/cartographer-master/WORKSPACE new file mode 100644 index 0000000..94af9ec --- /dev/null +++ b/carto_ws/src/cartographer-master/WORKSPACE @@ -0,0 +1,33 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +workspace(name = "com_github_googlecartographer_cartographer") + +load("//:bazel/repositories.bzl", "cartographer_repositories") + +cartographer_repositories() + +# This can't be inside cartographer_repositories() because of: +# https://github.com/bazelbuild/bazel/issues/1550 +load("@com_github_nelhage_rules_boost//:boost/boost.bzl", "boost_deps") + +boost_deps() + +load("@com_github_grpc_grpc//bazel:grpc_deps.bzl", "grpc_deps") + +grpc_deps() + +load("@com_github_jupp0r_prometheus_cpp//:repositories.bzl", "prometheus_cpp_repositories") + +prometheus_cpp_repositories() diff --git a/carto_ws/src/cartographer-master/azure-pipelines.yml b/carto_ws/src/cartographer-master/azure-pipelines.yml new file mode 100644 index 0000000..cd6c908 --- /dev/null +++ b/carto_ws/src/cartographer-master/azure-pipelines.yml @@ -0,0 +1,60 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +jobs: +- job: Build + pool: + vmImage: 'vs2017-win2016' + timeoutInMinutes: 360 + steps: + - script: | + choco sources add -n=roswin -s https://aka.ms/ros/public --priority 1 + rem Azure VM runs out of space on C:, so use D: for ros and rosdeps + mkdir D:\opt && mklink /J C:\opt D:\opt + choco upgrade %ROS_METAPACKAGE% -y + robocopy "." ".\src\cartographer" /E /MOVE /XD "src" > NUL + call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y + env: + ROS_METAPACKAGE: 'ros-melodic-desktop' + displayName: Install prerequisites + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + catkin_make_isolated --use-ninja --install + displayName: Build + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + cd build_isolated\cartographer\install && ctest --no-compress-output -T Test + displayName: Run tests + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer\install + displayName: Convert tests to jUnit + condition: always() + + - task: PublishTestResults@2 + displayName: Publish test results + inputs: + testRunner: 'jUnit' + testResultsFiles: '**\jUnit.xml' + searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer\install\Testing' + condition: always() diff --git a/carto_ws/src/cartographer-master/bazel/repositories.bzl b/carto_ws/src/cartographer-master/bazel/repositories.bzl new file mode 100644 index 0000000..c4bd67e --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/repositories.bzl @@ -0,0 +1,302 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""External dependencies for Cartographer.""" + +load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") + +def cartographer_repositories(): + _maybe( + http_archive, + name = "com_github_nelhage_rules_boost", + sha256 = "371f49e7b29e44a718baf8b9a2dd3eca865005a851c9ecf8fb6a10a715aa58dd", + strip_prefix = "rules_boost-a5a95642f6097f8949020646ffe89d7243008981", + urls = [ + "https://mirror.bazel.build/github.com/nelhage/rules_boost/archive/a5a95642f6097f8949020646ffe89d7243008981.tar.gz", + "https://github.com/nelhage/rules_boost/archive/a5a95642f6097f8949020646ffe89d7243008981.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_github_antonovvk_bazel_rules", + sha256 = "2f5327a2dc9a0cc8ead93953a5d2ae2e0308aece685e46cc89c27538a7e9a73a", + strip_prefix = "bazel_rules-c76e47ebe6f0a03b9dd99e245d5a0611813c36f9", + urls = [ + "https://github.com/drigz/bazel_rules/archive/c76e47ebe6f0a03b9dd99e245d5a0611813c36f9.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_github_gflags_gflags", + sha256 = "6e16c8bc91b1310a44f3965e616383dbda48f83e8c1eaa2370a215057b00cabe", + strip_prefix = "gflags-77592648e3f3be87d6c7123eb81cbad75f9aef5a", + urls = [ + "https://mirror.bazel.build/github.com/gflags/gflags/archive/77592648e3f3be87d6c7123eb81cbad75f9aef5a.tar.gz", + "https://github.com/gflags/gflags/archive/77592648e3f3be87d6c7123eb81cbad75f9aef5a.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_google_glog", + sha256 = "dfc074b41a5b86fc5dda4f0e2e2d6cc5b21f798c9fcc8ed5fea9c8f7c4613be6", + strip_prefix = "glog-dd2b93d761a19860190cb3fa92066c8031e912e3", + urls = [ + "https://mirror.bazel.build/github.com/google/glog/archive/dd2b93d761a19860190cb3fa92066c8031e912e3.tar.gz", + "https://github.com/google/glog/archive/dd2b93d761a19860190cb3fa92066c8031e912e3.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "net_zlib_zlib", + sha256 = "6d4d6640ca3121620995ee255945161821218752b551a1a180f4215f7d124d45", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:zlib.BUILD", + strip_prefix = "zlib-cacf7f1d4e3d44d871b605da3b647f07d718623f", + urls = [ + "https://mirror.bazel.build/github.com/madler/zlib/archive/cacf7f1d4e3d44d871b605da3b647f07d718623f.tar.gz", + "https://github.com/madler/zlib/archive/cacf7f1d4e3d44d871b605da3b647f07d718623f.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_cairographics_pixman", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party/pixman:pixman.BUILD", + sha256 = "21b6b249b51c6800dc9553b65106e1e37d0e25df942c90531d4c3997aa20a88e", + strip_prefix = "pixman-0.34.0", + urls = [ + "https://mirror.bazel.build/www.cairographics.org/releases/pixman-0.34.0.tar.gz", + "https://www.cairographics.org/releases/pixman-0.34.0.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_cairographics_cairo", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party/cairo:cairo.BUILD", + sha256 = "7e87878658f2c9951a14fc64114d4958c0e65ac47530b8ac3078b2ce41b66a09", + strip_prefix = "cairo-1.14.10", + urls = [ + "https://mirror.bazel.build/www.cairographics.org/releases/cairo-1.14.10.tar.xz", + "https://www.cairographics.org/releases/cairo-1.14.10.tar.xz", + ], + ) + + _maybe( + http_archive, + name = "org_freetype_freetype2", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:freetype2.BUILD", + sha256 = "33a28fabac471891d0523033e99c0005b95e5618dc8ffa7fa47f9dadcacb1c9b", + strip_prefix = "freetype-2.8", + urls = [ + "https://mirror.bazel.build/download.savannah.gnu.org/releases/freetype/freetype-2.8.tar.gz", + "http://download.savannah.gnu.org/releases/freetype/freetype-2.8.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_libgd_libgd", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:gd.BUILD", + sha256 = "a66111c9b4a04e818e9e2a37d7ae8d4aae0939a100a36b0ffb52c706a09074b5", + strip_prefix = "libgd-2.2.5", + urls = [ + "https://mirror.bazel.build/github.com/libgd/libgd/releases/download/gd-2.2.5/libgd-2.2.5.tar.gz", + "https://github.com/libgd/libgd/releases/download/gd-2.2.5/libgd-2.2.5.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_freedesktop_fontconfig", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party/fontconfig:fontconfig.BUILD", + sha256 = "fd5a6a663f4c4a00e196523902626654dd0c4a78686cbc6e472f338e50fdf806", + strip_prefix = "fontconfig-2.12.4", + urls = [ + "https://mirror.bazel.build/www.freedesktop.org/software/fontconfig/release/fontconfig-2.12.4.tar.gz", + "https://www.freedesktop.org/software/fontconfig/release/fontconfig-2.12.4.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_ceres_solver_ceres_solver", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:ceres.BUILD", + sha256 = "ede5b4205ee8d7c7e029e9da74c7ee759fee6961f7e6bfa694274e4a55b8c7ca", + strip_prefix = "ceres-solver-58c5edae2f7c4d2533fe8a975c1f5f0b892dfd3e", + urls = [ + "https://mirror.bazel.build/github.com/ceres-solver/ceres-solver/archive/58c5edae2f7c4d2533fe8a975c1f5f0b892dfd3e.tar.gz", + "https://github.com/ceres-solver/ceres-solver/archive/58c5edae2f7c4d2533fe8a975c1f5f0b892dfd3e.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_tuxfamily_eigen", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:eigen.BUILD", + sha256 = "ca7beac153d4059c02c8fc59816c82d54ea47fe58365e8aded4082ded0b820c4", + strip_prefix = "eigen-eigen-f3a22f35b044", + urls = [ + "http://mirror.bazel.build/bitbucket.org/eigen/eigen/get/f3a22f35b044.tar.gz", + "https://bitbucket.org/eigen/eigen/get/f3a22f35b044.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_github_libexpat_libexpat", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:expat.BUILD", + sha256 = "b5dcb503e40f615a0296a18acc6d975f2f1a3d01c7b3a4b3bb69de27bc9475c3", + strip_prefix = "libexpat-R_2_2_4/expat", + urls = [ + "https://mirror.bazel.build/github.com/libexpat/libexpat/archive/R_2_2_4.tar.gz", + "https://github.com/libexpat/libexpat/archive/R_2_2_4.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "libjpeg", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:libjpeg.BUILD", + sha256 = "240fd398da741669bf3c90366f58452ea59041cacc741a489b99f2f6a0bad052", + strip_prefix = "jpeg-9b", + urls = [ + "https://mirror.bazel.build/www.ijg.org/files/jpegsrc.v9b.tar.gz", + "http://www.ijg.org/files/jpegsrc.v9b.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "org_libpng_libpng", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:libpng.BUILD", + sha256 = "7f415186d38ca71c23058386d7cf5135c8beda821ee1beecdc2a7a26c0356615", + strip_prefix = "libpng-1.2.57", + urls = [ + "https://mirror.bazel.build/github.com/glennrp/libpng/archive/v1.2.57.tar.gz", + "https://github.com/glennrp/libpng/archive/v1.2.57.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_google_googletest", + sha256 = "c18f281fd6621bb264570b99860a0241939b4a251c9b1af709b811d33bc63af8", + strip_prefix = "googletest-e3bd4cbeaeef3cee65a68a8bd3c535cb779e9b6d", + urls = [ + "https://mirror.bazel.build/github.com/google/googletest/archive/e3bd4cbeaeef3cee65a68a8bd3c535cb779e9b6d.tar.gz", + "https://github.com/google/googletest/archive/e3bd4cbeaeef3cee65a68a8bd3c535cb779e9b6d.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "bazel_skylib", + sha256 = "e5d90f0ec952883d56747b7604e2a15ee36e288bb556c3d0ed33e818a4d971f2", + strip_prefix = "bazel-skylib-1.0.2", + urls = ["https://github.com/bazelbuild/bazel-skylib/archive/1.0.2.tar.gz"], + ) + + _maybe( + http_archive, + name = "com_google_protobuf", + sha256 = "1c744a6a1f2c901e68c5521bc275e22bdc66256eeb605c2781923365b7087e5f", + strip_prefix = "protobuf-3.13.0", + urls = [ + "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.13.0.zip", + "https://github.com/google/protobuf/archive/v3.13.0.zip", + ], + repo_mapping = {"@zlib": "@net_zlib_zlib"}, + ) + + _maybe( + http_archive, + name = "org_lua_lua", + build_file = "@com_github_googlecartographer_cartographer//bazel/third_party:lua.BUILD", + sha256 = "b9e2e4aad6789b3b63a056d442f7b39f0ecfca3ae0f1fc0ae4e9614401b69f4b", + strip_prefix = "lua-5.2.4", + urls = [ + "https://mirror.bazel.build/www.lua.org/ftp/lua-5.2.4.tar.gz", + "https://www.lua.org/ftp/lua-5.2.4.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_github_grpc_grpc", + sha256 = "f869c648090e8bddaa1260a271b1089caccbe735bf47ac9cd7d44d35a02fb129", + strip_prefix = "grpc-1.19.1", + urls = [ + "https://mirror.bazel.build/github.com/grpc/grpc/archive/v1.19.1.tar.gz", + "https://github.com/grpc/grpc/archive/v1.19.1.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_github_jupp0r_prometheus_cpp", + sha256 = "07a704819cb90ed619cbf1a2713ba39faab27b8898b4561cc11a3c8b3ace83ea", + strip_prefix = "prometheus-cpp-4b11ee7a0aa7157494df06c4a324bf6d11bd0eec", + urls = [ + "https://github.com/jupp0r/prometheus-cpp/archive/4b11ee7a0aa7157494df06c4a324bf6d11bd0eec.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_github_googlecartographer_async_grpc", + sha256 = "83c2a27c92979787f38810adc4b6bb67aa09607c53dbadca3430a5f29e0a1cd3", + strip_prefix = "async_grpc-771af45374af7f7bfc3b622ed7efbe29a4aba403", + urls = [ + "https://github.com/cartographer-project/async_grpc/archive/771af45374af7f7bfc3b622ed7efbe29a4aba403.tar.gz", + ], + ) + + _maybe( + http_archive, + name = "com_google_absl", + sha256 = "c8ba586a9ab12bc4a67bb419fc0d2146200942b072bac95f50490f977b7fb04f", + strip_prefix = "abseil-cpp-5441bbe1db5d0f2ca24b5b60166367b0966790af", + urls = ["https://github.com/abseil/abseil-cpp/archive/5441bbe1db5d0f2ca24b5b60166367b0966790af.tar.gz"], + ) + + _maybe( + http_archive, + name = "rules_python", + sha256 = "e5470e92a18aa51830db99a4d9c492cc613761d5bdb7131c04bd92b9834380f6", + strip_prefix = "rules_python-4b84ad270387a7c439ebdccfd530e2339601ef27", + urls = [ + "https://mirror.bazel.build/github.com/bazelbuild/rules_python/archive/4b84ad270387a7c439ebdccfd530e2339601ef27.tar.gz", + "https://github.com/bazelbuild/rules_python/archive/4b84ad270387a7c439ebdccfd530e2339601ef27.tar.gz", + ], + ) + + # TODO(rodrigoq): remove these binds once grpc#14140 has been merged, as well + # as removing `use_external` in cartographer_grpc/BUILD.bazel. + # https://github.com/grpc/grpc/pull/14140 + native.bind( + name = "grpc_cpp_plugin", + actual = "@com_github_grpc_grpc//:grpc_cpp_plugin", + ) + native.bind( + name = "grpc++_codegen_proto", + actual = "@com_github_grpc_grpc//:grpc++_codegen_proto", + ) + +def _maybe(repo_rule, name, **kwargs): + if name not in native.existing_rules(): + repo_rule(name = name, **kwargs) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/BUILD.bazel b/carto_ws/src/cartographer-master/bazel/third_party/BUILD.bazel new file mode 100644 index 0000000..d375340 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/BUILD.bazel @@ -0,0 +1,22 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Bazel build support for third-party packages. + +licenses(["notice"]) # Apache 2.0 + +exports_files( + glob(["*.BUILD"]), + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/cairo/BUILD.bazel b/carto_ws/src/cartographer-master/bazel/third_party/cairo/BUILD.bazel new file mode 100644 index 0000000..b90ab6c --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/cairo/BUILD.bazel @@ -0,0 +1,26 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Files required for building Cairo with Bazel. + +licenses(["notice"]) + +exports_files( + [ + "cairo.BUILD", + "config.h", + "cairo-features.h", + ], + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/cairo/cairo-features.h b/carto_ws/src/cartographer-master/bazel/third_party/cairo/cairo-features.h new file mode 100644 index 0000000..ec161ea --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/cairo/cairo-features.h @@ -0,0 +1,50 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CAIRO_FEATURES_H +#define CAIRO_FEATURES_H + +#define CAIRO_HAS_FC_FONT 1 +#define CAIRO_HAS_FT_FONT 1 +#define CAIRO_HAS_GOBJECT_FUNCTIONS 1 +#define CAIRO_HAS_IMAGE_SURFACE 1 +#define CAIRO_HAS_MIME_SURFACE 1 +#define CAIRO_HAS_OBSERVER_SURFACE 1 +#define CAIRO_HAS_PDF_SURFACE 1 +#define CAIRO_HAS_PNG_FUNCTIONS 1 +#define CAIRO_HAS_PS_SURFACE 1 +#define CAIRO_HAS_RECORDING_SURFACE 1 +#define CAIRO_HAS_SCRIPT_SURFACE 1 +#define CAIRO_HAS_SVG_SURFACE 1 +#define CAIRO_HAS_TEE_SURFACE 1 +#define CAIRO_HAS_USER_FONT 1 + +#define CAIRO_HAS_GIF_FUNCTIONS 1 +#define CAIRO_HAS_JPEG_FUNCTIONS 1 + +/*#undef CAIRO_HAS_EGL_FUNCTIONS */ +/*#undef CAIRO_HAS_GLX_FUNCTIONS */ +/*#undef CAIRO_HAS_QUARTZ_FONT */ +/*#undef CAIRO_HAS_QUARTZ_SURFACE */ +/*#undef CAIRO_HAS_WGL_FUNCTIONS */ +/*#undef CAIRO_HAS_WIN32_FONT */ +/*#undef CAIRO_HAS_WIN32_SURFACE */ +/*#undef CAIRO_HAS_XCB_SHM_FUNCTIONS */ +/*#undef CAIRO_HAS_XCB_SURFACE */ +/*#undef CAIRO_HAS_XLIB_SURFACE */ +/*#undef CAIRO_HAS_XLIB_XRENDER_SURFACE */ + +#endif diff --git a/carto_ws/src/cartographer-master/bazel/third_party/cairo/cairo.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/cairo/cairo.BUILD new file mode 100644 index 0000000..78bcf5c --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/cairo/cairo.BUILD @@ -0,0 +1,304 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# Cairo is a 2D graphics library with support for multiple output devices. + +licenses(["reciprocal"]) # MPL 1.1 + +genrule( + name = "config_h", + srcs = ["@com_github_googlecartographer_cartographer//bazel/third_party/cairo:config.h"], + outs = ["cairo_internal/config.h"], + cmd = "cp $< $@", +) + +genrule( + name = "cairo_features_h", + srcs = ["@com_github_googlecartographer_cartographer//bazel/third_party/cairo:cairo-features.h"], + outs = ["src/cairo-features.h"], + cmd = "cp $< $@", +) + +cc_library( + name = "cairo", + srcs = [ + "cairo-version.h", + "cairo_internal/config.h", + "src/cairo.c", + "src/cairo.h", + "src/cairo-analysis-surface.c", + "src/cairo-analysis-surface-private.h", + "src/cairo-arc.c", + "src/cairo-arc-private.h", + "src/cairo-array.c", + "src/cairo-array-private.h", + "src/cairo-atomic.c", + "src/cairo-atomic-private.h", + "src/cairo-backend-private.h", + "src/cairo-base64-stream.c", + "src/cairo-base85-stream.c", + "src/cairo-bentley-ottmann.c", + "src/cairo-bentley-ottmann-rectangular.c", + "src/cairo-bentley-ottmann-rectilinear.c", + "src/cairo-botor-scan-converter.c", + "src/cairo-box-inline.h", + "src/cairo-boxes.c", + "src/cairo-boxes-intersect.c", + "src/cairo-boxes-private.h", + "src/cairo-cache.c", + "src/cairo-cache-private.h", + "src/cairo-cff-subset.c", + "src/cairo-clip.c", + "src/cairo-clip-boxes.c", + "src/cairo-clip-inline.h", + "src/cairo-clip-polygon.c", + "src/cairo-clip-private.h", + "src/cairo-clip-region.c", + "src/cairo-clip-surface.c", + "src/cairo-clip-tor-scan-converter.c", + "src/cairo-color.c", + "src/cairo-combsort-inline.h", + "src/cairo-compiler-private.h", + "src/cairo-composite-rectangles.c", + "src/cairo-composite-rectangles-private.h", + "src/cairo-compositor.c", + "src/cairo-compositor-private.h", + "src/cairo-contour.c", + "src/cairo-contour-inline.h", + "src/cairo-contour-private.h", + "src/cairo-damage.c", + "src/cairo-damage-private.h", + "src/cairo-debug.c", + "src/cairo-default-context.c", + "src/cairo-default-context-private.h", + "src/cairo-deflate-stream.c", + "src/cairo-deprecated.h", + "src/cairo-device.c", + "src/cairo-device-private.h", + "src/cairo-error.c", + "src/cairo-error-private.h", + "src/cairo-fallback-compositor.c", + "src/cairo-features.h", + "src/cairo-fixed.c", + "src/cairo-fixed-private.h", + "src/cairo-fixed-type-private.h", + "src/cairo-font-face.c", + "src/cairo-font-face-twin.c", + "src/cairo-font-face-twin-data.c", + "src/cairo-font-options.c", + "src/cairo-fontconfig-private.h", + "src/cairo-freed-pool.c", + "src/cairo-freed-pool-private.h", + "src/cairo-freelist.c", + "src/cairo-freelist-private.h", + "src/cairo-freelist-type-private.h", + "src/cairo-ft.h", + "src/cairo-ft-font.c", + "src/cairo-ft-private.h", + "src/cairo-gstate.c", + "src/cairo-gstate-private.h", + "src/cairo-hash.c", + "src/cairo-hash-private.h", + "src/cairo-hull.c", + "src/cairo-image-compositor.c", + "src/cairo-image-info.c", + "src/cairo-image-info-private.h", + "src/cairo-image-source.c", + "src/cairo-image-surface.c", + "src/cairo-image-surface-inline.h", + "src/cairo-image-surface-private.h", + "src/cairo-line.c", + "src/cairo-line-inline.h", + "src/cairo-line-private.h", + "src/cairo-list-inline.h", + "src/cairo-list-private.h", + "src/cairo-lzw.c", + "src/cairo-malloc-private.h", + "src/cairo-mask-compositor.c", + "src/cairo-matrix.c", + "src/cairo-mempool.c", + "src/cairo-mempool-private.h", + "src/cairo-mesh-pattern-rasterizer.c", + "src/cairo-misc.c", + "src/cairo-mono-scan-converter.c", + "src/cairo-mutex.c", + "src/cairo-mutex-impl-private.h", + "src/cairo-mutex-list-private.h", + "src/cairo-mutex-private.h", + "src/cairo-mutex-type-private.h", + "src/cairo-no-compositor.c", + "src/cairo-observer.c", + "src/cairo-output-stream.c", + "src/cairo-output-stream-private.h", + "src/cairo-paginated-private.h", + "src/cairo-paginated-surface.c", + "src/cairo-paginated-surface-private.h", + "src/cairo-path.c", + "src/cairo-path-bounds.c", + "src/cairo-path-fill.c", + "src/cairo-path-fixed.c", + "src/cairo-path-fixed-private.h", + "src/cairo-path-in-fill.c", + "src/cairo-path-private.h", + "src/cairo-path-stroke.c", + "src/cairo-path-stroke-boxes.c", + "src/cairo-path-stroke-polygon.c", + "src/cairo-path-stroke-traps.c", + "src/cairo-path-stroke-tristrip.c", + "src/cairo-pattern.c", + "src/cairo-pattern-inline.h", + "src/cairo-pattern-private.h", + "src/cairo-pdf.h", + "src/cairo-pdf-operators.c", + "src/cairo-pdf-operators-private.h", + "src/cairo-pdf-shading.c", + "src/cairo-pdf-shading-private.h", + "src/cairo-pdf-surface.c", + "src/cairo-pdf-surface-private.h", + "src/cairo-pen.c", + "src/cairo-pixman-private.h", + "src/cairo-png.c", + "src/cairo-polygon.c", + "src/cairo-polygon-intersect.c", + "src/cairo-polygon-reduce.c", + "src/cairo-private.h", + "src/cairo-ps.h", + "src/cairo-ps-surface.c", + "src/cairo-ps-surface-private.h", + "src/cairo-raster-source-pattern.c", + "src/cairo-recording-surface.c", + "src/cairo-recording-surface-inline.h", + "src/cairo-recording-surface-private.h", + "src/cairo-rectangle.c", + "src/cairo-rectangular-scan-converter.c", + "src/cairo-reference-count-private.h", + "src/cairo-region.c", + "src/cairo-region-private.h", + "src/cairo-rtree.c", + "src/cairo-rtree-private.h", + "src/cairo-scaled-font.c", + "src/cairo-scaled-font-private.h", + "src/cairo-scaled-font-subsets.c", + "src/cairo-scaled-font-subsets-private.h", + "src/cairo-script.h", + "src/cairo-script-private.h", + "src/cairo-script-surface.c", + "src/cairo-shape-mask-compositor.c", + "src/cairo-slope.c", + "src/cairo-slope-private.h", + "src/cairo-spans.c", + "src/cairo-spans-compositor.c", + "src/cairo-spans-compositor-private.h", + "src/cairo-spans-private.h", + "src/cairo-spline.c", + "src/cairo-stroke-dash.c", + "src/cairo-stroke-dash-private.h", + "src/cairo-stroke-style.c", + "src/cairo-surface.c", + "src/cairo-surface-backend-private.h", + "src/cairo-surface-clipper.c", + "src/cairo-surface-clipper-private.h", + "src/cairo-surface-fallback.c", + "src/cairo-surface-fallback-private.h", + "src/cairo-surface-inline.h", + "src/cairo-surface-observer.c", + "src/cairo-surface-observer-inline.h", + "src/cairo-surface-observer-private.h", + "src/cairo-surface-offset.c", + "src/cairo-surface-offset-private.h", + "src/cairo-surface-private.h", + "src/cairo-surface-snapshot.c", + "src/cairo-surface-snapshot-inline.h", + "src/cairo-surface-snapshot-private.h", + "src/cairo-surface-subsurface.c", + "src/cairo-surface-subsurface-inline.h", + "src/cairo-surface-subsurface-private.h", + "src/cairo-surface-wrapper.c", + "src/cairo-surface-wrapper-private.h", + "src/cairo-svg.h", + "src/cairo-svg-surface.c", + "src/cairo-svg-surface-private.h", + "src/cairo-tee.h", + "src/cairo-tee-surface.c", + "src/cairo-tee-surface-private.h", + "src/cairo-time.c", + "src/cairo-time-private.h", + "src/cairo-tor-scan-converter.c", + "src/cairo-tor22-scan-converter.c", + "src/cairo-toy-font-face.c", + "src/cairo-traps.c", + "src/cairo-traps-compositor.c", + "src/cairo-traps-private.h", + "src/cairo-tristrip.c", + "src/cairo-tristrip-private.h", + "src/cairo-truetype-subset.c", + "src/cairo-truetype-subset-private.h", + "src/cairo-type1-fallback.c", + "src/cairo-type1-glyph-names.c", + "src/cairo-type1-private.h", + "src/cairo-type1-subset.c", + "src/cairo-type3-glyph-surface.c", + "src/cairo-type3-glyph-surface-private.h", + "src/cairo-types-private.h", + "src/cairo-unicode.c", + "src/cairo-user-font.c", + "src/cairo-user-font-private.h", + "src/cairo-version.c", + "src/cairo-version.h", + "src/cairo-wideint.c", + "src/cairo-wideint-private.h", + "src/cairo-wideint-type-private.h", + "src/cairoint.h", + ], + hdrs = [ + "src/cairo.h", + "src/cairo-deprecated.h", + "src/cairo-features.h", + "src/cairo-version.h", + ], + copts = [ + "-DHAVE_CONFIG_H", + "-D_REENTRANT", + "-Wno-attributes", + "-Wno-cpp", + "-Wno-int-in-bool-context", + "-Wno-maybe-uninitialized", + "-Wno-misleading-indentation", + "-Wno-strict-aliasing", + "-Wno-unused-but-set-variable", + "-Wno-unused-function", + "-I$(GENDIR)/external/org_cairographics_cairo/src", + "-I$(GENDIR)/external/org_cairographics_cairo/cairo_internal", + "-DCAIRO_HAS_XLIB_SURFACE=1", # Only effect is to create xlib display mutex + ], + include_prefix = "cairo", + linkopts = [ + "-lpthread", + "-lrt", + "-lm", + ], + strip_include_prefix = "src", + visibility = ["//visibility:public"], + deps = [ + "@com_github_libexpat_libexpat//:expat", + "@libjpeg//:jpeg", + "@net_zlib_zlib//:zlib", + "@org_cairographics_pixman//:pixman", + "@org_freedesktop_fontconfig//:fontconfig", + "@org_freetype_freetype2//:freetype2", + "@org_libgd_libgd//:gd", + "@org_libpng_libpng//:libpng", + ], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/cairo/config.h b/carto_ws/src/cartographer-master/bazel/third_party/cairo/config.h new file mode 100644 index 0000000..2a7f986 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/cairo/config.h @@ -0,0 +1,428 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* Define if building universal (internal helper macro) */ +/* #undef AC_APPLE_UNIVERSAL_BUILD */ + +/* whether memory barriers are needed around atomic operations */ +/* #undef ATOMIC_OP_NEEDS_MEMORY_BARRIER */ + +/* Define to 1 if the PDF backend can be tested (need poppler and other + dependencies for pdf2png) */ +/* #undef CAIRO_CAN_TEST_PDF_SURFACE */ + +/* Define to 1 if the PS backend can be tested (needs ghostscript) */ +#define CAIRO_CAN_TEST_PS_SURFACE 1 + +/* Define to 1 if the SVG backend can be tested */ +/* #undef CAIRO_CAN_TEST_SVG_SURFACE */ + +/* Define to 1 if the Win32 Printing backend can be tested (needs ghostscript) + */ +/* #undef CAIRO_CAN_TEST_WIN32_PRINTING_SURFACE */ + +/* Define to 1 if dlsym is available */ +#define CAIRO_HAS_DLSYM 1 + +/* Define to 1 to enable cairo's cairo-script-interpreter feature */ +/* #undef CAIRO_HAS_INTERPRETER */ + +/* Define to 1 to enable cairo's pthread feature */ +#define CAIRO_HAS_PTHREAD 1 + +/* Define to 1 if we have full pthread support */ +#define CAIRO_HAS_REAL_PTHREAD 1 + +/* Define to 1 if libspectre is available */ +/* #undef CAIRO_HAS_SPECTRE */ + +/* Define to 1 to enable cairo's symbol-lookup feature */ +/* #undef CAIRO_HAS_SYMBOL_LOOKUP */ + +/* Define to 1 to enable cairo's test surfaces feature */ +/* #undef CAIRO_HAS_TEST_SURFACES */ + +/* Define to 1 to enable cairo's cairo-trace feature */ +/* #undef CAIRO_HAS_TRACE */ + +/* Define to 1 to disable certain code paths that rely heavily on double + precision floating-point calculation */ +/* #undef DISABLE_SOME_FLOATING_POINT */ + +/* Define to 1 if your system stores words within floats with the most + significant word first */ +/* #undef FLOAT_WORDS_BIGENDIAN */ + +/* Enable pixman glyph cache */ +#define HAS_PIXMAN_GLYPHS 1 + +/* Define to 1 if you have the `alarm' function. */ +#define HAVE_ALARM 1 + +/* Define to 1 if you have the binutils development files installed */ +/* #undef HAVE_BFD */ + +/* Define to 1 if your compiler supports the __builtin_return_address() + intrinsic. */ +#define HAVE_BUILTIN_RETURN_ADDRESS 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_BYTESWAP_H 1 + +/* Define to 1 if you have the `clock_gettime' function. */ +#define HAVE_CLOCK_GETTIME 1 + +/* Define to 1 if you have the `ctime_r' function. */ +#define HAVE_CTIME_R 1 + +/* Enable if your compiler supports the GCC __atomic_* atomic primitives */ +#define HAVE_CXX11_ATOMIC_PRIMITIVES 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_DLFCN_H 1 + +/* Define to 1 if you have the `drand48' function. */ +#define HAVE_DRAND48 1 + +/* Define to 1 if you have the `FcFini' function. */ +/* #undef HAVE_FCFINI */ + +/* Define to 1 if you have the `FcInit' function. */ +/* #undef HAVE_FCINIT */ + +/* Define to 1 if you have the header file. */ +#define HAVE_FCNTL_H 1 + +/* Define to 1 if you have the `feclearexcept' function. */ +#define HAVE_FECLEAREXCEPT 1 + +/* Define to 1 if you have the `fedisableexcept' function. */ +#define HAVE_FEDISABLEEXCEPT 1 + +/* Define to 1 if you have the `feenableexcept' function. */ +#define HAVE_FEENABLEEXCEPT 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_FENV_H 1 + +/* Define to 1 if you have the `flockfile' function. */ +#define HAVE_FLOCKFILE 1 + +/* Define to 1 if you have the `fork' function. */ +#define HAVE_FORK 1 + +/* Define to 1 if you have the `FT_Get_X11_Font_Format' function. */ +#define HAVE_FT_GET_X11_FONT_FORMAT 1 + +/* Define to 1 if you have the `FT_GlyphSlot_Embolden' function. */ +#define HAVE_FT_GLYPHSLOT_EMBOLDEN 1 + +/* Define to 1 if you have the `FT_GlyphSlot_Oblique' function. */ +#define HAVE_FT_GLYPHSLOT_OBLIQUE 1 + +/* Define to 1 if you have the `FT_Library_SetLcdFilter' function. */ +#define HAVE_FT_LIBRARY_SETLCDFILTER 1 + +/* Define to 1 if you have the `FT_Load_Sfnt_Table' function. */ +#define HAVE_FT_LOAD_SFNT_TABLE 1 + +/* Define to 1 if you have the `funlockfile' function. */ +#define HAVE_FUNLOCKFILE 1 + +/* Whether you have gcov */ +/* #undef HAVE_GCOV */ + +/* Define to 1 if you have the `getline' function. */ +#define HAVE_GETLINE 1 + +/* Enable if your compiler supports the Intel __sync_* atomic primitives */ +/* #undef HAVE_INTEL_ATOMIC_PRIMITIVES */ + +/* Define to 1 if you have the header file. */ +#define HAVE_INTTYPES_H 1 + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_IO_H */ + +/* Define to 1 if you have the header file. */ +#define HAVE_LIBGEN_H 1 + +/* Define to 1 if you have the `rt' library (-lrt). */ +#define HAVE_LIBRT 1 + +/* Enable if you have libatomic-ops-dev installed */ +/* #undef HAVE_LIB_ATOMIC_OPS */ + +/* Define to 1 if you have the `link' function. */ +#define HAVE_LINK 1 + +/* Define to 1 if you have the Valgrind lockdep tool */ +/* #undef HAVE_LOCKDEP */ + +/* Define to 1 if you have lzo available */ +/* #undef HAVE_LZO */ + +/* Define to 1 if you have the Valgrind memfault tool */ +/* #undef HAVE_MEMFAULT */ + +/* Define to 1 if you have the header file. */ +#define HAVE_MEMORY_H 1 + +/* Define to non-zero if your system has mkdir, and to 2 if your version of + mkdir requires a mode parameter */ +#define HAVE_MKDIR 2 + +/* Define to 1 if you have the `mmap' function. */ +#define HAVE_MMAP 1 + +/* Enable if you have MacOS X atomic operations */ +/* #undef HAVE_OS_ATOMIC_OPS */ + +/* Define to 1 if you have the `poppler_page_render' function. */ +/* #undef HAVE_POPPLER_PAGE_RENDER */ + +/* Define to 1 if you have the `raise' function. */ +#define HAVE_RAISE 1 + +/* Define to 1 if you have the `rsvg_pixbuf_from_file' function. */ +/* #undef HAVE_RSVG_PIXBUF_FROM_FILE */ + +/* Define to 1 if you have the `sched_getaffinity' function. */ +#define HAVE_SCHED_GETAFFINITY 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SCHED_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SETJMP_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SIGNAL_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDINT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDLIB_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRINGS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRING_H 1 + +/* Define to 1 if you have the `strndup' function. */ +#define HAVE_STRNDUP 1 + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_SYS_INT_TYPES_H */ + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_IOCTL_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_MMAN_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_POLL_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_SOCKET_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STAT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_TYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_UN_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_WAIT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_TIME_H 1 + +/* Define to 1 if the system has the type `uint128_t'. */ +/* #undef HAVE_UINT128_T */ + +/* Define to 1 if the system has the type `uint64_t'. */ +#define HAVE_UINT64_T 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_UNISTD_H 1 + +/* Define to 1 if you have Valgrind */ +/* #undef HAVE_VALGRIND */ + +/* Define to 1 if you have the `waitpid' function. */ +#define HAVE_WAITPID 1 + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_WINDOWS_H */ + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_X11_EXTENSIONS_SHMPROTO_H */ + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_X11_EXTENSIONS_SHMSTR_H */ + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_X11_EXTENSIONS_XSHM_H */ + +/* Define to 1 if you have the `XRenderCreateConicalGradient' function. */ +/* #undef HAVE_XRENDERCREATECONICALGRADIENT */ + +/* Define to 1 if you have the `XRenderCreateLinearGradient' function. */ +/* #undef HAVE_XRENDERCREATELINEARGRADIENT */ + +/* Define to 1 if you have the `XRenderCreateRadialGradient' function. */ +/* #undef HAVE_XRENDERCREATERADIALGRADIENT */ + +/* Define to 1 if you have zlib available */ +#define HAVE_ZLIB 1 + +/* Define to 1 if the system has the type `__uint128_t'. */ +#define HAVE___UINT128_T 1 + +/* Define to 1 if shared memory segments are released deferred. */ +/* #undef IPC_RMID_DEFERRED_RELEASE */ + +/* Define to the sub-directory in which libtool stores uninstalled libraries. + */ +#define LT_OBJDIR ".libs/" + +/* Define to 1 if your C compiler doesn't accept -c and -o together. */ +/* #undef NO_MINUS_C_MINUS_O */ + +/* Define to the address where bug reports for this package should be sent. */ +#define PACKAGE_BUGREPORT \ + "http://bugs.freedesktop.org/enter_bug.cgi?product=cairo" + +/* Define to the full name of this package. */ +#define PACKAGE_NAME USE_cairo_INSTEAD + +/* Define to the full name and version of this package. */ +#define PACKAGE_STRING USE_cairo_version_OR_cairo_version_string_INSTEAD + +/* Define to the one symbol short name of this package. */ +#define PACKAGE_TARNAME USE_cairo_INSTEAD + +/* Define to the home page for this package. */ +#define PACKAGE_URL "http://cairographics.org/" + +/* Define to the version of this package. */ +#define PACKAGE_VERSION USE_cairo_version_OR_cairo_version_string_INSTEAD + +/* Shared library file extension */ +#define SHARED_LIB_EXT "so" + +/* The size of `int', as computed by sizeof. */ +#define SIZEOF_INT 4 + +/* The size of `long', as computed by sizeof. */ +#define SIZEOF_LONG 8 + +/* The size of `long long', as computed by sizeof. */ +#define SIZEOF_LONG_LONG 8 + +/* The size of `size_t', as computed by sizeof. */ +#define SIZEOF_SIZE_T 8 + +/* The size of `void *', as computed by sizeof. */ +#define SIZEOF_VOID_P 8 + +/* Define to 1 if you have the ANSI C header files. */ +#define STDC_HEADERS 1 + +/* Enable extensions on AIX 3, Interix. */ +#ifndef _ALL_SOURCE +#define _ALL_SOURCE 1 +#endif +/* Enable GNU extensions on systems that have them. */ +#ifndef _GNU_SOURCE +#define _GNU_SOURCE 1 +#endif +/* Enable threading extensions on Solaris. */ +#ifndef _POSIX_PTHREAD_SEMANTICS +#define _POSIX_PTHREAD_SEMANTICS 1 +#endif +/* Enable extensions on HP NonStop. */ +#ifndef _TANDEM_SOURCE +#define _TANDEM_SOURCE 1 +#endif +/* Enable general extensions on Solaris. */ +#ifndef __EXTENSIONS__ +#define __EXTENSIONS__ 1 +#endif + +/* Define to the value your compiler uses to support the warn-unused-result + attribute */ +#define WARN_UNUSED_RESULT + +/* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most + significant byte first (like Motorola and SPARC, unlike Intel). */ +#if defined AC_APPLE_UNIVERSAL_BUILD +#if defined __BIG_ENDIAN__ +#define WORDS_BIGENDIAN 1 +#endif +#else +#ifndef WORDS_BIGENDIAN +/* # undef WORDS_BIGENDIAN */ +#endif +#endif + +/* Deal with multiple architecture compiles on Mac OS X */ +#ifdef __APPLE_CC__ +#ifdef __BIG_ENDIAN__ +#define WORDS_BIGENDIAN 1 +#define FLOAT_WORDS_BIGENDIAN 1 +#else +/* #undef WORDS_BIGENDIAN */ +/* #undef FLOAT_WORDS_BIGENDIAN */ +#endif +#endif + +/* Define to 1 if the X Window System is missing or not being used. */ +/* #undef X_DISPLAY_MISSING */ + +/* Enable large inode numbers on Mac OS X 10.5. */ +#ifndef _DARWIN_USE_64_BIT_INODE +#define _DARWIN_USE_64_BIT_INODE 1 +#endif + +/* Number of bits in a file offset, on hosts where this is settable. */ +/* #undef _FILE_OFFSET_BITS */ + +/* Define for large files, on AIX-style hosts. */ +/* #undef _LARGE_FILES */ + +/* Define to 1 if on MINIX. */ +/* #undef _MINIX */ + +/* Define to 2 if the system does not provide POSIX.1 features except with + this defined. */ +/* #undef _POSIX_1_SOURCE */ + +/* Define to 1 if you need to in order for `stat' and other things to work. */ +/* #undef _POSIX_SOURCE */ + +/* Define to `__inline__' or `__inline' if that's what the C compiler + calls it, or to nothing if 'inline' is not supported under any name. */ +#ifndef __cplusplus +/* #undef inline */ +#endif diff --git a/carto_ws/src/cartographer-master/bazel/third_party/ceres.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/ceres.BUILD new file mode 100644 index 0000000..b8b75d8 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/ceres.BUILD @@ -0,0 +1,164 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# Ceres Solver is an open source C++ library for modeling and solving large, +# complicated optimization problems. + +licenses(["notice"]) # New BSD, portions MIT. + +CERES_DEFINES = [ + "CERES_USE_CXX11", + "CERES_USE_EIGEN_SPARSE", + "CERES_NO_SUITESPARSE", + "CERES_NO_LAPACK", + "CERES_NO_CXSPARSE", + "CERES_STD_UNORDERED_MAP", + "CERES_USE_CXX11_THREADS", + + # Use the internal mutex code. Not ideal, but it works. + "CERES_HAVE_PTHREAD", + "CERES_HAVE_RWLOCK", +] + +cc_library( + name = "ceres", + srcs = [ + "internal/ceres/array_utils.cc", + "internal/ceres/blas.cc", + "internal/ceres/block_evaluate_preparer.cc", + "internal/ceres/block_jacobi_preconditioner.cc", + "internal/ceres/block_jacobian_writer.cc", + "internal/ceres/block_random_access_dense_matrix.cc", + "internal/ceres/block_random_access_diagonal_matrix.cc", + "internal/ceres/block_random_access_matrix.cc", + "internal/ceres/block_random_access_sparse_matrix.cc", + "internal/ceres/block_sparse_matrix.cc", + "internal/ceres/block_structure.cc", + "internal/ceres/callbacks.cc", + "internal/ceres/c_api.cc", + "internal/ceres/canonical_views_clustering.cc", + "internal/ceres/cgnr_solver.cc", + "internal/ceres/coordinate_descent_minimizer.cc", + "internal/ceres/compressed_col_sparse_matrix_utils.cc", + "internal/ceres/compressed_row_jacobian_writer.cc", + "internal/ceres/compressed_row_sparse_matrix.cc", + "internal/ceres/conditioned_cost_function.cc", + "internal/ceres/conjugate_gradients_solver.cc", + "internal/ceres/context_impl.cc", + "internal/ceres/corrector.cc", + "internal/ceres/covariance.cc", + "internal/ceres/covariance_impl.cc", + "internal/ceres/dense_normal_cholesky_solver.cc", + "internal/ceres/dense_qr_solver.cc", + "internal/ceres/dense_sparse_matrix.cc", + "internal/ceres/detect_structure.cc", + "internal/ceres/dogleg_strategy.cc", + "internal/ceres/dynamic_compressed_row_jacobian_writer.cc", + "internal/ceres/dynamic_compressed_row_sparse_matrix.cc", + "internal/ceres/dynamic_sparse_normal_cholesky_solver.cc", + "internal/ceres/eigensparse.cc", + "internal/ceres/evaluator.cc", + "internal/ceres/file.cc", + "internal/ceres/function_sample.cc", + "internal/ceres/gradient_checker.cc", + "internal/ceres/gradient_checking_cost_function.cc", + "internal/ceres/gradient_problem.cc", + "internal/ceres/gradient_problem_solver.cc", + "internal/ceres/implicit_schur_complement.cc", + "internal/ceres/inner_product_computer.cc", + "internal/ceres/is_close.cc", + "internal/ceres/iterative_refiner.cc", + "internal/ceres/iterative_schur_complement_solver.cc", + "internal/ceres/lapack.cc", + "internal/ceres/levenberg_marquardt_strategy.cc", + "internal/ceres/line_search.cc", + "internal/ceres/line_search_direction.cc", + "internal/ceres/line_search_minimizer.cc", + "internal/ceres/line_search_preprocessor.cc", + "internal/ceres/linear_least_squares_problems.cc", + "internal/ceres/linear_operator.cc", + "internal/ceres/linear_solver.cc", + "internal/ceres/local_parameterization.cc", + "internal/ceres/loss_function.cc", + "internal/ceres/low_rank_inverse_hessian.cc", + "internal/ceres/minimizer.cc", + "internal/ceres/normal_prior.cc", + "internal/ceres/parallel_for_cxx.cc", + "internal/ceres/parameter_block_ordering.cc", + "internal/ceres/partitioned_matrix_view.cc", + "internal/ceres/polynomial.cc", + "internal/ceres/preconditioner.cc", + "internal/ceres/preprocessor.cc", + "internal/ceres/problem.cc", + "internal/ceres/problem_impl.cc", + "internal/ceres/program.cc", + "internal/ceres/reorder_program.cc", + "internal/ceres/residual_block.cc", + "internal/ceres/residual_block_utils.cc", + "internal/ceres/schur_complement_solver.cc", + "internal/ceres/schur_eliminator.cc", + "internal/ceres/schur_jacobi_preconditioner.cc", + "internal/ceres/schur_templates.cc", + "internal/ceres/scratch_evaluate_preparer.cc", + "internal/ceres/single_linkage_clustering.cc", + "internal/ceres/solver.cc", + "internal/ceres/solver_utils.cc", + "internal/ceres/sparse_cholesky.cc", + "internal/ceres/sparse_matrix.cc", + "internal/ceres/sparse_normal_cholesky_solver.cc", + "internal/ceres/split.cc", + "internal/ceres/stringprintf.cc", + "internal/ceres/suitesparse.cc", + "internal/ceres/thread_pool.cc", + "internal/ceres/thread_token_provider.cc", + "internal/ceres/trust_region_minimizer.cc", + "internal/ceres/trust_region_preprocessor.cc", + "internal/ceres/trust_region_step_evaluator.cc", + "internal/ceres/trust_region_strategy.cc", + "internal/ceres/triplet_sparse_matrix.cc", + "internal/ceres/types.cc", + "internal/ceres/visibility_based_preconditioner.cc", + "internal/ceres/visibility.cc", + "internal/ceres/wall_time.cc", + ] + glob([ + "internal/ceres/generated/schur_eliminator_*.cc", + "internal/ceres/generated/partitioned_matrix_view_*.cc", + "config/**/*.h", + "internal/**/*.h", + ]), + hdrs = glob([ + "include/ceres/*.h", + "include/ceres/internal/*.h", + ]), + copts = [ + "-fopenmp", + "-Wno-sign-compare", + ], + defines = CERES_DEFINES, + includes = [ + "config", + "include", + "internal", + ], + linkopts = [ + "-lgomp", + ], + linkstatic = 1, + visibility = ["//visibility:public"], + deps = [ + "@com_google_glog//:glog", + "@org_tuxfamily_eigen//:eigen", + ], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/eigen.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/eigen.BUILD new file mode 100644 index 0000000..e4e1ca3 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/eigen.BUILD @@ -0,0 +1,56 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# Eigen is a C++ template library for linear algebra: vectors, +# matrices, and related algorithms. + +licenses([ + # Note: Eigen is an MPL2 library that includes GPL v3 and LGPL v2.1+ code. + # We've taken special care to not reference any restricted code. + "reciprocal", # MPL2 + "notice", # Portions BSD +]) + +exports_files(["COPYING.MPL2"]) + +EIGEN_FILES = [ + "Eigen/**", + "unsupported/Eigen/CXX11/**", + "unsupported/Eigen/FFT", + "unsupported/Eigen/KroneckerProduct", + "unsupported/Eigen/src/FFT/**", + "unsupported/Eigen/src/KroneckerProduct/**", + "unsupported/Eigen/MatrixFunctions", + "unsupported/Eigen/SpecialFunctions", + "unsupported/Eigen/src/MatrixFunctions/**", + "unsupported/Eigen/src/SpecialFunctions/**", +] + +# List of files picked up by glob but actually part of another target. +EIGEN_EXCLUDE_FILES = [ + "Eigen/src/Core/arch/AVX/PacketMathGoogleTest.cc", +] + +EIGEN_MPL2_HEADER_FILES = glob( + EIGEN_FILES, + exclude = EIGEN_EXCLUDE_FILES, +) + +cc_library( + name = "eigen", + hdrs = EIGEN_MPL2_HEADER_FILES, + includes = ["."], + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/expat.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/expat.BUILD new file mode 100644 index 0000000..85d21bd --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/expat.BUILD @@ -0,0 +1,93 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# Expat is an XML parser library written in C. + +licenses(["notice"]) # MIT-style + +load("@com_github_antonovvk_bazel_rules//:config.bzl", "cc_fix_config") + +cc_fix_config( + name = "expat_config_h", + cmake = True, + files = {"expat_config.h.cmake": "expat_config.h"}, + values = { + "BYTEORDER": "1234", + "HAVE_BCOPY": "1", + "HAVE_DLFCN": "1", + "HAVE_FCNTL_H": "1", + "HAVE_GETPAGESIZE": "1", + "HAVE_INTTYPES_H": "1", + "HAVE_MEMMOVE": "1", + "HAVE_MEMORY_H": "1", + "HAVE_STDINT_H": "1", + "HAVE_STDLIB_H": "1", + "HAVE_STRINGS_H": "1", + "HAVE_STRING_H": "1", + "HAVE_SYS_PARAM_H": "1", + "HAVE_SYS_STAT_H": "1", + "HAVE_SYS_TYPES_H": "1", + "HAVE_UNISTD_H": "1", + "LT_OBJDIR": "\".libs/\"", + "PACKAGE_BUGREPORT": "expat-bugs@libexpat.org", + "PACKAGE_NAME": "expat", + "PACKAGE_STRING": "expat 2.2.4", + "PACKAGE_TARNAME": "expat", + "PACKAGE_URL": "", + "PACKAGE_VERSION": "2.2.4", + "STDC_HEADERS": "1", + "XML_CONTEXT_BYTES": "1024", + "XML_DTD": "1", + "XML_NS": "1", + }, +) + +# TODO(rodrigoq): review if we're exposing more headers than users need. +cc_library( + name = "expat", + srcs = [ + "lib/xmlparse.c", + "lib/xmlrole.c", + "lib/xmltok.c", + ], + hdrs = [ + "expat_config.h", + "lib/ascii.h", + "lib/asciitab.h", + "lib/expat.h", + "lib/expat_external.h", + "lib/iasciitab.h", + "lib/internal.h", + "lib/latin1tab.h", + "lib/nametab.h", + "lib/siphash.h", + "lib/utf8tab.h", + "lib/xmlrole.h", + "lib/xmltok.h", + "lib/xmltok_impl.c", + "lib/xmltok_impl.h", + "lib/xmltok_ns.c", + ], + copts = [ + "-DHAVE_EXPAT_CONFIG_H", + "-DXML_DEV_URANDOM", + ], + defines = ["XML_STATIC"], + includes = [ + ".", + "lib", + ], + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/BUILD.bazel b/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/BUILD.bazel new file mode 100644 index 0000000..bb72c8d --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/BUILD.bazel @@ -0,0 +1,25 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Files required for building Fontconfig with Bazel. + +licenses(["notice"]) + +exports_files( + [ + "fontconfig.BUILD", + "config.h", + ], + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/config.h b/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/config.h new file mode 100644 index 0000000..3686792 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/config.h @@ -0,0 +1,372 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* Define if building universal (internal helper macro) */ +/* #undef AC_APPLE_UNIVERSAL_BUILD */ + +/* The normal alignment of `double', in bytes. */ +#define ALIGNOF_DOUBLE 8 + +/* The normal alignment of `void *', in bytes. */ +#define ALIGNOF_VOID_P 8 + +/* Use libxml2 instead of Expat */ +/* #undef ENABLE_LIBXML2 */ + +/* Additional font directories */ +#define FC_ADD_FONTS "yes" + +/* Architecture prefix to use for cache file names */ +#define FC_ARCHITECTURE "elf64-x86-64" + +/* System font directory */ +#define FC_DEFAULT_FONTS "/usr/share/fonts" + +/* The type of len parameter of the gperf hash/lookup function */ +#define FC_GPERF_SIZE_T unsigned int + +/* Define to nothing if C supports flexible array members, and to 1 if it does + not. That way, with a declaration like `struct s { int n; double + d[FLEXIBLE_ARRAY_MEMBER]; };', the struct hack can be used with pre-C99 + compilers. When computing the size of such an object, don't use 'sizeof + (struct s)' as it overestimates the size. Use 'offsetof (struct s, d)' + instead. Don't use 'offsetof (struct s, d[0])', as this doesn't work with + MSVC and with C++ compilers. */ +#define FLEXIBLE_ARRAY_MEMBER /**/ + +/* Define to 1 if you have the header file, and it defines `DIR'. + */ +#define HAVE_DIRENT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_DLFCN_H 1 + +/* Define to 1 if you don't have `vprintf' but do have `_doprnt.' */ +/* #undef HAVE_DOPRNT */ + +/* Define to 1 if you have the header file. */ +#define HAVE_FCNTL_H 1 + +/* Define to 1 if you have the `fstatfs' function. */ +#define HAVE_FSTATFS 1 + +/* Define to 1 if you have the `fstatvfs' function. */ +#define HAVE_FSTATVFS 1 + +/* FT_Bitmap_Size structure includes y_ppem field */ +#define HAVE_FT_BITMAP_SIZE_Y_PPEM 1 + +/* Define to 1 if you have the `FT_Get_BDF_Property' function. */ +#define HAVE_FT_GET_BDF_PROPERTY 1 + +/* Define to 1 if you have the `FT_Get_Next_Char' function. */ +#define HAVE_FT_GET_NEXT_CHAR 1 + +/* Define to 1 if you have the `FT_Get_PS_Font_Info' function. */ +#define HAVE_FT_GET_PS_FONT_INFO 1 + +/* Define to 1 if you have the `FT_Get_X11_Font_Format' function. */ +#define HAVE_FT_GET_X11_FONT_FORMAT 1 + +/* Define to 1 if you have the `FT_Has_PS_Glyph_Names' function. */ +#define HAVE_FT_HAS_PS_GLYPH_NAMES 1 + +/* Define to 1 if you have the `FT_Select_Size' function. */ +#define HAVE_FT_SELECT_SIZE 1 + +/* Define to 1 if you have the `getexecname' function. */ +/* #undef HAVE_GETEXECNAME */ + +/* Define to 1 if you have the `getopt' function. */ +#define HAVE_GETOPT 1 + +/* Define to 1 if you have the `getopt_long' function. */ +#define HAVE_GETOPT_LONG 1 + +/* Define to 1 if you have the `getpagesize' function. */ +#define HAVE_GETPAGESIZE 1 + +/* Define to 1 if you have the `getprogname' function. */ +/* #undef HAVE_GETPROGNAME */ + +/* Have Intel __sync_* atomic primitives */ +#define HAVE_INTEL_ATOMIC_PRIMITIVES 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_INTTYPES_H 1 + +/* Define to 1 if you have the `link' function. */ +#define HAVE_LINK 1 + +/* Define to 1 if you have the `lrand48' function. */ +#define HAVE_LRAND48 1 + +/* Define to 1 if you have the `lstat' function. */ +#define HAVE_LSTAT 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_MEMORY_H 1 + +/* Define to 1 if you have the `mkdtemp' function. */ +#define HAVE_MKDTEMP 1 + +/* Define to 1 if you have the `mkostemp' function. */ +#define HAVE_MKOSTEMP 1 + +/* Define to 1 if you have the `mkstemp' function. */ +#define HAVE_MKSTEMP 1 + +/* Define to 1 if you have a working `mmap' system call. */ +#define HAVE_MMAP 1 + +/* Define to 1 if you have the header file, and it defines `DIR'. */ +/* #undef HAVE_NDIR_H */ + +/* Define to 1 if you have the 'posix_fadvise' function. */ +#define HAVE_POSIX_FADVISE 1 + +/* Have POSIX threads */ +#define HAVE_PTHREAD 1 + +/* Have PTHREAD_PRIO_INHERIT. */ +#define HAVE_PTHREAD_PRIO_INHERIT 1 + +/* Define to 1 if you have the `rand' function. */ +#define HAVE_RAND 1 + +/* Define to 1 if you have the `random' function. */ +#define HAVE_RANDOM 1 + +/* Define to 1 if you have the `random_r' function. */ +#define HAVE_RANDOM_R 1 + +/* Define to 1 if you have the `rand_r' function. */ +#define HAVE_RAND_R 1 + +/* Define to 1 if you have the `readlink' function. */ +#define HAVE_READLINK 1 + +/* Define to 1 if you have the header file. */ +/* #undef HAVE_SCHED_H */ + +/* Have sched_yield */ +/* #undef HAVE_SCHED_YIELD */ + +/* Have Solaris __machine_*_barrier and atomic_* operations */ +/* #undef HAVE_SOLARIS_ATOMIC_OPS */ + +/* Define to 1 if you have the header file. */ +#define HAVE_STDINT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDLIB_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRINGS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRING_H 1 + +/* Define to 1 if `d_type' is a member of `struct dirent'. */ +#define HAVE_STRUCT_DIRENT_D_TYPE 1 + +/* Define to 1 if `f_flags' is a member of `struct statfs'. */ +#define HAVE_STRUCT_STATFS_F_FLAGS 1 + +/* Define to 1 if `f_fstypename' is a member of `struct statfs'. */ +/* #undef HAVE_STRUCT_STATFS_F_FSTYPENAME */ + +/* Define to 1 if `f_basetype' is a member of `struct statvfs'. */ +/* #undef HAVE_STRUCT_STATVFS_F_BASETYPE */ + +/* Define to 1 if `f_fstypename' is a member of `struct statvfs'. */ +/* #undef HAVE_STRUCT_STATVFS_F_FSTYPENAME */ + +/* Define to 1 if you have the header file, and it defines `DIR'. + */ +/* #undef HAVE_SYS_DIR_H */ + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_MOUNT_H 1 + +/* Define to 1 if you have the header file, and it defines `DIR'. + */ +/* #undef HAVE_SYS_NDIR_H */ + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_PARAM_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STATFS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STATVFS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STAT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_TYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_VFS_H 1 + +/* Define to 1 if `usLowerOpticalPointSize' is a member of `TT_OS2'. */ +#define HAVE_TT_OS2_USLOWEROPTICALPOINTSIZE 1 + +/* Define to 1 if `usUpperOpticalPointSize' is a member of `TT_OS2'. */ +#define HAVE_TT_OS2_USUPPEROPTICALPOINTSIZE 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_UNISTD_H 1 + +/* Define to 1 if you have the `vprintf' function. */ +#define HAVE_VPRINTF 1 + +/* Can use #warning in C files */ +#define HAVE_WARNING_CPP_DIRECTIVE 1 + +/* Use xmlparse.h instead of expat.h */ +/* #undef HAVE_XMLPARSE_H */ + +/* Define to 1 if you have the `XML_SetDoctypeDeclHandler' function. */ +#define HAVE_XML_SETDOCTYPEDECLHANDLER 1 + +/* Define to 1 if you have the `_mktemp_s' function. */ +/* #undef HAVE__MKTEMP_S */ + +/* Define to the sub-directory in which libtool stores uninstalled libraries. + */ +#define LT_OBJDIR ".libs/" + +/* Name of package */ +#define PACKAGE "fontconfig" + +/* Define to the address where bug reports for this package should be sent. */ +#define PACKAGE_BUGREPORT \ + "https://bugs.freedesktop.org/enter_bug.cgi?product=fontconfig" + +/* Define to the full name of this package. */ +#define PACKAGE_NAME "fontconfig" + +/* Define to the full name and version of this package. */ +#define PACKAGE_STRING "fontconfig 2.11.94" + +/* Define to the one symbol short name of this package. */ +#define PACKAGE_TARNAME "fontconfig" + +/* Define to the home page for this package. */ +#define PACKAGE_URL "" + +/* Define to the version of this package. */ +#define PACKAGE_VERSION "2.11.94" + +/* Define to necessary symbol if this constant uses a non-standard name on + your system. */ +/* #undef PTHREAD_CREATE_JOINABLE */ + +/* The size of `char', as computed by sizeof. */ +/* #undef SIZEOF_CHAR */ + +/* The size of `int', as computed by sizeof. */ +/* #undef SIZEOF_INT */ + +/* The size of `long', as computed by sizeof. */ +/* #undef SIZEOF_LONG */ + +/* The size of `short', as computed by sizeof. */ +/* #undef SIZEOF_SHORT */ + +/* The size of `void*', as computed by sizeof. */ +/* #undef SIZEOF_VOIDP */ + +/* The size of `void *', as computed by sizeof. */ +#define SIZEOF_VOID_P 8 + +/* Define to 1 if you have the ANSI C header files. */ +#define STDC_HEADERS 1 + +/* Use iconv. */ +#define USE_ICONV 0 + +/* Enable extensions on AIX 3, Interix. */ +#ifndef _ALL_SOURCE +#define _ALL_SOURCE 1 +#endif +/* Enable GNU extensions on systems that have them. */ +#ifndef _GNU_SOURCE +#define _GNU_SOURCE 1 +#endif +/* Enable threading extensions on Solaris. */ +#ifndef _POSIX_PTHREAD_SEMANTICS +#define _POSIX_PTHREAD_SEMANTICS 1 +#endif +/* Enable extensions on HP NonStop. */ +#ifndef _TANDEM_SOURCE +#define _TANDEM_SOURCE 1 +#endif +/* Enable general extensions on Solaris. */ +#ifndef __EXTENSIONS__ +#define __EXTENSIONS__ 1 +#endif + +/* Version number of package */ +#define VERSION "2.11.94" + +/* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most + significant byte first (like Motorola and SPARC, unlike Intel). */ +#if defined AC_APPLE_UNIVERSAL_BUILD +#if defined __BIG_ENDIAN__ +#define WORDS_BIGENDIAN 1 +#endif +#else +#ifndef WORDS_BIGENDIAN +/* # undef WORDS_BIGENDIAN */ +#endif +#endif + +/* Enable large inode numbers on Mac OS X 10.5. */ +#ifndef _DARWIN_USE_64_BIT_INODE +#define _DARWIN_USE_64_BIT_INODE 1 +#endif + +/* Number of bits in a file offset, on hosts where this is settable. */ +/* #undef _FILE_OFFSET_BITS */ + +/* Define for large files, on AIX-style hosts. */ +/* #undef _LARGE_FILES */ + +/* Define to 1 if on MINIX. */ +/* #undef _MINIX */ + +/* Define to 2 if the system does not provide POSIX.1 features except with + this defined. */ +/* #undef _POSIX_1_SOURCE */ + +/* Define to 1 if you need to in order for `stat' and other things to work. */ +/* #undef _POSIX_SOURCE */ + +/* Define to empty if `const' does not conform to ANSI C. */ +/* #undef const */ + +/* Define to `__inline__' or `__inline' if that's what the C compiler + calls it, or to nothing if 'inline' is not supported under any name. */ +#ifndef __cplusplus +/* #undef inline */ +#endif + +/* Define to `int' if does not define. */ +/* #undef pid_t */ diff --git a/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/fontconfig.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/fontconfig.BUILD new file mode 100644 index 0000000..214397f --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/fontconfig/fontconfig.BUILD @@ -0,0 +1,130 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# Fontconfig is a library for font customization and configuration. + +licenses(["notice"]) # BSD-like + +genrule( + name = "config_h", + srcs = ["@com_github_googlecartographer_cartographer//bazel/third_party/fontconfig:config.h"], + outs = ["fontconfig_internal/config.h"], + cmd = "cp $< $@", +) + +genrule( + name = "fcalias", + srcs = [ + "fontconfig/fontconfig.h", + "src/fcdeprecate.h", + "fontconfig/fcprivate.h", + ] + glob(["src/*.c"]), + outs = [ + "src/fcalias.h", + "src/fcaliastail.h", + ], + cmd = """./$(location src/makealias) \ + $$(dirname $(location src/makealias)) \ + $(OUTS) \ + $(location fontconfig/fontconfig.h) \ + $(location src/fcdeprecate.h) \ + $(location fontconfig/fcprivate.h)""", + tools = ["src/makealias"], +) + +genrule( + name = "fcftalias", + srcs = ["fontconfig/fcfreetype.h"] + glob(["src/*.c"]), + outs = [ + "src/fcftalias.h", + "src/fcftaliastail.h", + ], + cmd = """./$(location src/makealias) \ + $$(dirname $(location src/makealias)) \ + $(OUTS) \ + $(location fontconfig/fcfreetype.h)""", + tools = ["src/makealias"], +) + +cc_library( + name = "fontconfig", + srcs = [ + "fc-blanks/fcblanks.h", + "fc-case/fccase.h", + "fc-glyphname/fcglyphname.h", + "fc-lang/fclang.h", + "fontconfig/fcfreetype.h", + "fontconfig/fcprivate.h", + "src/fcarch.h", + "src/fcatomic.c", + "src/fcatomic.h", + "src/fcblanks.c", + "src/fccache.c", + "src/fccfg.c", + "src/fccharset.c", + "src/fccompat.c", + "src/fcdbg.c", + "src/fcdefault.c", + "src/fcdeprecate.h", + "src/fcdir.c", + "src/fcformat.c", + "src/fcfreetype.c", + "src/fcfs.c", + "src/fcftint.h", + "src/fcinit.c", + "src/fcint.h", + "src/fclang.c", + "src/fclist.c", + "src/fcmatch.c", + "src/fcmatrix.c", + "src/fcmutex.h", + "src/fcname.c", + "src/fcobjs.c", + "src/fcobjs.h", + "src/fcobjshash.h", + "src/fcpat.c", + "src/fcrange.c", + "src/fcserialize.c", + "src/fcstat.c", + "src/fcstdint.h", + "src/fcstr.c", + "src/fcweight.c", + "src/fcxml.c", + "src/ftglue.c", + "src/ftglue.h", + ":config_h", + ":fcalias", + ":fcftalias", + ], + hdrs = [ + "fontconfig/fontconfig.h", + ], + copts = [ + "-Iexternal/org_freedesktop_fontconfig/src", + "-I$(GENDIR)/external/org_freedesktop_fontconfig/src", + "-I$(GENDIR)/external/org_freedesktop_fontconfig/fontconfig_internal", + "-DFC_CACHEDIR='\"/var/cache/fontconfig\"'", + "-DFONTCONFIG_PATH='\"/etc/fonts\"'", + "-DHAVE_CONFIG_H", + "-Wno-strict-aliasing", + ], + includes = ["."], + visibility = ["//visibility:public"], + deps = [ + "@com_github_libexpat_libexpat//:expat", + "@net_zlib_zlib//:zlib", + "@org_freetype_freetype2//:freetype2", + ], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/freetype2.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/freetype2.BUILD new file mode 100644 index 0000000..282c134 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/freetype2.BUILD @@ -0,0 +1,262 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# A free font library. + +licenses(["notice"]) # BSD-style + +cc_library( + name = "freetype2", + srcs = [ + "builds/unix/ftsystem.c", + "src/autofit/autofit.c", + "src/base/ftbase.c", + "src/base/ftbbox.c", + "src/base/ftbdf.c", + "src/base/ftbitmap.c", + "src/base/ftcid.c", + "src/base/ftdebug.c", + "src/base/ftfntfmt.c", + "src/base/ftfstype.c", + "src/base/ftgasp.c", + "src/base/ftglyph.c", + "src/base/ftgxval.c", + "src/base/ftinit.c", + "src/base/ftlcdfil.c", + "src/base/ftmm.c", + "src/base/ftotval.c", + "src/base/ftpatent.c", + "src/base/ftpfr.c", + "src/base/ftstroke.c", + "src/base/ftsynth.c", + "src/base/fttype1.c", + "src/base/ftwinfnt.c", + "src/bdf/bdf.c", + "src/bzip2/ftbzip2.c", + "src/cache/ftcache.c", + "src/cff/cff.c", + "src/cid/type1cid.c", + "src/gxvalid/gxvalid.c", + "src/gzip/ftgzip.c", + "src/lzw/ftlzw.c", + "src/otvalid/otvalid.c", + "src/pcf/pcf.c", + "src/pfr/pfr.c", + "src/psaux/psaux.c", + "src/pshinter/pshinter.c", + "src/psnames/psnames.c", + "src/raster/raster.c", + "src/sfnt/sfnt.c", + "src/smooth/smooth.c", + "src/truetype/truetype.c", + "src/type1/type1.c", + "src/type42/type42.c", + "src/winfonts/winfnt.c", + ] + glob([ + "src/**/*.h", + "builds/unix/*.h", + "include/freetype/internal/**/*.h", + ]), + hdrs = glob([ + "include/freetype/*.h", + "include/freetype/config/*.h", + "include/ft2build.h", + ]) + [ + "src/autofit/afangles.c", + "src/autofit/afblue.c", + "src/autofit/afcjk.c", + "src/autofit/afdummy.c", + "src/autofit/afglobal.c", + "src/autofit/afhints.c", + "src/autofit/afindic.c", + "src/autofit/aflatin.c", + "src/autofit/aflatin2.c", + "src/autofit/afloader.c", + "src/autofit/afmodule.c", + "src/autofit/afpic.c", + "src/autofit/afranges.c", + "src/autofit/afwarp.c", + "src/autofit/afshaper.c", + "src/base/basepic.c", + "src/base/ftadvanc.c", + "src/base/ftapi.c", + "src/base/ftcalc.c", + "src/base/ftdbgmem.c", + "src/base/ftgloadr.c", + "src/base/fthash.c", + "src/base/ftmac.c", + "src/base/ftobjs.c", + "src/base/ftoutln.c", + "src/base/ftpic.c", + "src/base/ftrfork.c", + "src/base/ftsnames.c", + "src/base/ftstream.c", + "src/base/ftsystem.c", + "src/base/fttrigon.c", + "src/base/ftutil.c", + "src/base/md5.c", + "src/bdf/bdfdrivr.c", + "src/bdf/bdflib.c", + "src/cache/ftcbasic.c", + "src/cache/ftccache.c", + "src/cache/ftccmap.c", + "src/cache/ftcglyph.c", + "src/cache/ftcimage.c", + "src/cache/ftcmanag.c", + "src/cache/ftcmru.c", + "src/cache/ftcsbits.c", + "src/cff/cf2arrst.c", + "src/cff/cf2blues.c", + "src/cff/cf2error.c", + "src/cff/cf2font.c", + "src/cff/cf2ft.c", + "src/cff/cf2hints.c", + "src/cff/cf2intrp.c", + "src/cff/cf2read.c", + "src/cff/cf2stack.c", + "src/cff/cffcmap.c", + "src/cff/cffdrivr.c", + "src/cff/cffgload.c", + "src/cff/cffload.c", + "src/cff/cffobjs.c", + "src/cff/cffparse.c", + "src/cff/cffpic.c", + "src/cid/cidgload.c", + "src/cid/cidload.c", + "src/cid/cidobjs.c", + "src/cid/cidparse.c", + "src/cid/cidriver.c", + "src/gxvalid/gxvalid.c", + "src/gxvalid/gxvbsln.c", + "src/gxvalid/gxvcommn.c", + "src/gxvalid/gxvfeat.c", + "src/gxvalid/gxvfgen.c", + "src/gxvalid/gxvjust.c", + "src/gxvalid/gxvkern.c", + "src/gxvalid/gxvlcar.c", + "src/gxvalid/gxvmod.c", + "src/gxvalid/gxvmort.c", + "src/gxvalid/gxvmort0.c", + "src/gxvalid/gxvmort1.c", + "src/gxvalid/gxvmort2.c", + "src/gxvalid/gxvmort4.c", + "src/gxvalid/gxvmort5.c", + "src/gxvalid/gxvmorx.c", + "src/gxvalid/gxvmorx0.c", + "src/gxvalid/gxvmorx1.c", + "src/gxvalid/gxvmorx2.c", + "src/gxvalid/gxvmorx4.c", + "src/gxvalid/gxvmorx5.c", + "src/gxvalid/gxvopbd.c", + "src/gxvalid/gxvprop.c", + "src/gxvalid/gxvtrak.c", + "src/gzip/adler32.c", + "src/gzip/infblock.c", + "src/gzip/infcodes.c", + "src/gzip/inflate.c", + "src/gzip/inftrees.c", + "src/gzip/infutil.c", + "src/gzip/zutil.c", + "src/lzw/ftzopen.c", + "src/otvalid/otvalid.c", + "src/otvalid/otvbase.c", + "src/otvalid/otvcommn.c", + "src/otvalid/otvgdef.c", + "src/otvalid/otvgpos.c", + "src/otvalid/otvgsub.c", + "src/otvalid/otvjstf.c", + "src/otvalid/otvmath.c", + "src/otvalid/otvmod.c", + "src/pcf/pcfdrivr.c", + "src/pcf/pcfread.c", + "src/pcf/pcfutil.c", + "src/pfr/pfrcmap.c", + "src/pfr/pfrdrivr.c", + "src/pfr/pfrgload.c", + "src/pfr/pfrload.c", + "src/pfr/pfrobjs.c", + "src/pfr/pfrsbit.c", + "src/psaux/afmparse.c", + "src/psaux/psauxmod.c", + "src/psaux/psconv.c", + "src/psaux/psobjs.c", + "src/psaux/t1cmap.c", + "src/psaux/t1decode.c", + "src/pshinter/pshalgo.c", + "src/pshinter/pshglob.c", + "src/pshinter/pshmod.c", + "src/pshinter/pshpic.c", + "src/pshinter/pshrec.c", + "src/psnames/psmodule.c", + "src/psnames/pspic.c", + "src/raster/ftraster.c", + "src/raster/ftrend1.c", + "src/raster/rastpic.c", + "src/sfnt/pngshim.c", + "src/sfnt/sfdriver.c", + "src/sfnt/sfntpic.c", + "src/sfnt/sfobjs.c", + "src/sfnt/ttbdf.c", + "src/sfnt/ttcmap.c", + "src/sfnt/ttkern.c", + "src/sfnt/ttload.c", + "src/sfnt/ttmtx.c", + "src/sfnt/ttpost.c", + "src/sfnt/ttsbit.c", + "src/smooth/ftgrays.c", + "src/smooth/ftsmooth.c", + "src/smooth/ftspic.c", + "src/tools/ftrandom/ftrandom.c", + "src/tools/test_afm.c", + "src/tools/test_bbox.c", + "src/tools/test_trig.c", + "src/truetype/ttdriver.c", + "src/truetype/ttgload.c", + "src/truetype/ttgxvar.c", + "src/truetype/ttinterp.c", + "src/truetype/ttobjs.c", + "src/truetype/ttpic.c", + "src/truetype/ttpload.c", + "src/truetype/ttsubpix.c", + "src/type1/t1afm.c", + "src/type1/t1driver.c", + "src/type1/t1gload.c", + "src/type1/t1load.c", + "src/type1/t1objs.c", + "src/type1/t1parse.c", + "src/type42/t42drivr.c", + "src/type42/t42objs.c", + "src/type42/t42parse.c", + ], + copts = [ + "-Wno-covered-switch-default", + "-DFT_CONFIG_OPTION_SYSTEM_ZLIB", + "-DFT_CONFIG_CONFIG_H=", + "-DFT_CONFIG_OPTION_USE_PNG", + "-DFT2_BUILD_LIBRARY", + "-DFT_CONFIG_MODULES_H=", + "-DHAVE_UNISTD_H=1", + "-DHAVE_FCNTL_H=1", + "-DHAVE_STDINT_H=1", + "-Iexternal/org_freetype_freetype2/builds/unix", + "-Iexternal/org_freetype_freetype2/include/freetype/config", + ], + includes = ["include"], + visibility = ["//visibility:public"], + deps = [ + "@net_zlib_zlib//:zlib", + "@org_libpng_libpng//:libpng", + ], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/gd.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/gd.BUILD new file mode 100644 index 0000000..c351eaf --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/gd.BUILD @@ -0,0 +1,124 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# GD is a library for the dynamic creation of images. + +load("@com_github_antonovvk_bazel_rules//:config.bzl", "cc_fix_config") + +licenses(["notice"]) # simple notice-style license + +cc_fix_config( + name = "config_h", + cmake = True, + files = {"src/config.h.cmake": "gd_internal/config.h"}, + values = { + "HAVE_DIRENT_H": "1", + "HAVE_DLFCN_H": "1", + "HAVE_ERRNO_H": "1", + "HAVE_FT2BUILD_H": "1", + "HAVE_ICONV": "1", + "HAVE_ICONV_H": "1", + "HAVE_ICONV_T_DEF": "1", + "HAVE_INTTYPES_H": "1", + "HAVE_LIBFREETYPE": "1", + "HAVE_LIBJPEG": "1", + "HAVE_LIBM": "1", + "HAVE_LIBPNG": "1", + "HAVE_LIBZ": "1", + "HAVE_LIMITS_H": "1", + "HAVE_MEMORY_H": "1", + "HAVE_PTHREAD": "1", + "HAVE_PTHREAD_PRIO_INHERIT": "1", + "HAVE_STDDEF_H": "1", + "HAVE_STDINT_H": "1", + "HAVE_STDLIB_H": "1", + "HAVE_STRINGS_H": "1", + "HAVE_STRING_H": "1", + "HAVE_SYS_STAT_H": "1", + "HAVE_SYS_TYPES_H": "1", + "HAVE_UNISTD_H": "1", + "HAVE_VISIBILITY": "1", + "ICONV_CONST": "", + "LT_OBJDIR": "\".libs/\"", + "PACKAGE": "libgd", + "PACKAGE_BUGREPORT": "https://bitbucket.org/libgd/gd-libgd/issues", + "PACKAGE_NAME": "GD", + "PACKAGE_STRING": "GD 2.2.4", + "PACKAGE_TARNAME": "libgd", + "PACKAGE_URL": "http://lib.gd", + "PACKAGE_VERSION": "2.2.4", + "STDC_HEADERS": "1", + "VERSION": "2.2.4", + }, +) + +cc_library( + name = "gd", + srcs = [ + "src/gd.c", + "src/gd_color.c", + "src/gd_gd.c", + "src/gd_gd2.c", + "src/gd_gif_in.c", + "src/gd_gif_out.c", + "src/gd_io.c", + "src/gd_io_dp.c", + "src/gd_io_file.c", + "src/gd_io_ss.c", + "src/gd_jpeg.c", + "src/gd_nnquant.c", + "src/gd_png.c", + "src/gd_security.c", + "src/gd_ss.c", + "src/gd_topal.c", + "src/gd_wbmp.c", + "src/gd_xbm.c", + "src/gdcache.c", + "src/gdfontg.c", + "src/gdfontl.c", + "src/gdfontmb.c", + "src/gdfonts.c", + "src/gdfontt.c", + "src/gdft.c", + "src/gdfx.c", + "src/gdhelpers.c", + "src/gdkanji.c", + "src/gdtables.c", + "src/gdxpm.c", + "src/wbmp.c", + "gd_internal/config.h", + ] + glob([ + "src/*.h", + ]), + hdrs = [ + "src/gd.h", + "src/gdhelpers.h", + ], + copts = [ + "-I$(GENDIR)/external/org_libgd_libgd/gd_internal", + "-DFC_CACHEDIR='\"/var/cache/fontconfig\"'", + "-DFONTCONFIG_PATH='\"/etc/fonts\"'", + "-DHAVE_CONFIG_H", + ], + includes = ["src"], + linkopts = ["-lm"], + visibility = ["//visibility:public"], + deps = [ + "@libjpeg//:jpeg", + "@net_zlib_zlib//:zlib", + "@org_freetype_freetype2//:freetype2", + "@org_libpng_libpng//:libpng", + ], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/libjpeg.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/libjpeg.BUILD new file mode 100644 index 0000000..cb9aa9d --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/libjpeg.BUILD @@ -0,0 +1,103 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# The Independent JPEG Group's JPEG runtime library. + +licenses(["notice"]) # custom notice-style license, see LICENSE + +cc_library( + name = "jpeg", + srcs = [ + "cderror.h", + "cdjpeg.h", + "jaricom.c", + "jcapimin.c", + "jcapistd.c", + "jcarith.c", + "jccoefct.c", + "jccolor.c", + "jcdctmgr.c", + "jchuff.c", + "jcinit.c", + "jcmainct.c", + "jcmarker.c", + "jcmaster.c", + "jcomapi.c", + "jconfig.h", + "jcparam.c", + "jcprepct.c", + "jcsample.c", + "jctrans.c", + "jdapimin.c", + "jdapistd.c", + "jdarith.c", + "jdatadst.c", + "jdatasrc.c", + "jdcoefct.c", + "jdcolor.c", + "jdct.h", + "jddctmgr.c", + "jdhuff.c", + "jdinput.c", + "jdmainct.c", + "jdmarker.c", + "jdmaster.c", + "jdmerge.c", + "jdpostct.c", + "jdsample.c", + "jdtrans.c", + "jerror.c", + "jfdctflt.c", + "jfdctfst.c", + "jfdctint.c", + "jidctflt.c", + "jidctfst.c", + "jidctint.c", + "jinclude.h", + "jmemmgr.c", + "jmemnobs.c", + "jmemsys.h", + "jmorecfg.h", + "jquant1.c", + "jquant2.c", + "jutils.c", + "jversion.h", + "transupp.h", + ], + hdrs = [ + "jerror.h", + "jpegint.h", + "jpeglib.h", + ], + includes = ["."], + visibility = ["//visibility:public"], +) + +genrule( + name = "configure", + outs = ["jconfig.h"], + cmd = "cat <$@\n" + + "#define HAVE_PROTOTYPES 1\n" + + "#define HAVE_UNSIGNED_CHAR 1\n" + + "#define HAVE_UNSIGNED_SHORT 1\n" + + "#define HAVE_STDDEF_H 1\n" + + "#define HAVE_STDLIB_H 1\n" + + "#ifdef WIN32\n" + + "#define INLINE __inline\n" + + "#else\n" + + "#define INLINE __inline__\n" + + "#endif\n" + + "EOF\n", +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/libpng.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/libpng.BUILD new file mode 100644 index 0000000..23d9a29 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/libpng.BUILD @@ -0,0 +1,47 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# libpng is the official PNG reference library. + +licenses(["notice"]) # BSD/MIT-like license + +cc_library( + name = "libpng", + srcs = [ + "png.c", + "pngerror.c", + "pngget.c", + "pngmem.c", + "pngpread.c", + "pngread.c", + "pngrio.c", + "pngrtran.c", + "pngrutil.c", + "pngset.c", + "pngtrans.c", + "pngwio.c", + "pngwrite.c", + "pngwtran.c", + "pngwutil.c", + ], + hdrs = [ + "png.h", + "pngconf.h", + ], + includes = ["."], + linkopts = ["-lm"], + visibility = ["//visibility:public"], + deps = ["@net_zlib_zlib//:zlib"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/lua.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/lua.BUILD new file mode 100644 index 0000000..49284a9 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/lua.BUILD @@ -0,0 +1,109 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# Lua language interpreter. + +package( + default_visibility = ["//visibility:public"], +) + +licenses(["notice"]) # MIT + +cc_library( + name = "lua_includes", + hdrs = [ + "src/lauxlib.h", + "src/lua.h", + "src/lua.hpp", + "src/luaconf.h", + "src/lualib.h", + ], + includes = ["src"], +) + +cc_library( + name = "lua", + srcs = [ + # Core language + "src/lapi.c", + "src/lapi.h", + "src/lcode.c", + "src/lcode.h", + "src/lctype.c", + "src/lctype.h", + "src/ldebug.c", + "src/ldebug.h", + "src/ldo.c", + "src/ldo.h", + "src/ldump.c", + "src/lfunc.c", + "src/lfunc.h", + "src/lgc.c", + "src/lgc.h", + "src/llex.c", + "src/llex.h", + "src/llimits.h", + "src/lmem.c", + "src/lmem.h", + "src/lobject.c", + "src/lobject.h", + "src/lopcodes.c", + "src/lopcodes.h", + "src/lparser.c", + "src/lparser.h", + "src/lstate.c", + "src/lstate.h", + "src/lstring.c", + "src/lstring.h", + "src/ltable.c", + "src/ltable.h", + "src/ltm.c", + "src/ltm.h", + "src/lundump.c", + "src/lundump.h", + "src/lvm.c", + "src/lvm.h", + "src/lzio.c", + + # Standard libraries + "src/lauxlib.c", + "src/lbaselib.c", + "src/lbitlib.c", + "src/lcorolib.c", + "src/ldblib.c", + "src/linit.c", + "src/liolib.c", + "src/lmathlib.c", + "src/loadlib.c", + "src/loslib.c", + "src/lstrlib.c", + "src/ltablib.c", + "src/lzio.h", + ], + hdrs = [ + "src/lauxlib.h", + "src/lua.h", + "src/lua.hpp", + "src/luaconf.h", + "src/lualib.h", + ], + copts = ["-w"], + defines = ["LUA_USE_LINUX"], + includes = ["src"], + linkopts = [ + "-lm", + "-ldl", + ], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/pixman/BUILD.bazel b/carto_ws/src/cartographer-master/bazel/third_party/pixman/BUILD.bazel new file mode 100644 index 0000000..b3c5577 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/pixman/BUILD.bazel @@ -0,0 +1,25 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Files required for building Pixman with Bazel. + +licenses(["notice"]) + +exports_files( + [ + "pixman.BUILD", + "config.h", + ], + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/pixman/config.h b/carto_ws/src/cartographer-master/bazel/third_party/pixman/config.h new file mode 100644 index 0000000..8f68a1b --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/pixman/config.h @@ -0,0 +1,193 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* Define if building universal (internal helper macro) */ +/* #undef AC_APPLE_UNIVERSAL_BUILD */ + +/* Whether we have alarm() */ +#define HAVE_ALARM 1 + +/* Whether the compiler supports __builtin_clz */ +#define HAVE_BUILTIN_CLZ /**/ + +/* Define to 1 if you have the header file. */ +#define HAVE_DLFCN_H 1 + +/* Whether we have feenableexcept() */ +#define HAVE_FEENABLEEXCEPT 1 + +/* Define to 1 if we have */ +#define HAVE_FENV_H 1 + +/* Whether the tool chain supports __float128 */ +#define HAVE_FLOAT128 /**/ + +/* Define to 1 if you have the `getisax' function. */ +/* #undef HAVE_GETISAX */ + +/* Whether we have getpagesize() */ +#define HAVE_GETPAGESIZE 1 + +/* Whether we have gettimeofday() */ +#define HAVE_GETTIMEOFDAY 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_INTTYPES_H 1 + +/* Define to 1 if you have the `pixman-1' library (-lpixman-1). */ +/* #undef HAVE_LIBPIXMAN_1 */ + +/* Whether we have libpng */ +#define HAVE_LIBPNG 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_MEMORY_H 1 + +/* Whether we have mmap() */ +#define HAVE_MMAP 1 + +/* Whether we have mprotect() */ +#define HAVE_MPROTECT 1 + +/* Whether we have posix_memalign() */ +#define HAVE_POSIX_MEMALIGN 1 + +/* Whether pthreads is supported */ +#define HAVE_PTHREADS /**/ + +/* Whether we have sigaction() */ +#define HAVE_SIGACTION 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDINT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDLIB_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRINGS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRING_H 1 + +/* Define to 1 if we have */ +#define HAVE_SYS_MMAN_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STAT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_TYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_UNISTD_H 1 + +/* Define to the sub-directory in which libtool stores uninstalled libraries. + */ +#define LT_OBJDIR ".libs/" + +/* Name of package */ +#define PACKAGE "pixman" + +/* Define to the address where bug reports for this package should be sent. */ +#define PACKAGE_BUGREPORT "pixman@lists.freedesktop.org" + +/* Define to the full name of this package. */ +#define PACKAGE_NAME "pixman" + +/* Define to the full name and version of this package. */ +#define PACKAGE_STRING "pixman 0.34.0" + +/* Define to the one symbol short name of this package. */ +#define PACKAGE_TARNAME "pixman" + +/* Define to the home page for this package. */ +#define PACKAGE_URL "" + +/* Define to the version of this package. */ +#define PACKAGE_VERSION "0.34.0" + +/* enable TIMER_BEGIN/TIMER_END macros */ +/* #undef PIXMAN_TIMERS */ + +/* The size of `long', as computed by sizeof. */ +#define SIZEOF_LONG 8 + +/* Define to 1 if you have the ANSI C header files. */ +#define STDC_HEADERS 1 + +/* The compiler supported TLS storage class */ +#define TLS __thread + +/* Whether the tool chain supports __attribute__((constructor)) */ +#define TOOLCHAIN_SUPPORTS_ATTRIBUTE_CONSTRUCTOR /**/ + +/* use ARM IWMMXT compiler intrinsics */ +/* #undef USE_ARM_IWMMXT */ + +/* use ARM NEON assembly optimizations */ +/* #undef USE_ARM_NEON */ + +/* use ARM SIMD assembly optimizations */ +/* #undef USE_ARM_SIMD */ + +/* use GNU-style inline assembler */ +#define USE_GCC_INLINE_ASM 1 + +/* use Loongson Multimedia Instructions */ +/* #undef USE_LOONGSON_MMI */ + +/* use MIPS DSPr2 assembly optimizations */ +/* #undef USE_MIPS_DSPR2 */ + +/* use OpenMP in the test suite */ +#define USE_OPENMP 1 + +/* use SSE2 compiler intrinsics */ +/* #undef USE_SSE2 */ + +/* use SSSE3 compiler intrinsics */ +/* #undef USE_SSSE3 */ + +/* use VMX compiler intrinsics */ +/* #undef USE_VMX */ + +/* use x86 MMX compiler intrinsics */ +/* #undef USE_X86_MMX */ + +/* Version number of package */ +#define VERSION "0.34.0" + +/* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most + significant byte first (like Motorola and SPARC, unlike Intel). */ +#if defined AC_APPLE_UNIVERSAL_BUILD +#if defined __BIG_ENDIAN__ +#define WORDS_BIGENDIAN 1 +#endif +#else +#ifndef WORDS_BIGENDIAN +/* # undef WORDS_BIGENDIAN */ +#endif +#endif + +/* Define to `__inline__' or `__inline' if that's what the C compiler + calls it, or to nothing if 'inline' is not supported under any name. */ +#ifndef __cplusplus +/* #undef inline */ +#endif + +/* Define to sqrt if you do not have the `sqrtf' function. */ +/* #undef sqrtf */ diff --git a/carto_ws/src/cartographer-master/bazel/third_party/pixman/pixman.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/pixman/pixman.BUILD new file mode 100644 index 0000000..8b1eb84 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/pixman/pixman.BUILD @@ -0,0 +1,87 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Description: +# pixman is a library that provides low-level pixel manipulation features. + +licenses(["notice"]) # X11/MIT + +genrule( + name = "config_h", + srcs = ["@com_github_googlecartographer_cartographer//bazel/third_party/pixman:config.h"], + outs = ["pixman_internal/config.h"], + cmd = "cp $< $@", +) + +COMMON_SRCS = [ + "pixman/pixman.c", + "pixman/pixman-access.c", + "pixman/pixman-access-accessors.c", + "pixman/pixman-arm.c", + "pixman/pixman-bits-image.c", + "pixman/pixman-combine-float.c", + "pixman/pixman-combine32.c", + "pixman/pixman-conical-gradient.c", + "pixman/pixman-edge-accessors.c", + "pixman/pixman-edge.c", + "pixman/pixman-fast-path.c", + "pixman/pixman-filter.c", + "pixman/pixman-general.c", + "pixman/pixman-glyph.c", + "pixman/pixman-gradient-walker.c", + "pixman/pixman-image.c", + "pixman/pixman-implementation.c", + "pixman/pixman-linear-gradient.c", + "pixman/pixman-matrix.c", + "pixman/pixman-mips.c", + "pixman/pixman-noop.c", + "pixman/pixman-ppc.c", + "pixman/pixman-radial-gradient.c", + "pixman/pixman-region16.c", + "pixman/pixman-region32.c", + "pixman/pixman-solid-fill.c", + "pixman/pixman-timer.c", + "pixman/pixman-trap.c", + "pixman/pixman-utils.c", + "pixman/pixman-x86.c", +] + +# TODO(rodrigoq): use MMX extensions where possible +cc_library( + name = "pixman", + srcs = COMMON_SRCS + [ + "pixman/pixman-accessor.h", + "pixman/pixman-combine32.h", + "pixman/pixman-compiler.h", + "pixman/pixman-edge-imp.h", + "pixman/pixman-inlines.h", + "pixman/pixman-private.h", + "pixman/pixman-version.h", + "pixman/pixman.h", + "pixman_internal/config.h", + ], + hdrs = [ + "pixman/pixman-access.c", + "pixman/pixman-edge.c", + "pixman/pixman-region.c", + ], + copts = [ + "-Wno-unused-const-variable", + "-Wno-unused-local-typedefs", + "-DHAVE_CONFIG_H", + "-I$(GENDIR)/external/org_cairographics_pixman/pixman_internal", + ], + includes = ["pixman"], + visibility = ["//visibility:public"], +) diff --git a/carto_ws/src/cartographer-master/bazel/third_party/zlib.BUILD b/carto_ws/src/cartographer-master/bazel/third_party/zlib.BUILD new file mode 100644 index 0000000..68746f0 --- /dev/null +++ b/carto_ws/src/cartographer-master/bazel/third_party/zlib.BUILD @@ -0,0 +1,58 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +package(default_visibility = ["//visibility:public"]) + +licenses(["notice"]) # BSD/MIT-like license (for zlib) + +cc_library( + name = "zlib", + srcs = [ + "adler32.c", + "compress.c", + "crc32.c", + "crc32.h", + "deflate.c", + "deflate.h", + "gzclose.c", + "gzguts.h", + "gzlib.c", + "gzread.c", + "gzwrite.c", + "infback.c", + "inffast.c", + "inffast.h", + "inffixed.h", + "inflate.c", + "inflate.h", + "inftrees.c", + "inftrees.h", + "trees.c", + "trees.h", + "uncompr.c", + "zconf.h", + "zutil.c", + "zutil.h", + ], + hdrs = ["zlib.h"], + copts = [ + "-Wno-implicit-function-declaration", + ], + includes = ["."], +) + +alias( + name = "z", + actual = ":zlib", +) diff --git a/carto_ws/src/cartographer-master/cartographer-config.cmake.in b/carto_ws/src/cartographer-master/cartographer-config.cmake.in new file mode 100644 index 0000000..98193da --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer-config.cmake.in @@ -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. + +# Usage from an external project: +# In your CMakeLists.txt, add these lines: +# +# find_package(cartographer REQUIRED) +# target_link_libraries(MY_TARGET_NAME PUBLIC cartographer) + +@PACKAGE_INIT@ + +set_and_check(CARTOGRAPHER_CMAKE_DIR "@PACKAGE_CARTOGRAPHER_CMAKE_DIR@") + +set(CERES_DIR_HINTS @Ceres_DIR@) +set(CARTOGRAPHER_HAS_GRPC @CARTOGRAPHER_HAS_GRPC@) + +if (cartographer_FIND_QUIETLY) + set(QUIET_OR_REQUIRED_OPTION "QUIET") +elseif (cartographer_FIND_REQUIRED) + set(QUIET_OR_REQUIRED_OPTION "REQUIRED") +else () + set(QUIET_OR_REQUIRED_OPTION "") +endif() + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CARTOGRAPHER_CMAKE_DIR}/modules) + +find_package(Ceres ${QUIET_OR_REQUIRED_OPTION} HINTS ${CERES_DIR_HINTS}) +if (WIN32) + find_package(glog REQUIRED) +endif() +find_package(absl ${QUIET_OR_REQUIRED_OPTION}) +if(CARTOGRAPHER_HAS_GRPC) + find_package(async_grpc ${QUIET_OR_REQUIRED_OPTION}) +endif() + +include("${CARTOGRAPHER_CMAKE_DIR}/CartographerTargets.cmake") + +unset(QUIET_OR_REQUIRED_OPTION) diff --git a/carto_ws/src/cartographer-master/cartographer/.clang-format b/carto_ws/src/cartographer-master/cartographer/.clang-format new file mode 100644 index 0000000..5650f22 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: Google +DerivePointerAlignment: false diff --git a/carto_ws/src/cartographer-master/cartographer/BUILD.bazel b/carto_ws/src/cartographer-master/cartographer/BUILD.bazel new file mode 100644 index 0000000..33d54fd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/BUILD.bazel @@ -0,0 +1,135 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Top-level proto and C++ targets for Cartographer. + +load("@com_github_antonovvk_bazel_rules//:config.bzl", "cc_fix_config") + +package( + default_visibility = ["//visibility:public"], +) + +licenses(["notice"]) # Apache 2.0 + +proto_library( + name = "protos", + srcs = glob([ + "**/*.proto", + ]), + deps = [ + "@com_google_protobuf//:empty_proto", + ], +) + +cc_proto_library( + name = "cc_protos", + deps = [":protos"], +) + +cc_fix_config( + name = "common_config_h", + cmake = True, + files = {"common/config.h.cmake": "common/config.h"}, + values = { + "CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY": "configuration_files", + "PROJECT_SOURCE_DIR": "todo_set_project_source_dir_in_cartographer.BUILD", + }, + visibility = ["//visibility:private"], +) + +TEST_LIBRARY_SRCS = glob([ + "**/*test_helpers.cc", + "**/fake_*.cc", + "**/mock_*.cc", +]) + +TEST_LIBRARY_HDRS = glob([ + "**/*test_helpers.h", + "**/fake_*.h", + "**/mock_*.h", +]) + +cc_library( + name = "cartographer_test_library", + testonly = 1, + srcs = TEST_LIBRARY_SRCS, + hdrs = TEST_LIBRARY_HDRS, + deps = [ + ":cartographer", + "@com_google_googletest//:gtest", + ], +) + +cc_library( + name = "cartographer", + srcs = glob( + [ + "**/*.cc", + ], + exclude = [ + "**/*_main.cc", + "**/*_test.cc", + ] + TEST_LIBRARY_SRCS, + ), + hdrs = [ + "common/config.h", + ] + glob( + [ + "**/*.h", + ], + exclude = TEST_LIBRARY_HDRS, + ), + copts = ["-Wno-sign-compare"], + includes = ["."], + deps = [ + ":cc_protos", + "@boost//:iostreams", + "@com_google_absl//absl/base", + "@com_google_absl//absl/container:flat_hash_map", + "@com_google_absl//absl/container:flat_hash_set", + "@com_google_absl//absl/strings", + "@com_google_absl//absl/strings:str_format", + "@com_google_absl//absl/synchronization", + "@com_google_absl//absl/types:optional", + "@com_google_glog//:glog", + "@org_cairographics_cairo//:cairo", + "@org_ceres_solver_ceres_solver//:ceres", + "@org_lua_lua//:lua", + "@org_tuxfamily_eigen//:eigen", + ], +) + +cc_binary( + name = "cartographer_print_configuration", + srcs = ["common/print_configuration_main.cc"], + deps = [ + ":cartographer", + "@com_github_gflags_gflags//:gflags", + "@com_google_glog//:glog", + ], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", ""), + srcs = [src], + data = ["//:configuration_files"], + deps = [ + ":cartographer", + ":cartographer_test_library", + "@com_google_absl//absl/memory", + "@com_google_googletest//:gtest_main", + ], +) for src in glob( + ["**/*_test.cc"], +)] diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/BUILD.bazel b/carto_ws/src/cartographer-master/cartographer/cloud/BUILD.bazel new file mode 100644 index 0000000..57d8f7b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/BUILD.bazel @@ -0,0 +1,106 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Top-level proto and C++ targets for Cartographer's gRPC server. + +licenses(["notice"]) # Apache 2.0 + +package(default_visibility = ["//visibility:public"]) + +proto_library( + name = "protos", + srcs = glob( + [ + "**/*.proto", + ], + ), + deps = [ + "//cartographer:protos", + "@com_google_protobuf//:empty_proto", + ], +) + +cc_proto_library( + name = "cc_protos", + deps = [":protos"], +) + +# TODO(rodrigoq): This special rule name is required by cc_grpc_library. This +# makes :protos look like it was created by +# cc_grpc_library(proto_only=True, ...) +proto_library( + name = "_cc_protos_only", + deps = [ + ":protos", + "//cartographer:protos", + "@com_google_protobuf//:empty_proto", + ], +) + +cc_library( + name = "cartographer_grpc", + srcs = glob( + [ + "**/*.cc", + ], + exclude = [ + "**/*_main.cc", + "**/*_test.cc", + ], + ), + hdrs = glob([ + "**/*.h", + ]), + copts = ["-Wno-sign-compare"], + defines = ["USE_PROMETHEUS=1"], + includes = ["."], + deps = [ + ":cc_protos", + "//cartographer", + "@com_github_googlecartographer_async_grpc//async_grpc", + "@com_github_grpc_grpc//:grpc++", + "@com_github_jupp0r_prometheus_cpp//core", + "@com_github_jupp0r_prometheus_cpp//pull", + "@com_google_glog//:glog", + "@com_google_protobuf//:cc_wkt_protos", + ], +) + +cc_binary( + name = "cartographer_grpc_server", + srcs = ["map_builder_server_main.cc"], + deps = [ + ":cartographer_grpc", + "@com_github_gflags_gflags//:gflags", + "@com_google_glog//:glog", + ], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", ""), + srcs = [src], + data = ["//:configuration_files"], + # TODO(gaschler): Fix UplinkServerRestarting test for Bazel. + args = ["--gtest_filter=-ClientServerTestByGridType/ClientServerTestByGridType.LocalSlam2DUplinkServerRestarting*"], + flaky = True, # :internal_client_server_test sometimes fails. + # Tests cannot run concurrently as some of them open the same port. + tags = ["exclusive"], + deps = [ + ":cartographer_grpc", + "//cartographer:cartographer_test_library", + "@com_google_googletest//:gtest_main", + ], +) for src in glob( + ["**/*_test.cc"], +)] diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/client/map_builder_stub.cc b/carto_ws/src/cartographer-master/cartographer/cloud/client/map_builder_stub.cc new file mode 100644 index 0000000..6ff4550 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/client/map_builder_stub.cc @@ -0,0 +1,249 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/cloud/client/map_builder_stub.h" + +#include "cartographer/cloud/internal/client/pose_graph_stub.h" +#include "cartographer/cloud/internal/client/trajectory_builder_stub.h" +#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/get_submap_handler.h" +#include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" +#include "cartographer/cloud/internal/handlers/load_state_handler.h" +#include "cartographer/cloud/internal/handlers/write_state_handler.h" +#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "glog/logging.h" + +namespace cartographer { +namespace cloud { +namespace { + +using absl::make_unique; +constexpr int kChannelTimeoutSeconds = 10; +constexpr int kRetryBaseDelayMilliseconds = 500; +constexpr float kRetryDelayFactor = 2.0; +constexpr int kMaxRetries = 5; + +} // namespace + +MapBuilderStub::MapBuilderStub(const std::string& server_address, + const std::string& client_id) + : client_channel_(::grpc::CreateChannel( + server_address, ::grpc::InsecureChannelCredentials())), + pose_graph_stub_(make_unique(client_channel_, client_id)), + client_id_(client_id) { + LOG(INFO) << "Connecting to SLAM process at " << server_address + << " with client_id " << client_id; + std::chrono::system_clock::time_point deadline( + std::chrono::system_clock::now() + + std::chrono::seconds(kChannelTimeoutSeconds)); + if (!client_channel_->WaitForConnected(deadline)) { + LOG(FATAL) << "Failed to connect to " << server_address; + } +} + +int MapBuilderStub::AddTrajectoryBuilder( + const std::set& expected_sensor_ids, + const mapping::proto::TrajectoryBuilderOptions& trajectory_options, + LocalSlamResultCallback local_slam_result_callback) { + proto::AddTrajectoryRequest request; + request.set_client_id(client_id_); + *request.mutable_trajectory_builder_options() = trajectory_options; + for (const auto& sensor_id : expected_sensor_ids) { + *request.add_expected_sensor_ids() = cloud::ToProto(sensor_id); + } + async_grpc::Client client( + client_channel_, common::FromSeconds(kChannelTimeoutSeconds), + async_grpc::CreateLimitedBackoffStrategy( + common::FromMilliseconds(kRetryBaseDelayMilliseconds), + kRetryDelayFactor, kMaxRetries)); + CHECK(client.Write(request)); + + // Construct trajectory builder stub. + trajectory_builder_stubs_.emplace( + std::piecewise_construct, + std::forward_as_tuple(client.response().trajectory_id()), + std::forward_as_tuple(make_unique( + client_channel_, client.response().trajectory_id(), client_id_, + local_slam_result_callback))); + return client.response().trajectory_id(); +} + +int MapBuilderStub::AddTrajectoryForDeserialization( + const mapping::proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) { + LOG(FATAL) << "Not implemented"; +} + +mapping::TrajectoryBuilderInterface* MapBuilderStub::GetTrajectoryBuilder( + int trajectory_id) const { + auto it = trajectory_builder_stubs_.find(trajectory_id); + if (it == trajectory_builder_stubs_.end()) { + return nullptr; + } + return it->second.get(); +} + +void MapBuilderStub::FinishTrajectory(int trajectory_id) { + proto::FinishTrajectoryRequest request; + request.set_client_id(client_id_); + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + ::grpc::Status status; + client.Write(request, &status); + if (!status.ok()) { + LOG(ERROR) << "Failed to finish trajectory " << trajectory_id + << " for client_id " << client_id_ << ": " + << status.error_message(); + return; + } + trajectory_builder_stubs_.erase(trajectory_id); +} + +std::string MapBuilderStub::SubmapToProto( + const mapping::SubmapId& submap_id, + mapping::proto::SubmapQuery::Response* submap_query_response) { + proto::GetSubmapRequest request; + submap_id.ToProto(request.mutable_submap_id()); + async_grpc::Client client(client_channel_); + CHECK(client.Write(request)); + submap_query_response->CopyFrom(client.response().submap_query_response()); + return client.response().error_msg(); +} + +void MapBuilderStub::SerializeState(bool include_unfinished_submaps, + io::ProtoStreamWriterInterface* writer) { + if (include_unfinished_submaps) { + LOG(WARNING) << "Serializing unfinished submaps is currently unsupported. " + "Proceeding to write the state without them."; + } + google::protobuf::Empty request; + async_grpc::Client client(client_channel_); + CHECK(client.Write(request)); + proto::WriteStateResponse response; + while (client.StreamRead(&response)) { + switch (response.state_chunk_case()) { + case proto::WriteStateResponse::kHeader: + writer->WriteProto(response.header()); + break; + case proto::WriteStateResponse::kSerializedData: + writer->WriteProto(response.serialized_data()); + break; + default: + LOG(FATAL) << "Unhandled message type"; + } + } +} + +bool MapBuilderStub::SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) { + if (include_unfinished_submaps) { + LOG(WARNING) << "Serializing unfinished submaps is currently unsupported. " + "Proceeding to write the state without them."; + } + proto::WriteStateToFileRequest request; + request.set_filename(filename); + ::grpc::Status status; + async_grpc::Client client( + client_channel_); + if (!client.Write(request, &status)) { + LOG(ERROR) << "WriteStateToFileRequest failed - " + << "code: " << status.error_code() + << " reason: " << status.error_message(); + } + return client.response().success(); +} + +std::map MapBuilderStub::LoadState( + io::ProtoStreamReaderInterface* reader, const bool load_frozen_state) { + async_grpc::Client client(client_channel_); + { + proto::LoadStateRequest request; + request.set_client_id(client_id_); + CHECK(client.Write(request)); + } + + io::ProtoStreamDeserializer deserializer(reader); + // Request with the SerializationHeader proto is sent first. + { + proto::LoadStateRequest request; + *request.mutable_serialization_header() = deserializer.header(); + request.set_load_frozen_state(load_frozen_state); + CHECK(client.Write(request)); + } + // Request with a PoseGraph proto is sent second. + { + proto::LoadStateRequest request; + *request.mutable_serialized_data()->mutable_pose_graph() = + deserializer.pose_graph(); + request.set_load_frozen_state(load_frozen_state); + CHECK(client.Write(request)); + } + // Request with an AllTrajectoryBuilderOptions should be third. + { + proto::LoadStateRequest request; + *request.mutable_serialized_data() + ->mutable_all_trajectory_builder_options() = + deserializer.all_trajectory_builder_options(); + request.set_load_frozen_state(load_frozen_state); + CHECK(client.Write(request)); + } + // Multiple requests with SerializedData are sent after. + proto::LoadStateRequest request; + while ( + deserializer.ReadNextSerializedData(request.mutable_serialized_data())) { + request.set_load_frozen_state(load_frozen_state); + CHECK(client.Write(request)); + } + + CHECK(reader->eof()); + CHECK(client.StreamWritesDone()); + CHECK(client.StreamFinish().ok()); + return FromProto(client.response().trajectory_remapping()); +} + +std::map MapBuilderStub::LoadStateFromFile( + const std::string& filename, const bool load_frozen_state) { + proto::LoadStateFromFileRequest request; + request.set_file_path(filename); + request.set_client_id(client_id_); + request.set_load_frozen_state(load_frozen_state); + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + return FromProto(client.response().trajectory_remapping()); +} + +int MapBuilderStub::num_trajectory_builders() const { + return trajectory_builder_stubs_.size(); +} + +mapping::PoseGraphInterface* MapBuilderStub::pose_graph() { + return pose_graph_stub_.get(); +} + +const std::vector& +MapBuilderStub::GetAllTrajectoryBuilderOptions() const { + LOG(FATAL) << "Not implemented"; +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/client/map_builder_stub.h b/carto_ws/src/cartographer-master/cartographer/cloud/client/map_builder_stub.h new file mode 100644 index 0000000..5ccfb3a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/client/map_builder_stub.h @@ -0,0 +1,75 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_CLIENT_MAP_BUILDER_STUB_H_ +#define CARTOGRAPHER_CLOUD_CLIENT_MAP_BUILDER_STUB_H_ + +#include + +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "grpc++/grpc++.h" + +namespace cartographer { +namespace cloud { + +class MapBuilderStub : public mapping::MapBuilderInterface { + public: + MapBuilderStub(const std::string& server_address, + const std::string& client_id); + + MapBuilderStub(const MapBuilderStub&) = delete; + MapBuilderStub& operator=(const MapBuilderStub&) = delete; + + int AddTrajectoryBuilder( + const std::set& expected_sensor_ids, + const mapping::proto::TrajectoryBuilderOptions& trajectory_options, + LocalSlamResultCallback local_slam_result_callback) override; + int AddTrajectoryForDeserialization( + const mapping::proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) override; + mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( + int trajectory_id) const override; + void FinishTrajectory(int trajectory_id) override; + std::string SubmapToProto( + const mapping::SubmapId& submap_id, + mapping::proto::SubmapQuery::Response* response) override; + void SerializeState(bool include_unfinished_submaps, + io::ProtoStreamWriterInterface* writer) override; + bool SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) override; + std::map LoadState(io::ProtoStreamReaderInterface* reader, + bool load_frozen_state) override; + std::map LoadStateFromFile(const std::string& filename, + bool load_frozen_state) override; + int num_trajectory_builders() const override; + mapping::PoseGraphInterface* pose_graph() override; + const std::vector& + GetAllTrajectoryBuilderOptions() const override; + + private: + std::shared_ptr<::grpc::Channel> client_channel_; + std::unique_ptr pose_graph_stub_; + std::map> + trajectory_builder_stubs_; + const std::string client_id_; +}; + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_CLIENT_MAP_BUILDER_STUB_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/pose_graph_stub.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/pose_graph_stub.cc new file mode 100644 index 0000000..23cacb2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -0,0 +1,223 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/cloud/internal/client/pose_graph_stub.h" + +#include "async_grpc/client.h" +#include "cartographer/cloud/internal/handlers/delete_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/get_all_submap_poses.h" +#include "cartographer/cloud/internal/handlers/get_constraints_handler.h" +#include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" +#include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" +#include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" +#include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" +#include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" +#include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace cloud { + +PoseGraphStub::PoseGraphStub(std::shared_ptr<::grpc::Channel> client_channel, + const std::string& client_id) + : client_channel_(client_channel), client_id_(client_id) {} + +void PoseGraphStub::RunFinalOptimization() { + google::protobuf::Empty request; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); +} + +mapping::MapById +PoseGraphStub::GetAllSubmapData() const { + LOG(FATAL) << "Not implemented"; +} + +mapping::PoseGraphInterface::SubmapData PoseGraphStub::GetSubmapData( + const mapping::SubmapId& submap_id) const { + LOG(FATAL) << "Not implemented"; +} + +mapping::MapById +PoseGraphStub::GetAllSubmapPoses() const { + google::protobuf::Empty request; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + mapping::MapById + submap_poses; + for (const auto& submap_pose : client.response().submap_poses()) { + submap_poses.Insert( + mapping::SubmapId{submap_pose.submap_id().trajectory_id(), + submap_pose.submap_id().submap_index()}, + mapping::PoseGraphInterface::SubmapPose{ + submap_pose.submap_version(), + transform::ToRigid3(submap_pose.global_pose())}); + } + return submap_poses; +} + +transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform( + int trajectory_id) const { + proto::GetLocalToGlobalTransformRequest request; + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + return transform::ToRigid3(client.response().local_to_global()); +} + +mapping::MapById +PoseGraphStub::GetTrajectoryNodes() const { + LOG(FATAL) << "Not implemented"; +} + +mapping::MapById +PoseGraphStub::GetTrajectoryNodePoses() const { + google::protobuf::Empty request; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + mapping::MapById node_poses; + for (const auto& node_pose : client.response().node_poses()) { + absl::optional + constant_pose_data; + if (node_pose.has_constant_pose_data()) { + constant_pose_data = mapping::TrajectoryNodePose::ConstantPoseData{ + common::FromUniversal(node_pose.constant_pose_data().timestamp()), + transform::ToRigid3(node_pose.constant_pose_data().local_pose())}; + } + node_poses.Insert( + mapping::NodeId{node_pose.node_id().trajectory_id(), + node_pose.node_id().node_index()}, + mapping::TrajectoryNodePose{ + transform::ToRigid3(node_pose.global_pose()), constant_pose_data}); + } + return node_poses; +} + +std::map +PoseGraphStub::GetTrajectoryStates() const { + google::protobuf::Empty request; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + std::map + trajectories_state; + for (const auto& entry : client.response().trajectories_state()) { + trajectories_state[entry.first] = FromProto(entry.second); + } + return trajectories_state; +} + +std::map PoseGraphStub::GetLandmarkPoses() + const { + google::protobuf::Empty request; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + std::map landmark_poses; + for (const auto& landmark_pose : client.response().landmark_poses()) { + landmark_poses[landmark_pose.landmark_id()] = + transform::ToRigid3(landmark_pose.global_pose()); + } + return landmark_poses; +} + +void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen) { + proto::SetLandmarkPoseRequest request; + request.mutable_landmark_pose()->set_landmark_id(landmark_id); + *request.mutable_landmark_pose()->mutable_global_pose() = + transform::ToProto(global_pose); + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); +} + +void PoseGraphStub::DeleteTrajectory(int trajectory_id) { + proto::DeleteTrajectoryRequest request; + request.set_client_id(client_id_); + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + ::grpc::Status status; + client.Write(request, &status); + if (!status.ok()) { + LOG(ERROR) << "Failed to delete trajectory " << trajectory_id + << " for client_id " << client_id_ << ": " + << status.error_message(); + } +} + +bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) const { + proto::IsTrajectoryFinishedRequest request; + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + ::grpc::Status status; + CHECK(client.Write(request, &status)) + << "Failed to check if trajectory " << trajectory_id + << " is finished: " << status.error_message(); + return client.response().is_finished(); +} + +bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) const { + proto::IsTrajectoryFrozenRequest request; + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + ::grpc::Status status; + CHECK(client.Write(request, &status)) + << "Failed to check if trajectory " << trajectory_id + << " is frozen: " << status.error_message(); + return client.response().is_frozen(); +} + +std::map +PoseGraphStub::GetTrajectoryData() const { + LOG(FATAL) << "Not implemented"; +} + +std::vector +PoseGraphStub::constraints() const { + google::protobuf::Empty request; + async_grpc::Client client(client_channel_); + ::grpc::Status status; + CHECK(client.Write(request, &status)) + << "Failed to get constraints: " << status.error_message(); + return mapping::FromProto(client.response().constraints()); +} + +mapping::proto::PoseGraph PoseGraphStub::ToProto( + bool include_unfinished_submaps) const { + LOG(FATAL) << "Not implemented"; +} + +void PoseGraphStub::SetGlobalSlamOptimizationCallback( + GlobalSlamOptimizationCallback callback) { + LOG(FATAL) << "Not implemented"; +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/pose_graph_stub.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/pose_graph_stub.h new file mode 100644 index 0000000..bcca67c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/pose_graph_stub.h @@ -0,0 +1,70 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_CLIENT_POSE_GRAPH_STUB_H_ +#define CARTOGRAPHER_CLOUD_INTERNAL_CLIENT_POSE_GRAPH_STUB_H_ + +#include "cartographer/mapping/pose_graph_interface.h" +#include "grpc++/grpc++.h" + +namespace cartographer { +namespace cloud { + +class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { + public: + PoseGraphStub(std::shared_ptr<::grpc::Channel> client_channel, + const std::string& client_id); + + PoseGraphStub(const PoseGraphStub&) = delete; + PoseGraphStub& operator=(const PoseGraphStub&) = delete; + + void RunFinalOptimization() override; + mapping::MapById GetAllSubmapData() + const override; + SubmapData GetSubmapData(const mapping::SubmapId& submap_id) const override; + mapping::MapById GetAllSubmapPoses() + const override; + transform::Rigid3d GetLocalToGlobalTransform( + int trajectory_id) const override; + mapping::MapById + GetTrajectoryNodes() const override; + mapping::MapById + GetTrajectoryNodePoses() const override; + std::map GetTrajectoryStates() const override; + std::map GetLandmarkPoses() const override; + void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen = false) override; + void DeleteTrajectory(int trajectory_id) override; + bool IsTrajectoryFinished(int trajectory_id) const override; + bool IsTrajectoryFrozen(int trajectory_id) const override; + std::map GetTrajectoryData() + const override; + std::vector constraints() const override; + mapping::proto::PoseGraph ToProto( + bool include_unfinished_submaps) const override; + void SetGlobalSlamOptimizationCallback( + GlobalSlamOptimizationCallback callback) override; + + private: + std::shared_ptr<::grpc::Channel> client_channel_; + const std::string client_id_; +}; + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_CLIENT_POSE_GRAPH_STUB_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/trajectory_builder_stub.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/trajectory_builder_stub.cc new file mode 100644 index 0000000..b12caa8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/trajectory_builder_stub.cc @@ -0,0 +1,173 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/cloud/internal/client/trajectory_builder_stub.h" + +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace cloud { + +TrajectoryBuilderStub::TrajectoryBuilderStub( + std::shared_ptr<::grpc::Channel> client_channel, const int trajectory_id, + const std::string& client_id, + LocalSlamResultCallback local_slam_result_callback) + : client_channel_(client_channel), + trajectory_id_(trajectory_id), + client_id_(client_id), + receive_local_slam_results_client_(client_channel_) { + if (local_slam_result_callback) { + proto::ReceiveLocalSlamResultsRequest request; + request.set_trajectory_id(trajectory_id); + receive_local_slam_results_client_.Write(request); + auto* receive_local_slam_results_client_ptr = + &receive_local_slam_results_client_; + receive_local_slam_results_thread_ = absl::make_unique( + [receive_local_slam_results_client_ptr, local_slam_result_callback]() { + RunLocalSlamResultsReader(receive_local_slam_results_client_ptr, + local_slam_result_callback); + }); + } +} + +TrajectoryBuilderStub::~TrajectoryBuilderStub() { + if (receive_local_slam_results_thread_) { + receive_local_slam_results_thread_->join(); + } + if (add_rangefinder_client_) { + CHECK(add_rangefinder_client_->StreamWritesDone()); + CHECK(add_rangefinder_client_->StreamFinish().ok()); + } + if (add_imu_client_) { + CHECK(add_imu_client_->StreamWritesDone()); + CHECK(add_imu_client_->StreamFinish().ok()); + } + if (add_odometry_client_) { + CHECK(add_odometry_client_->StreamWritesDone()); + CHECK(add_odometry_client_->StreamFinish().ok()); + } + if (add_landmark_client_) { + CHECK(add_landmark_client_->StreamWritesDone()); + CHECK(add_landmark_client_->StreamFinish().ok()); + } + if (add_fixed_frame_pose_client_) { + CHECK(add_fixed_frame_pose_client_->StreamWritesDone()); + CHECK(add_fixed_frame_pose_client_->StreamFinish().ok()); + } +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data) { + if (!add_rangefinder_client_) { + add_rangefinder_client_ = absl::make_unique< + async_grpc::Client>( + client_channel_); + } + proto::AddRangefinderDataRequest request; + CreateAddRangeFinderDataRequest(sensor_id, trajectory_id_, client_id_, + sensor::ToProto(timed_point_cloud_data), + &request); + add_rangefinder_client_->Write(request); +} + +void TrajectoryBuilderStub::AddSensorData(const std::string& sensor_id, + const sensor::ImuData& imu_data) { + if (!add_imu_client_) { + add_imu_client_ = + absl::make_unique>( + client_channel_); + } + proto::AddImuDataRequest request; + CreateAddImuDataRequest(sensor_id, trajectory_id_, client_id_, + sensor::ToProto(imu_data), &request); + add_imu_client_->Write(request); +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, const sensor::OdometryData& odometry_data) { + if (!add_odometry_client_) { + add_odometry_client_ = absl::make_unique< + async_grpc::Client>( + client_channel_); + } + proto::AddOdometryDataRequest request; + CreateAddOdometryDataRequest(sensor_id, trajectory_id_, client_id_, + sensor::ToProto(odometry_data), &request); + add_odometry_client_->Write(request); +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const sensor::FixedFramePoseData& fixed_frame_pose) { + if (!add_fixed_frame_pose_client_) { + add_fixed_frame_pose_client_ = absl::make_unique< + async_grpc::Client>( + client_channel_); + } + proto::AddFixedFramePoseDataRequest request; + CreateAddFixedFramePoseDataRequest(sensor_id, trajectory_id_, client_id_, + sensor::ToProto(fixed_frame_pose), + &request); + add_fixed_frame_pose_client_->Write(request); +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, const sensor::LandmarkData& landmark_data) { + if (!add_landmark_client_) { + add_landmark_client_ = absl::make_unique< + async_grpc::Client>( + client_channel_); + } + proto::AddLandmarkDataRequest request; + CreateAddLandmarkDataRequest(sensor_id, trajectory_id_, client_id_, + sensor::ToProto(landmark_data), &request); + add_landmark_client_->Write(request); +} + +void TrajectoryBuilderStub::AddLocalSlamResultData( + std::unique_ptr local_slam_result_data) { + LOG(FATAL) << "Not implemented"; +} + +void TrajectoryBuilderStub::RunLocalSlamResultsReader( + async_grpc::Client* client, + LocalSlamResultCallback local_slam_result_callback) { + proto::ReceiveLocalSlamResultsResponse response; + while (client->StreamRead(&response)) { + int trajectory_id = response.trajectory_id(); + common::Time time = common::FromUniversal(response.timestamp()); + transform::Rigid3d local_pose = transform::ToRigid3(response.local_pose()); + sensor::RangeData range_data = sensor::FromProto(response.range_data()); + auto insertion_result = + response.has_insertion_result() + ? absl::make_unique( + InsertionResult{mapping::NodeId{ + response.insertion_result().node_id().trajectory_id(), + response.insertion_result().node_id().node_index()}}) + : nullptr; + local_slam_result_callback(trajectory_id, time, local_pose, range_data, + std::move(insertion_result)); + } + client->StreamFinish(); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/trajectory_builder_stub.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/trajectory_builder_stub.h new file mode 100644 index 0000000..c57a4f3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client/trajectory_builder_stub.h @@ -0,0 +1,89 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_CLIENT_TRAJECTORY_BUILDER_STUB_H_ +#define CARTOGRAPHER_CLOUD_INTERNAL_CLIENT_TRAJECTORY_BUILDER_STUB_H_ + +#include + +#include "async_grpc/client.h" +#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" +#include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "grpc++/grpc++.h" +#include "pose_graph_stub.h" +#include "trajectory_builder_stub.h" + +namespace cartographer { +namespace cloud { + +class TrajectoryBuilderStub : public mapping::TrajectoryBuilderInterface { + public: + TrajectoryBuilderStub(std::shared_ptr<::grpc::Channel> client_channel, + const int trajectory_id, const std::string& client_id, + LocalSlamResultCallback local_slam_result_callback); + ~TrajectoryBuilderStub() override; + TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete; + TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete; + + void AddSensorData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data) override; + void AddSensorData(const std::string& sensor_id, + const sensor::ImuData& imu_data) override; + void AddSensorData(const std::string& sensor_id, + const sensor::OdometryData& odometry_data) override; + void AddSensorData( + const std::string& sensor_id, + const sensor::FixedFramePoseData& fixed_frame_pose) override; + void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) override; + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override; + + private: + static void RunLocalSlamResultsReader( + async_grpc::Client* + client_reader, + LocalSlamResultCallback local_slam_result_callback); + + std::shared_ptr<::grpc::Channel> client_channel_; + const int trajectory_id_; + const std::string client_id_; + std::unique_ptr> + add_rangefinder_client_; + std::unique_ptr> + add_imu_client_; + std::unique_ptr> + add_odometry_client_; + std::unique_ptr> + add_fixed_frame_pose_client_; + std::unique_ptr> + add_landmark_client_; + async_grpc::Client + receive_local_slam_results_client_; + std::unique_ptr receive_local_slam_results_thread_; +}; + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_CLIENT_TRAJECTORY_BUILDER_STUB_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/client_server_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client_server_test.cc new file mode 100644 index 0000000..ade3c98 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/client_server_test.cc @@ -0,0 +1,775 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer/cloud/client/map_builder_stub.h" +#include "cartographer/cloud/internal/map_builder_server.h" +#include "cartographer/cloud/map_builder_server_options.h" +#include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/io/internal/testing/test_helpers.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/internal/testing/mock_map_builder.h" +#include "cartographer/mapping/internal/testing/mock_pose_graph.h" +#include "cartographer/mapping/internal/testing/mock_trajectory_builder.h" +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "cartographer/mapping/map_builder.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using ::cartographer::io::testing::ProtoReaderFromStrings; +using ::cartographer::mapping::CreateMapBuilder; +using ::cartographer::mapping::MapBuilderInterface; +using ::cartographer::mapping::PoseGraphInterface; +using ::cartographer::mapping::TrajectoryBuilderInterface; +using ::cartographer::mapping::testing::MockMapBuilder; +using ::cartographer::mapping::testing::MockPoseGraph; +using ::cartographer::mapping::testing::MockTrajectoryBuilder; +using ::testing::_; +using SensorId = ::cartographer::mapping::TrajectoryBuilderInterface::SensorId; + +namespace cartographer { +namespace cloud { +namespace { + +constexpr char kClientId[] = "CLIENT_ID"; +const SensorId kImuSensorId{SensorId::SensorType::IMU, "imu"}; +const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"}; +constexpr double kDuration = 4.; // Seconds. +constexpr double kTimeStep = 0.1; // Seconds. +constexpr double kTravelDistance = 1.2; // Meters. + +constexpr char kSerializationHeaderProtoString[] = "format_version: 2"; +constexpr char kPoseGraphProtoString[] = R"(pose_graph { + trajectory: { + trajectory_id: 0 + node: {} + submap: {} + } + })"; +constexpr char kAllTrajectoryBuilderOptionsProtoString[] = + R"(all_trajectory_builder_options { + options_with_sensor_ids: {} + })"; +constexpr char kSubmapProtoString[] = "submap {}"; +constexpr char kNodeProtoString[] = "node {}"; +constexpr char kTrajectoryDataProtoString[] = "trajectory_data {}"; +constexpr char kImuDataProtoString[] = "imu_data {}"; +constexpr char kOdometryDataProtoString[] = "odometry_data {}"; +constexpr char kFixedFramePoseDataProtoString[] = "fixed_frame_pose_data {}"; +constexpr char kLandmarkDataProtoString[] = "landmark_data {}"; + +template +class ClientServerTestBase : public T { + protected: + void SetUp() override { + // TODO(cschuet): Due to the hard-coded addresses these tests will become + // flaky when run in parallel. + const std::string kMapBuilderServerLua = R"text( + include "map_builder_server.lua" + MAP_BUILDER.use_trajectory_builder_2d = true + MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0 + MAP_BUILDER_SERVER.num_event_threads = 1 + MAP_BUILDER_SERVER.num_grpc_threads = 1 + MAP_BUILDER_SERVER.uplink_server_address = "" + MAP_BUILDER_SERVER.server_address = "0.0.0.0:50051" + return MAP_BUILDER_SERVER)text"; + auto map_builder_server_parameters = + mapping::testing::ResolveLuaParameters(kMapBuilderServerLua); + map_builder_server_options_ = + CreateMapBuilderServerOptions(map_builder_server_parameters.get()); + + const std::string kUploadingMapBuilderServerLua = R"text( + include "map_builder_server.lua" + MAP_BUILDER.use_trajectory_builder_2d = true + MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0 + MAP_BUILDER_SERVER.num_event_threads = 1 + MAP_BUILDER_SERVER.num_grpc_threads = 1 + MAP_BUILDER_SERVER.uplink_server_address = "localhost:50051" + MAP_BUILDER_SERVER.server_address = "0.0.0.0:50052" + MAP_BUILDER_SERVER.upload_batch_size = 1 + return MAP_BUILDER_SERVER)text"; + auto uploading_map_builder_server_parameters = + mapping::testing::ResolveLuaParameters(kUploadingMapBuilderServerLua); + uploading_map_builder_server_options_ = CreateMapBuilderServerOptions( + uploading_map_builder_server_parameters.get()); + EXPECT_NE(map_builder_server_options_.server_address(), + uploading_map_builder_server_options_.server_address()); + + const std::string kTrajectoryBuilderLua = R"text( + include "trajectory_builder.lua" + TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false + TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 4 + return TRAJECTORY_BUILDER)text"; + auto trajectory_builder_parameters = + mapping::testing::ResolveLuaParameters(kTrajectoryBuilderLua); + trajectory_builder_options_ = mapping::CreateTrajectoryBuilderOptions( + trajectory_builder_parameters.get()); + number_of_insertion_results_ = 0; + local_slam_result_callback_ = + [this](int, common::Time, transform::Rigid3d local_pose, + sensor::RangeData, + std::unique_ptr< + const mapping::TrajectoryBuilderInterface::InsertionResult> + insertion_result) { + std::unique_lock lock(local_slam_result_mutex_); + if (insertion_result) { + ++number_of_insertion_results_; + } + local_slam_result_poses_.push_back(local_pose); + lock.unlock(); + local_slam_result_condition_.notify_all(); + }; + } + + void InitializeRealServer() { + auto map_builder = + CreateMapBuilder(map_builder_server_options_.map_builder_options()); + server_ = absl::make_unique(map_builder_server_options_, + std::move(map_builder)); + EXPECT_TRUE(server_ != nullptr); + } + + void InitializeRealUploadingServer() { + auto map_builder = CreateMapBuilder( + uploading_map_builder_server_options_.map_builder_options()); + uploading_server_ = absl::make_unique( + uploading_map_builder_server_options_, std::move(map_builder)); + EXPECT_TRUE(uploading_server_ != nullptr); + } + + void InitializeServerWithMockMapBuilder() { + auto mock_map_builder = absl::make_unique(); + mock_map_builder_ = mock_map_builder.get(); + mock_pose_graph_ = absl::make_unique(); + EXPECT_CALL(*mock_map_builder_, pose_graph()) + .WillOnce(::testing::Return(mock_pose_graph_.get())); + EXPECT_CALL(*mock_pose_graph_, SetGlobalSlamOptimizationCallback(_)); + server_ = absl::make_unique(map_builder_server_options_, + std::move(mock_map_builder)); + EXPECT_TRUE(server_ != nullptr); + mock_trajectory_builder_ = absl::make_unique(); + } + + void InitializeStub() { + stub_ = absl::make_unique( + map_builder_server_options_.server_address(), kClientId); + EXPECT_TRUE(stub_ != nullptr); + } + + void InitializeStubForUploadingServer() { + stub_for_uploading_server_ = absl::make_unique( + uploading_map_builder_server_options_.server_address(), kClientId); + EXPECT_TRUE(stub_for_uploading_server_ != nullptr); + } + + void SetOptionsToTSDF2D() { + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_range_data_inserter_options() + ->set_range_data_inserter_type( + ::cartographer::mapping::proto::RangeDataInserterOptions:: + TSDF_INSERTER_2D); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_grid_options_2d() + ->set_grid_type(::cartographer::mapping::proto::GridOptions2D::TSDF); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(10.0); + map_builder_server_options_.mutable_map_builder_options() + ->mutable_pose_graph_options() + ->mutable_constraint_builder_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(50.0); + uploading_map_builder_server_options_.mutable_map_builder_options() + ->mutable_pose_graph_options() + ->mutable_constraint_builder_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(50.0); + } + + void WaitForLocalSlamResults(size_t size) { + std::unique_lock lock(local_slam_result_mutex_); + local_slam_result_condition_.wait( + lock, [&] { return local_slam_result_poses_.size() >= size; }); + } + + void WaitForLocalSlamResultUploads(size_t size) { + while (stub_->pose_graph()->GetTrajectoryNodePoses().size() < size) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + + proto::MapBuilderServerOptions map_builder_server_options_; + proto::MapBuilderServerOptions uploading_map_builder_server_options_; + MockMapBuilder* mock_map_builder_; + std::unique_ptr mock_pose_graph_; + std::unique_ptr mock_trajectory_builder_; + ::cartographer::mapping::proto::TrajectoryBuilderOptions + trajectory_builder_options_; + std::unique_ptr server_; + std::unique_ptr uploading_server_; + std::unique_ptr stub_; + std::unique_ptr stub_for_uploading_server_; + TrajectoryBuilderInterface::LocalSlamResultCallback + local_slam_result_callback_; + std::condition_variable local_slam_result_condition_; + std::condition_variable local_slam_result_upload_condition_; + std::mutex local_slam_result_mutex_; + std::mutex local_slam_result_upload_mutex_; + std::vector local_slam_result_poses_; + int number_of_insertion_results_; +}; + +class ClientServerTest : public ClientServerTestBase<::testing::Test> {}; +class ClientServerTestByGridType + : public ClientServerTestBase< + ::testing::TestWithParam<::cartographer::mapping::GridType>> {}; + +INSTANTIATE_TEST_CASE_P( + ClientServerTestByGridType, ClientServerTestByGridType, + ::testing::Values(::cartographer::mapping::GridType::PROBABILITY_GRID, + ::cartographer::mapping::GridType::TSDF)); + +TEST_F(ClientServerTest, StartAndStopServer) { + InitializeRealServer(); + server_->Start(); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, AddTrajectoryBuilder) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = stub_->AddTrajectoryBuilder( + {kImuSensorId}, trajectory_builder_options_, nullptr); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id)); + stub_->FinishTrajectory(trajectory_id); + EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id)); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(trajectory_id)); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithMock) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeServerWithMockMapBuilder(); + server_->Start(); + InitializeStub(); + std::set expected_sensor_ids = {kImuSensorId}; + EXPECT_CALL( + *mock_map_builder_, + AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _)) + .WillOnce(::testing::Return(3)); + EXPECT_CALL(*mock_map_builder_, GetTrajectoryBuilder(_)) + .WillRepeatedly(::testing::Return(nullptr)); + int trajectory_id = stub_->AddTrajectoryBuilder( + expected_sensor_ids, trajectory_builder_options_, nullptr); + EXPECT_EQ(trajectory_id, 3); + EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id)); + stub_->FinishTrajectory(trajectory_id); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, AddSensorData) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->set_use_imu_data(true); + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = stub_->AddTrajectoryBuilder( + {kImuSensorId}, trajectory_builder_options_, nullptr); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + sensor::ImuData imu_data{common::FromUniversal(42), + Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}; + trajectory_stub->AddSensorData(kImuSensorId.id, imu_data); + stub_->FinishTrajectory(trajectory_id); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, AddSensorDataWithMock) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeServerWithMockMapBuilder(); + server_->Start(); + InitializeStub(); + std::set expected_sensor_ids = {kImuSensorId}; + EXPECT_CALL( + *mock_map_builder_, + AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _)) + .WillOnce(::testing::Return(3)); + int trajectory_id = stub_->AddTrajectoryBuilder( + expected_sensor_ids, trajectory_builder_options_, nullptr); + EXPECT_EQ(trajectory_id, 3); + EXPECT_CALL(*mock_map_builder_, GetTrajectoryBuilder(_)) + .WillRepeatedly(::testing::Return(mock_trajectory_builder_.get())); + mapping::TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + sensor::ImuData imu_data{common::FromUniversal(42), + Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}; + EXPECT_CALL(*mock_trajectory_builder_, + AddSensorData(::testing::StrEq(kImuSensorId.id), + ::testing::Matcher(_))) + .WillOnce(::testing::Return()); + trajectory_stub->AddSensorData(kImuSensorId.id, imu_data); + EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id)); + stub_->FinishTrajectory(trajectory_id); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, LocalSlam2D) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + EXPECT_TRUE(stub_->pose_graph()->GetTrajectoryStates().empty()); + int trajectory_id = + stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); + } + WaitForLocalSlamResults(measurements.size()); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); + stub_->FinishTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::FINISHED); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, LocalSlamAndDelete2D) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = + stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); + } + WaitForLocalSlamResults(measurements.size()); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); + EXPECT_GT(stub_->pose_graph()->GetAllSubmapPoses().size(), 0); + EXPECT_GT(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0); + stub_->FinishTrajectory(trajectory_id); + stub_->pose_graph()->DeleteTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::DELETED); + EXPECT_EQ(stub_->pose_graph()->GetAllSubmapPoses().size(), 0); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0); + server_->Shutdown(); +} + +TEST_F(ClientServerTest, GlobalSlam3D) { + map_builder_server_options_.mutable_map_builder_options() + ->set_use_trajectory_builder_2d(false); + map_builder_server_options_.mutable_map_builder_options() + ->set_use_trajectory_builder_3d(true); + map_builder_server_options_.mutable_map_builder_options() + ->mutable_pose_graph_options() + ->set_optimize_every_n_nodes(3); + trajectory_builder_options_.mutable_trajectory_builder_3d_options() + ->mutable_motion_filter_options() + ->set_max_distance_meters(0); + trajectory_builder_options_.mutable_trajectory_builder_3d_options() + ->mutable_motion_filter_options() + ->set_max_angle_radians(0); + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = stub_->AddTrajectoryBuilder( + {kRangeSensorId, kImuSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + sensor::ImuData imu_data{ + measurement.time - common::FromSeconds(kTimeStep / 2), + Eigen::Vector3d(0., 0., 9.8), Eigen::Vector3d::Zero()}; + trajectory_stub->AddSensorData(kImuSensorId.id, imu_data); + trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); + } + // First range data will not call back while initializing PoseExtrapolator, so + // expect one less. + WaitForLocalSlamResults(measurements.size() - 1); + stub_->FinishTrajectory(trajectory_id); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, StartAndStopUploadingServerAndServer) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeRealUploadingServer(); + uploading_server_->Start(); + uploading_server_->Shutdown(); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithUploadingServer) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeRealUploadingServer(); + uploading_server_->Start(); + InitializeStub(); + InitializeStubForUploadingServer(); + int trajectory_id = stub_for_uploading_server_->AddTrajectoryBuilder( + {kImuSensorId}, trajectory_builder_options_, nullptr); + EXPECT_FALSE(stub_for_uploading_server_->pose_graph()->IsTrajectoryFinished( + trajectory_id)); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id)); + stub_for_uploading_server_->FinishTrajectory(trajectory_id); + EXPECT_TRUE(stub_for_uploading_server_->pose_graph()->IsTrajectoryFinished( + trajectory_id)); + EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id)); + EXPECT_FALSE(stub_for_uploading_server_->pose_graph()->IsTrajectoryFrozen( + trajectory_id)); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(trajectory_id)); + uploading_server_->Shutdown(); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, LocalSlam2DWithUploadingServer) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + InitializeRealUploadingServer(); + uploading_server_->Start(); + InitializeStubForUploadingServer(); + int trajectory_id = stub_for_uploading_server_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_for_uploading_server_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); + } + WaitForLocalSlamResults(measurements.size()); + WaitForLocalSlamResultUploads(number_of_insertion_results_); + + std::queue> chunks; + io::ForwardingProtoStreamWriter writer( + [&chunks](const google::protobuf::Message* proto) -> bool { + if (!proto) { + return true; + } + std::unique_ptr p(proto->New()); + p->CopyFrom(*proto); + chunks.push(std::move(p)); + return true; + }); + stub_->SerializeState(false, &writer); + CHECK(writer.Close()); + // Ensure it can be read. + io::InMemoryProtoStreamReader reader(std::move(chunks)); + io::ProtoStreamDeserializer deserializer(&reader); + EXPECT_EQ(deserializer.pose_graph().trajectory_size(), 1); + + stub_for_uploading_server_->FinishTrajectory(trajectory_id); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + uploading_server_->Shutdown(); + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, LocalSlam2DUplinkServerRestarting) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + InitializeRealUploadingServer(); + uploading_server_->Start(); + InitializeStubForUploadingServer(); + int trajectory_id = stub_for_uploading_server_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_for_uploading_server_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + + // Insert half of the measurements. + for (unsigned int i = 0; i < measurements.size() / 2; ++i) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurements.at(i)); + } + WaitForLocalSlamResults(measurements.size() / 2); + WaitForLocalSlamResultUploads(number_of_insertion_results_); + + // Simulate a cloud server restart. + LOG(INFO) << "Simulating server restart."; + constexpr int kUplinkTrajectoryId = 0; + stub_->FinishTrajectory(kUplinkTrajectoryId); + server_->Shutdown(); + server_->WaitForShutdown(); + InitializeRealServer(); + server_->Start(); + InitializeStub(); + + // Insert the second half of the measurements. + for (unsigned int i = measurements.size() / 2; i < measurements.size(); ++i) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurements.at(i)); + } + + WaitForLocalSlamResults(measurements.size() / 2); + WaitForLocalSlamResultUploads(2); + stub_for_uploading_server_->FinishTrajectory(trajectory_id); + uploading_server_->Shutdown(); + uploading_server_->WaitForShutdown(); + server_->Shutdown(); + server_->WaitForShutdown(); +} + +TEST_F(ClientServerTest, DelayedConnectionToUplinkServer) { + InitializeRealUploadingServer(); + uploading_server_->Start(); + InitializeStubForUploadingServer(); + int trajectory_id = stub_for_uploading_server_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_for_uploading_server_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + + // Insert the first measurement. + trajectory_stub->AddSensorData(kRangeSensorId.id, measurements.at(0)); + WaitForLocalSlamResults(1); + + LOG(INFO) << "Delayed start of uplink server."; + InitializeRealServer(); + server_->Start(); + InitializeStub(); + + // Insert all other measurements. + for (unsigned int i = 1; i < measurements.size(); ++i) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurements.at(i)); + } + WaitForLocalSlamResults(measurements.size()); + WaitForLocalSlamResultUploads(2); + stub_for_uploading_server_->FinishTrajectory(trajectory_id); + uploading_server_->Shutdown(); + uploading_server_->WaitForShutdown(); + server_->Shutdown(); + server_->WaitForShutdown(); +} + +TEST_P(ClientServerTestByGridType, LoadStateAndDelete) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + + // Load text proto into in_memory_reader. + auto reader = + ProtoReaderFromStrings(kSerializationHeaderProtoString, + { + kPoseGraphProtoString, + kAllTrajectoryBuilderOptionsProtoString, + kSubmapProtoString, + kNodeProtoString, + kImuDataProtoString, + kOdometryDataProtoString, + kLandmarkDataProtoString, + }); + + auto trajectory_remapping = stub_->LoadState(reader.get(), true); + int expected_trajectory_id = 0; + EXPECT_EQ(trajectory_remapping.size(), 1); + EXPECT_EQ(trajectory_remapping.at(0), expected_trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFrozen(expected_trajectory_id)); + EXPECT_FALSE( + stub_->pose_graph()->IsTrajectoryFinished(expected_trajectory_id)); + for (const auto& entry : trajectory_remapping) { + int trajectory_id = entry.second; + stub_->pose_graph()->DeleteTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::DELETED); + } + server_->Shutdown(); +} + +TEST_P(ClientServerTestByGridType, LoadUnfrozenStateAndDelete) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + + // Load text proto into in_memory_reader. + auto reader = + ProtoReaderFromStrings(kSerializationHeaderProtoString, + { + kPoseGraphProtoString, + kAllTrajectoryBuilderOptionsProtoString, + kSubmapProtoString, + kNodeProtoString, + kImuDataProtoString, + kOdometryDataProtoString, + kLandmarkDataProtoString, + }); + + auto trajectory_remapping = + stub_->LoadState(reader.get(), false /* load_frozen_state */); + int expected_trajectory_id = 0; + EXPECT_EQ(trajectory_remapping.size(), 1); + EXPECT_EQ(trajectory_remapping.at(0), expected_trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(expected_trajectory_id)); + EXPECT_FALSE( + stub_->pose_graph()->IsTrajectoryFinished(expected_trajectory_id)); + EXPECT_EQ( + stub_->pose_graph()->GetTrajectoryStates().at(expected_trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); + stub_->FinishTrajectory(expected_trajectory_id); + EXPECT_EQ( + stub_->pose_graph()->GetTrajectoryStates().at(expected_trajectory_id), + PoseGraphInterface::TrajectoryState::FINISHED); + for (const auto& entry : trajectory_remapping) { + int trajectory_id = entry.second; + stub_->pose_graph()->DeleteTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::DELETED); + } + server_->Shutdown(); +} + +// TODO(gaschler): Test-cover LoadStateFromFile. + +TEST_P(ClientServerTestByGridType, LocalSlam2DHandlesInvalidRequests) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = + stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = mapping::testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); + } + WaitForLocalSlamResults(measurements.size()); + stub_->pose_graph()->RunFinalOptimization(); + + const int kInvalidTrajectoryId = 7; + stub_->pose_graph()->DeleteTrajectory(kInvalidTrajectoryId); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(kInvalidTrajectoryId)); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(kInvalidTrajectoryId)); + EXPECT_EQ(nullptr, stub_->GetTrajectoryBuilder(kInvalidTrajectoryId)); + stub_->FinishTrajectory(kInvalidTrajectoryId); + const mapping::SubmapId kInvalidSubmapId0{kInvalidTrajectoryId, 0}, + kInvalidSubmapId1{trajectory_id, 424242}; + mapping::proto::SubmapQuery::Response submap_query_response; + // Expect that it returns non-empty error string. + EXPECT_NE("", + stub_->SubmapToProto(kInvalidSubmapId0, &submap_query_response)); + EXPECT_NE("", + stub_->SubmapToProto(kInvalidSubmapId1, &submap_query_response)); + + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); + auto submap_poses = stub_->pose_graph()->GetAllSubmapPoses(); + EXPECT_GT(submap_poses.size(), 0); + EXPECT_GT(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0); + stub_->FinishTrajectory(trajectory_id); + stub_->pose_graph()->DeleteTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + mapping::SubmapId deleted_submap_id = submap_poses.begin()->id; + EXPECT_NE("", + stub_->SubmapToProto(deleted_submap_id, &submap_query_response)); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::DELETED); + // Make sure optimization runs with a deleted trajectory. + stub_->pose_graph()->RunFinalOptimization(); + server_->Shutdown(); +} + +} // namespace +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.cc new file mode 100644 index 0000000..b705300 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.cc @@ -0,0 +1,60 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void AddFixedFramePoseDataHandler::OnSensorData( + const proto::AddFixedFramePoseDataRequest& request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + sensor::FromProto(request.fixed_frame_pose_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto sensor_data = absl::make_unique(); + *sensor_data->mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data->mutable_fixed_frame_pose_data() = + request.fixed_frame_pose_data(); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueSensorData(std::move(sensor_data)); + } +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h new file mode 100644 index 0000000..86d8a18 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h @@ -0,0 +1,46 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddFixedFramePoseDataSignature, + async_grpc::Stream, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/AddFixedFramePoseData") + +class AddFixedFramePoseDataHandler + : public AddSensorDataHandlerBase { + public: + void OnSensorData( + const proto::AddFixedFramePoseDataRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc new file mode 100644 index 0000000..33c0dbf --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc @@ -0,0 +1,103 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"( + sensor_metadata { + trajectory_id: 1 + sensor_id: "sensor_id" + } + fixed_frame_pose_data { + timestamp: 2 + pose { + translation { + x: 3 y: 4 z: 5 + } + rotation { + w: 6 x: 7 y: 8 z: 9 + } + } + })"; + +using AddFixedFramePoseDataHandlerTest = + testing::HandlerTest; + +TEST_F(AddFixedFramePoseDataHandlerTest, NoLocalSlamUploader) { + proto::AddFixedFramePoseDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetNoLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +TEST_F(AddFixedFramePoseDataHandlerTest, WithMockLocalSlamUploader) { + proto::AddFixedFramePoseDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetMockLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + proto::SensorData sensor_data; + *sensor_data.mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data.mutable_fixed_frame_pose_data() = + request.fixed_frame_pose_data(); + EXPECT_CALL(*mock_local_trajectory_uploader_, + DoEnqueueSensorData(Pointee( + Truly(testing::BuildProtoPredicateEquals(&sensor_data))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler.cc new file mode 100644 index 0000000..99e51d1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler.cc @@ -0,0 +1,57 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void AddImuDataHandler::OnSensorData(const proto::AddImuDataRequest& request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable(request.sensor_metadata().sensor_id(), + sensor::FromProto(request.imu_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto sensor_data = absl::make_unique(); + *sensor_data->mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data->mutable_imu_data() = request.imu_data(); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueSensorData(std::move(sensor_data)); + } +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler.h new file mode 100644 index 0000000..6ad996e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_ADD_IMU_DATA_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_IMU_DATA_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddImuDataSignature, async_grpc::Stream, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/AddImuData") + +class AddImuDataHandler : public AddSensorDataHandlerBase { + public: + void OnSensorData(const proto::AddImuDataRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_IMU_DATA_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc new file mode 100644 index 0000000..4052a86 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc @@ -0,0 +1,99 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"( + sensor_metadata { + trajectory_id: 1 + sensor_id: "sensor_id" + } + imu_data { + timestamp: 2 + linear_acceleration { + x: 3 y: 4 z: 5 + } + angular_velocity { + x: 6 y: 7 z: 8 + } + })"; + +using AddImuDataHandlerTest = + testing::HandlerTest; + +TEST_F(AddImuDataHandlerTest, NoLocalSlamUploader) { + proto::AddImuDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetNoLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +TEST_F(AddImuDataHandlerTest, WithMockLocalSlamUploader) { + proto::AddImuDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetMockLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + proto::SensorData sensor_data; + *sensor_data.mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data.mutable_imu_data() = request.imu_data(); + EXPECT_CALL(*mock_local_trajectory_uploader_, + DoEnqueueSensorData(Pointee( + Truly(testing::BuildProtoPredicateEquals(&sensor_data))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler.cc new file mode 100644 index 0000000..a820002 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler.cc @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "cartographer/sensor/landmark_data.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void AddLandmarkDataHandler::OnSensorData( + const proto::AddLandmarkDataRequest& request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable(request.sensor_metadata().sensor_id(), + sensor::FromProto(request.landmark_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto sensor_data = absl::make_unique(); + *sensor_data->mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data->mutable_landmark_data() = request.landmark_data(); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueSensorData(std::move(sensor_data)); + } +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler.h new file mode 100644 index 0000000..2f71465 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler.h @@ -0,0 +1,44 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddLandmarkDataSignature, async_grpc::Stream, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/AddLandmarkData") + +class AddLandmarkDataHandler + : public AddSensorDataHandlerBase { + public: + void OnSensorData(const proto::AddLandmarkDataRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc new file mode 100644 index 0000000..5f772cd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc @@ -0,0 +1,106 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"( + sensor_metadata { + trajectory_id: 1 + sensor_id: "sensor_id" + } + landmark_data { + timestamp: 2 + landmark_observations { + id: "3" + landmark_to_tracking_transform { + translation { + x: 4 y: 5 z: 6 + } + rotation { + w:7 x: 8 y: 9 z: 10 + } + } + translation_weight: 11.0 + rotation_weight: 12.0 + } + })"; + +using AddLandmarkDataHandlerTest = + testing::HandlerTest; + +TEST_F(AddLandmarkDataHandlerTest, NoLocalSlamUploader) { + proto::AddLandmarkDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetNoLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +TEST_F(AddLandmarkDataHandlerTest, WithMockLocalSlamUploader) { + proto::AddLandmarkDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetMockLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + proto::SensorData sensor_data; + *sensor_data.mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data.mutable_landmark_data() = request.landmark_data(); + EXPECT_CALL(*mock_local_trajectory_uploader_, + DoEnqueueSensorData(Pointee( + Truly(testing::BuildProtoPredicateEquals(&sensor_data))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler.cc new file mode 100644 index 0000000..0cc1670 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler.cc @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "cartographer/sensor/odometry_data.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void AddOdometryDataHandler::OnSensorData( + const proto::AddOdometryDataRequest& request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable(request.sensor_metadata().sensor_id(), + sensor::FromProto(request.odometry_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto sensor_data = absl::make_unique(); + *sensor_data->mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data->mutable_odometry_data() = request.odometry_data(); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueSensorData(std::move(sensor_data)); + } +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler.h new file mode 100644 index 0000000..5c4eb38 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler.h @@ -0,0 +1,44 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddOdometryDataSignature, async_grpc::Stream, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/AddOdometryData") + +class AddOdometryDataHandler + : public AddSensorDataHandlerBase { + public: + void OnSensorData(const proto::AddOdometryDataRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc new file mode 100644 index 0000000..e0571a8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc @@ -0,0 +1,101 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"( + sensor_metadata { + trajectory_id: 1 + sensor_id: "sensor_id" + } + odometry_data { + timestamp: 2 + pose { + translation { + x: 3 y: 4 z: 5 + } + rotation { + w: 6 x: 7 y: 8 z: 9 + } + } + })"; + +using AddOdometryDataHandlerTest = + testing::HandlerTest; + +TEST_F(AddOdometryDataHandlerTest, NoLocalSlamUploader) { + proto::AddOdometryDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetNoLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +TEST_F(AddOdometryDataHandlerTest, WithMockLocalSlamUploader) { + proto::AddOdometryDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + SetMockLocalTrajectoryUploader(); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + proto::SensorData sensor_data; + *sensor_data.mutable_sensor_metadata() = request.sensor_metadata(); + *sensor_data.mutable_odometry_data() = request.odometry_data(); + EXPECT_CALL(*mock_local_trajectory_uploader_, + DoEnqueueSensorData(Pointee( + Truly(testing::BuildProtoPredicateEquals(&sensor_data))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler.cc new file mode 100644 index 0000000..4ef584b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler.cc @@ -0,0 +1,45 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void AddRangefinderDataHandler::OnSensorData( + const proto::AddRangefinderDataRequest& request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + sensor::FromProto(request.timed_point_cloud_data()))); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h new file mode 100644 index 0000000..411893b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h @@ -0,0 +1,45 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddRangefinderDataSignature, + async_grpc::Stream, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/AddRangefinderData") + +class AddRangefinderDataHandler + : public AddSensorDataHandlerBase { + public: + void OnSensorData(const proto::AddRangefinderDataRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc new file mode 100644 index 0000000..63b0642 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc @@ -0,0 +1,77 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"( + sensor_metadata { + trajectory_id: 1 + sensor_id: "sensor_id" + } + timed_point_cloud_data { + timestamp: 2 + origin { + x: 3.f y: 4.f z: 5.f + } + point_data { + position { + x: 6.f y: 7.f z: 8.f + } + time: 9.f + } + })"; + +using AddRangefinderDataHandlerTest = + testing::HandlerTest; + +TEST_F(AddRangefinderDataHandlerTest, NoLocalSlamUploader) { + proto::AddRangefinderDataRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + EXPECT_CALL( + *mock_map_builder_context_, + CheckClientIdForTrajectory(Eq(request.sensor_metadata().client_id()), + Eq(request.sensor_metadata().trajectory_id()))) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*mock_map_builder_context_, + DoEnqueueSensorData( + Eq(request.sensor_metadata().trajectory_id()), + Pointee(Truly(testing::BuildDataPredicateEquals(request))))); + test_server_->SendWrite(request); + test_server_->SendWritesDone(); + test_server_->SendFinish(); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc new file mode 100644 index 0000000..c277428 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc @@ -0,0 +1,162 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +metrics::Family* + AddSensorDataBatchHandler::counter_metrics_family_ = + metrics::Family::Null(); + +void AddSensorDataBatchHandler::OnRequest( + const proto::AddSensorDataBatchRequest& request) { + for (const proto::SensorData& sensor_data : request.sensor_data()) { + if (!GetContext()->CheckClientIdForTrajectory( + sensor_data.sensor_metadata().client_id(), + sensor_data.sensor_metadata().trajectory_id())) { + LOG(ERROR) << "Unknown trajectory with ID " + << sensor_data.sensor_metadata().trajectory_id() + << " and client_id " + << sensor_data.sensor_metadata().client_id(); + Finish(::grpc::Status(::grpc::NOT_FOUND, "Unknown trajectory")); + return; + } + ClientMetrics* const metrics = + GetOrCreateClientMetrics(sensor_data.sensor_metadata().client_id(), + sensor_data.sensor_metadata().trajectory_id()); + switch (sensor_data.sensor_data_case()) { + case proto::SensorData::kOdometryData: + GetUnsynchronizedContext() + ->EnqueueSensorData( + sensor_data.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + sensor_data.sensor_metadata().sensor_id(), + sensor::FromProto(sensor_data.odometry_data()))); + metrics->odometry_sensor_counter->Increment(); + break; + case proto::SensorData::kImuData: + GetUnsynchronizedContext() + ->EnqueueSensorData(sensor_data.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + sensor_data.sensor_metadata().sensor_id(), + sensor::FromProto(sensor_data.imu_data()))); + metrics->imu_sensor_counter->Increment(); + break; + case proto::SensorData::kTimedPointCloudData: + GetUnsynchronizedContext() + ->EnqueueSensorData( + sensor_data.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + sensor_data.sensor_metadata().sensor_id(), + sensor::FromProto(sensor_data.timed_point_cloud_data()))); + metrics->timed_point_cloud_counter->Increment(); + break; + case proto::SensorData::kFixedFramePoseData: + GetUnsynchronizedContext() + ->EnqueueSensorData( + sensor_data.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + sensor_data.sensor_metadata().sensor_id(), + sensor::FromProto(sensor_data.fixed_frame_pose_data()))); + metrics->fixed_frame_pose_counter->Increment(); + break; + case proto::SensorData::kLandmarkData: + GetUnsynchronizedContext() + ->EnqueueSensorData( + sensor_data.sensor_metadata().trajectory_id(), + sensor::MakeDispatchable( + sensor_data.sensor_metadata().sensor_id(), + sensor::FromProto(sensor_data.landmark_data()))); + metrics->landmark_counter->Increment(); + break; + case proto::SensorData::kLocalSlamResultData: + GetContext()->EnqueueLocalSlamResultData( + sensor_data.sensor_metadata().trajectory_id(), + sensor_data.sensor_metadata().sensor_id(), + sensor_data.local_slam_result_data()); + metrics->local_slam_result_counter->Increment(); + break; + default: + LOG(FATAL) << "Unknown sensor data type: " + << sensor_data.sensor_data_case(); + } + } + Send(absl::make_unique()); +} + +void AddSensorDataBatchHandler::RegisterMetrics( + metrics::FamilyFactory* family_factory) { + counter_metrics_family_ = family_factory->NewCounterFamily( + "cartographer_sensor_data_total", "Sensor data received"); +} + +AddSensorDataBatchHandler::ClientMetrics* +AddSensorDataBatchHandler::GetOrCreateClientMetrics( + const std::string& client_id, int trajectory_id) { + const std::string map_key = client_id + std::to_string(trajectory_id); + auto client_metric_map_itr = client_metric_map_.find(map_key); + if (client_metric_map_itr != client_metric_map_.end()) { + return client_metric_map_itr->second.get(); + } + + auto new_metrics = absl::make_unique(); + new_metrics->odometry_sensor_counter = counter_metrics_family_->Add( + {{"client_id", client_id}, + {"trajectory_id", std::to_string(trajectory_id)}, + {"sensor", "odometry"}}); + new_metrics->imu_sensor_counter = counter_metrics_family_->Add( + {{"client_id", client_id}, + {"trajectory_id", std::to_string(trajectory_id)}, + {"sensor", "imu"}}); + new_metrics->fixed_frame_pose_counter = counter_metrics_family_->Add( + {{"client_id", client_id}, + {"trajectory_id", std::to_string(trajectory_id)}, + {"sensor", "fixed_frame_pose"}}); + new_metrics->landmark_counter = counter_metrics_family_->Add( + {{"client_id", client_id}, + {"trajectory_id", std::to_string(trajectory_id)}, + {"sensor", "landmark"}}); + new_metrics->local_slam_result_counter = counter_metrics_family_->Add( + {{"client_id", client_id}, + {"trajectory_id", std::to_string(trajectory_id)}, + {"sensor", "local_slam_result"}}); + new_metrics->timed_point_cloud_counter = counter_metrics_family_->Add( + {{"client_id", client_id}, + {"trajectory_id", std::to_string(trajectory_id)}, + {"sensor", "timed_point_cloud"}}); + + // Obtain pointer before ownership is transferred. + auto* new_metrics_ptr = new_metrics.get(); + client_metric_map_[map_key] = std::move(new_metrics); + return new_metrics_ptr; +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h new file mode 100644 index 0000000..2173465 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h @@ -0,0 +1,71 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H + +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/family_factory.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddSensorDataBatchSignature, proto::AddSensorDataBatchRequest, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/AddSensorDataBatch") + +class AddSensorDataBatchHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::AddSensorDataBatchRequest& request) override; + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + struct ClientMetrics { + metrics::Counter* odometry_sensor_counter; + metrics::Counter* imu_sensor_counter; + metrics::Counter* timed_point_cloud_counter; + metrics::Counter* fixed_frame_pose_counter; + metrics::Counter* landmark_counter; + metrics::Counter* local_slam_result_counter; + }; + + ClientMetrics* GetOrCreateClientMetrics(const std::string& client_id, + int trajectory_id); + + static cartographer::metrics::Family* + counter_metrics_family_; + + // Holds individual metrics for each client. + absl::flat_hash_map> + client_metric_map_; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h new file mode 100644 index 0000000..2c0afba --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_sensor_data_handler_base.h @@ -0,0 +1,61 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_HANDLER_BASE_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_HANDLER_BASE_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +template +class AddSensorDataHandlerBase + : public async_grpc::RpcHandler { + public: + using SensorDataType = + async_grpc::StripStream; + + void OnRequest(const SensorDataType& request) override { + if (!this->template GetContext< + cartographer::cloud::MapBuilderContextInterface>() + ->CheckClientIdForTrajectory( + request.sensor_metadata().client_id(), + request.sensor_metadata().trajectory_id())) { + LOG(ERROR) << "Unknown trajectory with ID " + << request.sensor_metadata().trajectory_id() + << " and client_id " << request.sensor_metadata().client_id(); + this->template Finish( + ::grpc::Status(::grpc::NOT_FOUND, "Unknown trajectory")); + return; + } + OnSensorData(request); + } + + virtual void OnSensorData(const SensorDataType& request) = 0; + + void OnReadsDone() override { + this->template Send(absl::make_unique()); + } +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_HANDLER_BASE_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler.cc new file mode 100644 index 0000000..d7c2e8a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler.cc @@ -0,0 +1,81 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void AddTrajectoryHandler::OnRequest( + const proto::AddTrajectoryRequest& request) { + auto local_slam_result_callback = + GetUnsynchronizedContext() + ->GetLocalSlamResultCallbackForSubscriptions(); + std::set expected_sensor_ids; + for (const auto& sensor_id : request.expected_sensor_ids()) { + expected_sensor_ids.insert(FromProto(sensor_id)); + } + const int trajectory_id = + GetContext() + ->map_builder() + .AddTrajectoryBuilder(expected_sensor_ids, + request.trajectory_builder_options(), + local_slam_result_callback); + GetContext()->RegisterClientIdForTrajectory( + request.client_id(), trajectory_id); + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto trajectory_builder_options = request.trajectory_builder_options(); + + // Clear the trajectory builder options to convey to the cloud + // Cartographer instance that does not need to instantiate a + // 'LocalTrajectoryBuilder'. + trajectory_builder_options.clear_trajectory_builder_2d_options(); + trajectory_builder_options.clear_trajectory_builder_3d_options(); + + // Don't instantiate the 'PureLocalizationTrimmer' on the server and don't + // freeze the trajectory on the server. + trajectory_builder_options.clear_pure_localization_trimmer(); + + // Ignore initial poses in trajectory_builder_options. + trajectory_builder_options.clear_initial_trajectory_pose(); + + auto status = + GetContext() + ->local_trajectory_uploader() + ->AddTrajectory(request.client_id(), trajectory_id, + expected_sensor_ids, trajectory_builder_options); + if (!status.ok()) { + LOG(ERROR) << "Failed to create trajectory_id " << trajectory_id + << " in uplink. Error: " << status.error_message(); + } + } + + auto response = absl::make_unique(); + response->set_trajectory_id(trajectory_id); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler.h new file mode 100644 index 0000000..76b4fc3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler.h @@ -0,0 +1,42 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_ADD_TRAJECTORY_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_TRAJECTORY_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + AddTrajectorySignature, proto::AddTrajectoryRequest, + proto::AddTrajectoryResponse, + "/cartographer.cloud.proto.MapBuilderService/AddTrajectory") + +class AddTrajectoryHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::AddTrajectoryRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_TRAJECTORY_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc new file mode 100644 index 0000000..4bbb209 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc @@ -0,0 +1,148 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" + +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "cartographer/mapping/internal/testing/mock_map_builder.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::testing::_; +using ::testing::ContainerEq; +using ::testing::Eq; +using ::testing::Return; +using ::testing::ReturnRef; +using ::testing::Test; +using ::testing::Truly; + +const std::string kMessage = R"( + client_id: "CLIENT_ID" + expected_sensor_ids { + id: "range_sensor" + type: RANGE + } + expected_sensor_ids { + id: "imu_sensor" + type: IMU + } + trajectory_builder_options { + trajectory_builder_2d_options { + min_range: 20 + max_range: 30 + } + pure_localization_trimmer { + max_submaps_to_keep: 3 + } + initial_trajectory_pose { + relative_pose { + translation { + x: 1 y: 2 z: 3 + } + rotation { + w: 4 x: 5 y: 6 z: 7 + } + } + to_trajectory_id: 8 + timestamp: 9 + } + } + )"; + +class AddTrajectoryHandlerTest + : public testing::HandlerTest { + public: + void SetUp() override { + testing::HandlerTest::SetUp(); + mock_map_builder_ = absl::make_unique(); + EXPECT_CALL(*mock_map_builder_context_, + GetLocalSlamResultCallbackForSubscriptions()) + .WillOnce(Return(nullptr)); + EXPECT_CALL(*mock_map_builder_context_, map_builder()) + .WillOnce(ReturnRef(*mock_map_builder_)); + } + + protected: + std::set ParseSensorIds( + const proto::AddTrajectoryRequest &request) { + std::set expected_sensor_ids; + for (const auto &sensor_id : request.expected_sensor_ids()) { + expected_sensor_ids.insert(cloud::FromProto(sensor_id)); + } + return expected_sensor_ids; + } + + std::unique_ptr mock_map_builder_; +}; + +TEST_F(AddTrajectoryHandlerTest, NoLocalSlamUploader) { + SetNoLocalTrajectoryUploader(); + proto::AddTrajectoryRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + EXPECT_CALL(*mock_map_builder_, + AddTrajectoryBuilder(ContainerEq(ParseSensorIds(request)), + Truly(testing::BuildProtoPredicateEquals( + &request.trajectory_builder_options())), + _)) + .WillOnce(Return(13)); + EXPECT_CALL(*mock_map_builder_context_, + RegisterClientIdForTrajectory(Eq("CLIENT_ID"), Eq(13))); + test_server_->SendWrite(request); + EXPECT_EQ(test_server_->response().trajectory_id(), 13); +} + +TEST_F(AddTrajectoryHandlerTest, WithLocalSlamUploader) { + SetMockLocalTrajectoryUploader(); + proto::AddTrajectoryRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + EXPECT_CALL(*mock_map_builder_, + AddTrajectoryBuilder(ContainerEq(ParseSensorIds(request)), + Truly(testing::BuildProtoPredicateEquals( + &request.trajectory_builder_options())), + _)) + .WillOnce(Return(13)); + EXPECT_CALL(*mock_map_builder_context_, + RegisterClientIdForTrajectory(Eq("CLIENT_ID"), Eq(13))); + auto upstream_trajectory_builder_options = + request.trajectory_builder_options(); + // Reproduce the changes to the request as done by the handler. + upstream_trajectory_builder_options.clear_trajectory_builder_2d_options(); + upstream_trajectory_builder_options.clear_trajectory_builder_3d_options(); + upstream_trajectory_builder_options.clear_pure_localization_trimmer(); + upstream_trajectory_builder_options.clear_initial_trajectory_pose(); + EXPECT_CALL(*mock_local_trajectory_uploader_, + AddTrajectory(Eq("CLIENT_ID"), Eq(13), ParseSensorIds(request), + Truly(testing::BuildProtoPredicateEquals( + &upstream_trajectory_builder_options)))) + .WillOnce(Return(grpc::Status::OK)); + test_server_->SendWrite(request); + EXPECT_EQ(test_server_->response().trajectory_id(), 13); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/delete_trajectory_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/delete_trajectory_handler.cc new file mode 100644 index 0000000..84f6e53 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/delete_trajectory_handler.cc @@ -0,0 +1,48 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/delete_trajectory_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void DeleteTrajectoryHandler::OnRequest( + const proto::DeleteTrajectoryRequest& request) { + if (!GetContext()->CheckClientIdForTrajectory( + request.client_id(), request.trajectory_id())) { + LOG(ERROR) << "Unknown trajectory with ID " << request.trajectory_id() + << " and client_id " << request.client_id(); + Finish(::grpc::Status(::grpc::NOT_FOUND, "Unknown trajectory")); + return; + } + GetContext() + ->map_builder() + .pose_graph() + ->DeleteTrajectory(request.trajectory_id()); + // TODO(gaschler): Think if LocalSlamUploader needs to be notified. + Send(absl::make_unique()); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/delete_trajectory_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/delete_trajectory_handler.h new file mode 100644 index 0000000..d390940 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/delete_trajectory_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_DELETE_TRAJECTORY_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_DELETE_TRAJECTORY_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + DeleteTrajectorySignature, proto::DeleteTrajectoryRequest, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/DeleteTrajectory") + +class DeleteTrajectoryHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::DeleteTrajectoryRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_DELETE_TRAJECTORY_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/finish_trajectory_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/finish_trajectory_handler.cc new file mode 100644 index 0000000..3eaec9b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/finish_trajectory_handler.cc @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void FinishTrajectoryHandler::OnRequest( + const proto::FinishTrajectoryRequest& request) { + if (!GetContext()->CheckClientIdForTrajectory( + request.client_id(), request.trajectory_id())) { + LOG(ERROR) << "Unknown trajectory with ID " << request.trajectory_id() + << " and client_id " << request.client_id(); + Finish(::grpc::Status(::grpc::NOT_FOUND, "Unknown trajectory")); + return; + } + GetContext()->map_builder().FinishTrajectory( + request.trajectory_id()); + GetUnsynchronizedContext() + ->NotifyFinishTrajectory(request.trajectory_id()); + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto status = + GetContext() + ->local_trajectory_uploader() + ->FinishTrajectory(request.client_id(), request.trajectory_id()); + if (!status.ok()) { + LOG(ERROR) << "Failed to finish trajectory in uplink: " + << status.error_message(); + } + } + Send(absl::make_unique()); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/finish_trajectory_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/finish_trajectory_handler.h new file mode 100644 index 0000000..f21d49a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/finish_trajectory_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_FINISH_TRAJECTORY_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_FINISH_TRAJECTORY_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + FinishTrajectorySignature, proto::FinishTrajectoryRequest, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/FinishTrajectory") + +class FinishTrajectoryHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::FinishTrajectoryRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_FINISH_TRAJECTORY_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_all_submap_poses.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_all_submap_poses.cc new file mode 100644 index 0000000..0f101b2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_all_submap_poses.cc @@ -0,0 +1,49 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_all_submap_poses.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetAllSubmapPosesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto submap_poses = GetContext() + ->map_builder() + .pose_graph() + ->GetAllSubmapPoses(); + auto response = absl::make_unique(); + for (const auto& submap_id_pose : submap_poses) { + auto* submap_pose = response->add_submap_poses(); + submap_id_pose.id.ToProto(submap_pose->mutable_submap_id()); + submap_pose->set_submap_version(submap_id_pose.data.version); + *submap_pose->mutable_global_pose() = + transform::ToProto(submap_id_pose.data.pose); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_all_submap_poses.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_all_submap_poses.h new file mode 100644 index 0000000..b184bb5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_all_submap_poses.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_ALL_SUBMAP_POSES_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_ALL_SUBMAP_POSES_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetAllSubmapPosesSignature, google::protobuf::Empty, + proto::GetAllSubmapPosesResponse, + "/cartographer.cloud.proto.MapBuilderService/GetAllSubmapPoses") + +class GetAllSubmapPosesHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_ALL_SUBMAP_POSES_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_constraints_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_constraints_handler.cc new file mode 100644 index 0000000..126217f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_constraints_handler.cc @@ -0,0 +1,45 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_constraints_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/pose_graph.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetConstraintsHandler::OnRequest(const google::protobuf::Empty& request) { + auto constraints = GetContext() + ->map_builder() + .pose_graph() + ->constraints(); + auto response = absl::make_unique(); + response->mutable_constraints()->Reserve(constraints.size()); + for (const auto& constraint : constraints) { + *response->add_constraints() = mapping::ToProto(constraint); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_constraints_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_constraints_handler.h new file mode 100644 index 0000000..36b630a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_constraints_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_CONSTRAINTS_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_CONSTRAINTS_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetConstraintsSignature, google::protobuf::Empty, + proto::GetConstraintsResponse, + "/cartographer.cloud.proto.MapBuilderService/GetConstraints") + +class GetConstraintsHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_CONSTRAINTS_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler.cc new file mode 100644 index 0000000..a60ce71 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler.cc @@ -0,0 +1,47 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetLandmarkPosesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto landmark_poses = GetContext() + ->map_builder() + .pose_graph() + ->GetLandmarkPoses(); + auto response = absl::make_unique(); + for (const auto& landmark_pose : landmark_poses) { + auto* landmark = response->add_landmark_poses(); + landmark->set_landmark_id(landmark_pose.first); + *landmark->mutable_global_pose() = transform::ToProto(landmark_pose.second); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler.h new file mode 100644 index 0000000..e39c29f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_LANDMARK_POSES_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_LANDMARK_POSES_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetLandmarkPosesSignature, google::protobuf::Empty, + proto::GetLandmarkPosesResponse, + "/cartographer.cloud.proto.MapBuilderService/GetLandmarkPoses") + +class GetLandmarkPosesHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_LANDMARK_POSES_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc new file mode 100644 index 0000000..be45875 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc @@ -0,0 +1,84 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +using ::cartographer::transform::Rigid3d; +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"( + landmark_poses { + landmark_id: "landmark_1" + global_pose { + translation { + x: 1 y: 2 z: 3 + } + rotation { + w: 1 x: 0 y: 0 z: 0 + } + } + } + landmark_poses { + landmark_id: "landmark_2" + global_pose { + translation { + x: 3 y: 2 z: 1 + } + rotation { + w: 0 x: 1 y: 0 z: 0 + } + } + } +)"; + +using GetLandmarkPosesHandlerTest = + testing::HandlerTest; + +TEST_F(GetLandmarkPosesHandlerTest, NoLocalSlamUploader) { + std::map landmark_poses{ + {"landmark_1", Rigid3d(Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(1., 0., 0., 0.))}, + {"landmark_2", Rigid3d(Eigen::Vector3d(3., 2., 1.), + Eigen::Quaterniond(0., 1., 0., 0.))}}; + EXPECT_CALL(*mock_pose_graph_, GetLandmarkPoses()) + .WillOnce(::testing::Return(landmark_poses)); + test_server_->SendWrite(google::protobuf::Empty()); + + proto::GetLandmarkPosesResponse expected_response; + EXPECT_TRUE(google::protobuf::TextFormat::ParseFromString( + kMessage, &expected_response)); + EXPECT_THAT( + test_server_->response(), + ::testing::Truly(testing::BuildProtoPredicateEquals(&expected_response))); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.cc new file mode 100644 index 0000000..4b62e4e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetLocalToGlobalTransformHandler::OnRequest( + const proto::GetLocalToGlobalTransformRequest& request) { + auto response = absl::make_unique(); + auto local_to_global = + GetContext() + ->map_builder() + .pose_graph() + ->GetLocalToGlobalTransform(request.trajectory_id()); + *response->mutable_local_to_global() = transform::ToProto(local_to_global); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h new file mode 100644 index 0000000..267fd34 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h @@ -0,0 +1,44 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetLocalToGlobalTransformSignature, proto::GetLocalToGlobalTransformRequest, + proto::GetLocalToGlobalTransformResponse, + "/cartographer.cloud.proto.MapBuilderService/GetLocalToGlobalTransform") + +class GetLocalToGlobalTransformHandler + : public async_grpc::RpcHandler { + public: + void OnRequest( + const proto::GetLocalToGlobalTransformRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_submap_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_submap_handler.cc new file mode 100644 index 0000000..554d932 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_submap_handler.cc @@ -0,0 +1,41 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_submap_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetSubmapHandler::OnRequest(const proto::GetSubmapRequest &request) { + auto response = absl::make_unique(); + response->set_error_msg( + GetContext()->map_builder().SubmapToProto( + mapping::SubmapId{request.submap_id().trajectory_id(), + request.submap_id().submap_index()}, + response->mutable_submap_query_response())); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_submap_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_submap_handler.h new file mode 100644 index 0000000..f4d18f2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_submap_handler.h @@ -0,0 +1,41 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_SUBMAP_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_SUBMAP_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetSubmapSignature, proto::GetSubmapRequest, proto::GetSubmapResponse, + "/cartographer.cloud.proto.MapBuilderService/GetSubmap") + +class GetSubmapHandler : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::GetSubmapRequest &request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_SUBMAP_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc new file mode 100644 index 0000000..5c4da10 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc @@ -0,0 +1,56 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetTrajectoryNodePosesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto node_poses = GetContext() + ->map_builder() + .pose_graph() + ->GetTrajectoryNodePoses(); + auto response = absl::make_unique(); + for (const auto& node_id_pose : node_poses) { + auto* node_pose = response->add_node_poses(); + node_id_pose.id.ToProto(node_pose->mutable_node_id()); + *node_pose->mutable_global_pose() = + transform::ToProto(node_id_pose.data.global_pose); + if (node_id_pose.data.constant_pose_data.has_value()) { + node_pose->mutable_constant_pose_data()->set_timestamp( + common::ToUniversal( + node_id_pose.data.constant_pose_data.value().time)); + *node_pose->mutable_constant_pose_data()->mutable_local_pose() = + transform::ToProto( + node_id_pose.data.constant_pose_data.value().local_pose); + } + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h new file mode 100644 index 0000000..3cbd308 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetTrajectoryNodePosesSignature, google::protobuf::Empty, + proto::GetTrajectoryNodePosesResponse, + "/cartographer.cloud.proto.MapBuilderService/GetTrajectoryNodePoses") + +class GetTrajectoryNodePosesHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc new file mode 100644 index 0000000..7f303c1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc @@ -0,0 +1,46 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetTrajectoryStatesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto trajectories_state = GetContext() + ->map_builder() + .pose_graph() + ->GetTrajectoryStates(); + auto response = absl::make_unique(); + for (const auto& entry : trajectories_state) { + (*response->mutable_trajectories_state())[entry.first] = + ToProto(entry.second); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_states_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_states_handler.h new file mode 100644 index 0000000..0a311fd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/get_trajectory_states_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetTrajectoryStatesSignature, google::protobuf::Empty, + proto::GetTrajectoryStatesResponse, + "/cartographer.cloud.proto.MapBuilderService/GetTrajectoryStates") + +class GetTrajectoryStatesHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc new file mode 100644 index 0000000..8278ee5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc @@ -0,0 +1,41 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void IsTrajectoryFinishedHandler::OnRequest( + const proto::IsTrajectoryFinishedRequest& request) { + auto response = absl::make_unique(); + response->set_is_finished( + GetContext() + ->map_builder() + .pose_graph() + ->IsTrajectoryFinished(request.trajectory_id())); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h new file mode 100644 index 0000000..5595a20 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + IsTrajectoryFinishedSignature, proto::IsTrajectoryFinishedRequest, + proto::IsTrajectoryFinishedResponse, + "/cartographer.cloud.proto.MapBuilderService/IsTrajectoryFinished") + +class IsTrajectoryFinishedHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::IsTrajectoryFinishedRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc new file mode 100644 index 0000000..4d22425 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc @@ -0,0 +1,40 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void IsTrajectoryFrozenHandler::OnRequest( + const proto::IsTrajectoryFrozenRequest& request) { + auto response = absl::make_unique(); + response->set_is_frozen(GetContext() + ->map_builder() + .pose_graph() + ->IsTrajectoryFrozen(request.trajectory_id())); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h new file mode 100644 index 0000000..6a5d892 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + IsTrajectoryFrozenSignature, proto::IsTrajectoryFrozenRequest, + proto::IsTrajectoryFrozenResponse, + "/cartographer.cloud.proto.MapBuilderService/IsTrajectoryFrozen") + +class IsTrajectoryFrozenHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::IsTrajectoryFrozenRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_from_file_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_from_file_handler.cc new file mode 100644 index 0000000..d11f6b7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_from_file_handler.cc @@ -0,0 +1,47 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void LoadStateFromFileHandler::OnRequest( + const proto::LoadStateFromFileRequest& request) { + // TODO(gaschler): This blocks a handler thread, consider working in + // background. + auto trajectory_remapping = + GetContext()->map_builder().LoadStateFromFile( + request.file_path(), request.load_frozen_state()); + for (const auto& entry : trajectory_remapping) { + GetContext()->RegisterClientIdForTrajectory( + request.client_id(), entry.second); + } + auto response = absl::make_unique(); + *response->mutable_trajectory_remapping() = ToProto(trajectory_remapping); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_from_file_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_from_file_handler.h new file mode 100644 index 0000000..f9ec5d0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_from_file_handler.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_FROM_FILE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_FROM_FILE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + LoadStateFromFileSignature, proto::LoadStateFromFileRequest, + proto::LoadStateFromFileResponse, + "/cartographer.cloud.proto.MapBuilderService/LoadStateFromFile") + +class LoadStateFromFileHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::LoadStateFromFileRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_FROM_FILE_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_handler.cc new file mode 100644 index 0000000..652a9ad --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_handler.cc @@ -0,0 +1,61 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/load_state_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void LoadStateHandler::OnRequest(const proto::LoadStateRequest& request) { + switch (request.state_chunk_case()) { + case proto::LoadStateRequest::kSerializedData: + reader_.AddProto(request.serialized_data()); + break; + case proto::LoadStateRequest::kSerializationHeader: + reader_.AddProto(request.serialization_header()); + break; + case proto::LoadStateRequest::kClientId: + client_id_ = request.client_id(); + break; + default: + LOG(FATAL) << "Unhandled proto::LoadStateRequest case."; + } + load_frozen_state_ = request.load_frozen_state(); +} + +void LoadStateHandler::OnReadsDone() { + auto trajectory_remapping = + GetContext()->map_builder().LoadState( + &reader_, load_frozen_state_); + for (const auto& entry : trajectory_remapping) { + GetContext()->RegisterClientIdForTrajectory( + client_id_, entry.second); + } + auto response = absl::make_unique(); + *response->mutable_trajectory_remapping() = ToProto(trajectory_remapping); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_handler.h new file mode 100644 index 0000000..325854c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/load_state_handler.h @@ -0,0 +1,48 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/io/internal/in_memory_proto_stream.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + LoadStateSignature, async_grpc::Stream, + proto::LoadStateResponse, + "/cartographer.cloud.proto.MapBuilderService/LoadState") + +class LoadStateHandler : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::LoadStateRequest& request) override; + void OnReadsDone() override; + + private: + io::InMemoryProtoStreamReader reader_; + std::string client_id_; + bool load_frozen_state_; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.cc new file mode 100644 index 0000000..acf8199 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.cc @@ -0,0 +1,81 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +std::unique_ptr GenerateResponse( + const std::map &last_optimized_submap_ids, + const std::map &last_optimized_node_ids) { + auto response = + absl::make_unique(); + for (const auto &entry : last_optimized_submap_ids) { + entry.second.ToProto( + &(*response->mutable_last_optimized_submap_ids())[entry.first]); + } + for (const auto &entry : last_optimized_node_ids) { + entry.second.ToProto( + &(*response->mutable_last_optimized_node_ids())[entry.first]); + } + return response; +} + +} // namespace + +void ReceiveGlobalSlamOptimizationsHandler::OnRequest( + const google::protobuf::Empty &request) { + auto writer = GetWriter(); + const int subscription_index = + GetUnsynchronizedContext() + ->SubscribeGlobalSlamOptimizations( + [writer](const std::map + &last_optimized_submap_ids, + const std::map + &last_optimized_node_ids) { + if (!writer.Write(GenerateResponse(last_optimized_submap_ids, + last_optimized_node_ids))) { + // Client closed connection. + LOG(INFO) << "Client closed connection."; + return false; + } + return true; + }); + + LOG(INFO) << "Added subscription: " << subscription_index; + subscription_index_ = subscription_index; +} + +void ReceiveGlobalSlamOptimizationsHandler::OnFinish() { + if (subscription_index_.has_value()) { + LOG(INFO) << "Removing subscription " << subscription_index_.value(); + GetUnsynchronizedContext() + ->UnsubscribeGlobalSlamOptimizations(subscription_index_.value()); + } +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.h new file mode 100644 index 0000000..725cd30 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.h @@ -0,0 +1,49 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RECEIVE_GLOBAL_SLAM_OPTIMIZATIONS_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RECEIVE_GLOBAL_SLAM_OPTIMIZATIONS_HANDLER_H + +#include + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + ReceiveGlobalSlamOptimizationsSignature, google::protobuf::Empty, + async_grpc::Stream, + "/cartographer.cloud.proto.MapBuilderService/" + "ReceiveGlobalSlamOptimizations") + +class ReceiveGlobalSlamOptimizationsHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty &request) override; + void OnFinish() override; + + private: + absl::optional subscription_index_; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RECEIVE_GLOBAL_SLAM_OPTIMIZATIONS_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_local_slam_results_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_local_slam_results_handler.cc new file mode 100644 index 0000000..af6b7cb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_local_slam_results_handler.cc @@ -0,0 +1,90 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +std::unique_ptr GenerateResponse( + std::unique_ptr + local_slam_result) { + auto response = absl::make_unique(); + response->set_trajectory_id(local_slam_result->trajectory_id); + response->set_timestamp(common::ToUniversal(local_slam_result->time)); + *response->mutable_local_pose() = + transform::ToProto(local_slam_result->local_pose); + if (local_slam_result->range_data) { + *response->mutable_range_data() = + sensor::ToProto(*local_slam_result->range_data); + } + if (local_slam_result->insertion_result) { + local_slam_result->insertion_result->node_id.ToProto( + response->mutable_insertion_result()->mutable_node_id()); + } + return response; +} + +} // namespace + +void ReceiveLocalSlamResultsHandler::OnRequest( + const proto::ReceiveLocalSlamResultsRequest& request) { + auto writer = GetWriter(); + MapBuilderContextInterface::LocalSlamSubscriptionId subscription_id = + GetUnsynchronizedContext() + ->SubscribeLocalSlamResults( + request.trajectory_id(), + [writer]( + std::unique_ptr + local_slam_result) { + if (local_slam_result) { + if (!writer.Write( + GenerateResponse(std::move(local_slam_result)))) { + // Client closed connection. + LOG(INFO) << "Client closed connection."; + return false; + } + } else { + // Callback with 'nullptr' signals that the trajectory + // finished. + writer.WritesDone(); + } + return true; + }); + + subscription_id_ = + absl::make_unique( + subscription_id); +} + +void ReceiveLocalSlamResultsHandler::OnFinish() { + if (subscription_id_) { + GetUnsynchronizedContext() + ->UnsubscribeLocalSlamResults(*subscription_id_); + } +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h new file mode 100644 index 0000000..41fb0b6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h @@ -0,0 +1,50 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H + +#include + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + ReceiveLocalSlamResultsSignature, proto::ReceiveLocalSlamResultsRequest, + async_grpc::Stream, + "/cartographer.cloud.proto.MapBuilderService/ReceiveLocalSlamResults") + +class ReceiveLocalSlamResultsHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::ReceiveLocalSlamResultsRequest& request) override; + void OnFinish() override; + + private: + std::unique_ptr + subscription_id_; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/run_final_optimization_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/run_final_optimization_handler.cc new file mode 100644 index 0000000..ac5b0dc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/run_final_optimization_handler.cc @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void RunFinalOptimizationHandler::OnRequest( + const google::protobuf::Empty& request) { + GetContext() + ->map_builder() + .pose_graph() + ->RunFinalOptimization(); + Send(absl::make_unique()); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/run_final_optimization_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/run_final_optimization_handler.h new file mode 100644 index 0000000..5e07e58 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/run_final_optimization_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RUN_FINAL_OPTIMIZATION_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RUN_FINAL_OPTIMIZATION_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + RunFinalOptimizationSignature, google::protobuf::Empty, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/RunFinalOptimization") + +class RunFinalOptimizationHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_RUN_FINAL_OPTIMIZATION_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler.cc new file mode 100644 index 0000000..b012c04 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler.cc @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/transform/transform.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void SetLandmarkPoseHandler::OnRequest( + const proto::SetLandmarkPoseRequest& request) { + GetContext() + ->map_builder() + .pose_graph() + ->SetLandmarkPose( + request.landmark_pose().landmark_id(), + transform::ToRigid3(request.landmark_pose().global_pose())); + Send(absl::make_unique()); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler.h new file mode 100644 index 0000000..9fea303 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SET_LANDMARK_POSE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SET_LANDMARK_POSE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + SetLandmarkPoseSignature, proto::SetLandmarkPoseRequest, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/SetLandmarkPose") + +class SetLandmarkPoseHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::SetLandmarkPoseRequest &request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SET_LANDMARK_POSE_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc new file mode 100644 index 0000000..485c594 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc @@ -0,0 +1,65 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" + +#include "cartographer/cloud/internal/testing/handler_test.h" +#include "cartographer/cloud/internal/testing/test_helpers.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace handlers { +namespace { + +const std::string kMessage = R"( + landmark_pose { + landmark_id: "landmark_1" + global_pose { + translation { + x: 1 y: 2 z: 3 + } + rotation { + w: 4 x: 5 y: 6 z: 7 + } + } + })"; + +using SetLandmarkPoseHandlerTest = + testing::HandlerTest; + +TEST_F(SetLandmarkPoseHandlerTest, SetLandmarkPose) { + constexpr double kEps = 1e-10; + proto::SetLandmarkPoseRequest request; + EXPECT_TRUE( + google::protobuf::TextFormat::ParseFromString(kMessage, &request)); + EXPECT_CALL( + *mock_pose_graph_, + SetLandmarkPose("landmark_1", + transform::IsNearly( + transform::Rigid3d(Eigen::Vector3d(1, 2, 3), + Eigen::Quaterniond(4, 5, 6, 7)), + kEps), + false)); + test_server_->SendWrite(request); +} + +} // namespace +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_handler.cc new file mode 100644 index 0000000..33dfc13 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_handler.cc @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/write_state_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/map_builder_server.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/io/internal/in_memory_proto_stream.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) { + auto writer = GetWriter(); + io::ForwardingProtoStreamWriter proto_stream_writer( + [writer](const google::protobuf::Message* proto) { + if (!proto) { + writer.WritesDone(); + return true; + } + + auto response = absl::make_unique(); + if (proto->GetTypeName() == + "cartographer.mapping.proto.SerializationHeader") { + response->mutable_header()->CopyFrom(*proto); + } else if (proto->GetTypeName() == + "cartographer.mapping.proto.SerializedData") { + response->mutable_serialized_data()->CopyFrom(*proto); + } else { + LOG(FATAL) << "Unsupported message type: " << proto->GetTypeName(); + } + writer.Write(std::move(response)); + return true; + }); + GetContext()->map_builder().SerializeState( + /*include_unfinished_submaps=*/false, &proto_stream_writer); + proto_stream_writer.Close(); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_handler.h new file mode 100644 index 0000000..dd00508 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_handler.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + WriteStateSignature, google::protobuf::Empty, + async_grpc::Stream, + "/cartographer.cloud.proto.MapBuilderService/WriteState") + +class WriteStateHandler : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc new file mode 100644 index 0000000..178cdd1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc @@ -0,0 +1,48 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h" + +#include "absl/memory/memory.h" +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/map_builder_server.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/io/proto_stream.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void WriteStateToFileHandler::OnRequest( + const proto::WriteStateToFileRequest& request) { + if (request.filename().empty()) { + Finish(::grpc::Status(::grpc::INVALID_ARGUMENT, "Filename empty.")); + return; + } + bool success = + GetContext() + ->map_builder() + .SerializeStateToFile( + /*include_unfinished_submaps=*/false, request.filename()); + auto response = absl::make_unique(); + response->set_success(success); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_to_file_handler.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_to_file_handler.h new file mode 100644 index 0000000..06a1e16 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/handlers/write_state_to_file_handler.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + WriteStateToFileSignature, proto::WriteStateToFileRequest, + proto::WriteStateToFileResponse, + "/cartographer.cloud.proto.MapBuilderService/WriteStateToFile") + +class WriteStateToFileHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::WriteStateToFileRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader.cc new file mode 100644 index 0000000..3f5a160 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader.cc @@ -0,0 +1,345 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/local_trajectory_uploader.h" + +#include +#include + +#include "absl/memory/memory.h" +#include "async_grpc/client.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h" +#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "cartographer/common/internal/blocking_queue.h" +#include "glog/logging.h" +#include "grpc++/grpc++.h" + +namespace cartographer { +namespace cloud { +namespace { + +using absl::make_unique; + +constexpr int kConnectionTimeoutInSeconds = 10; +constexpr int kConnectionRecoveryTimeoutInSeconds = 60; +constexpr int kTokenRefreshIntervalInSeconds = 60; +const common::Duration kPopTimeout = common::FromMilliseconds(100); + +// This defines the '::grpc::StatusCode's that are considered unrecoverable +// errors and hence no retries will be attempted by the client. +const std::set<::grpc::StatusCode> kUnrecoverableStatusCodes = { + ::grpc::DEADLINE_EXCEEDED, + ::grpc::NOT_FOUND, + ::grpc::UNAVAILABLE, + ::grpc::UNKNOWN, +}; + +bool IsNewSubmap(const mapping::proto::Submap& submap) { + return (submap.has_submap_2d() && submap.submap_2d().num_range_data() == 1) || + (submap.has_submap_3d() && submap.submap_3d().num_range_data() == 1); +} + +class LocalTrajectoryUploader : public LocalTrajectoryUploaderInterface { + public: + struct TrajectoryInfo { + // nullopt if uplink has not yet responded to AddTrajectoryRequest. + absl::optional uplink_trajectory_id; + const std::set expected_sensor_ids; + const mapping::proto::TrajectoryBuilderOptions trajectory_options; + const std::string client_id; + }; + + public: + LocalTrajectoryUploader(const std::string& uplink_server_address, + int batch_size, bool enable_ssl_encryption, + bool enable_google_auth); + ~LocalTrajectoryUploader(); + + // Starts the upload thread. + void Start() final; + + // Shuts down the upload thread. This method blocks until the shutdown is + // complete. + void Shutdown() final; + + grpc::Status AddTrajectory( + const std::string& client_id, int local_trajectory_id, + const std::set& expected_sensor_ids, + const mapping::proto::TrajectoryBuilderOptions& trajectory_options) final; + grpc::Status FinishTrajectory(const std::string& client_id, + int local_trajectory_id) final; + void EnqueueSensorData(std::unique_ptr sensor_data) final; + void TryRecovery(); + + SensorId GetLocalSlamResultSensorId(int local_trajectory_id) const final { + return SensorId{SensorId::SensorType::LOCAL_SLAM_RESULT, + "local_slam_result_" + std::to_string(local_trajectory_id)}; + } + + private: + void ProcessSendQueue(); + // Returns 'false' for failure. + bool TranslateTrajectoryId(proto::SensorMetadata* sensor_metadata); + grpc::Status RegisterTrajectory(int local_trajectory_id); + + std::shared_ptr<::grpc::Channel> client_channel_; + int batch_size_; + std::map local_trajectory_id_to_trajectory_info_; + common::BlockingQueue> send_queue_; + bool shutting_down_ = false; + std::unique_ptr upload_thread_; +}; + +LocalTrajectoryUploader::LocalTrajectoryUploader( + const std::string& uplink_server_address, int batch_size, + bool enable_ssl_encryption, bool enable_google_auth) + : batch_size_(batch_size) { + auto channel_creds = + enable_google_auth + ? grpc::GoogleDefaultCredentials() + : (enable_ssl_encryption + ? ::grpc::SslCredentials(::grpc::SslCredentialsOptions()) + : ::grpc::InsecureChannelCredentials()); + + client_channel_ = ::grpc::CreateChannel(uplink_server_address, channel_creds); + std::chrono::system_clock::time_point deadline = + std::chrono::system_clock::now() + + std::chrono::seconds(kConnectionTimeoutInSeconds); + LOG(INFO) << "Connecting to uplink " << uplink_server_address; + if (!client_channel_->WaitForConnected(deadline)) { + LOG(ERROR) << "Failed to connect to " << uplink_server_address; + } +} + +LocalTrajectoryUploader::~LocalTrajectoryUploader() {} + +void LocalTrajectoryUploader::Start() { + CHECK(!shutting_down_); + CHECK(!upload_thread_); + upload_thread_ = + make_unique([this]() { this->ProcessSendQueue(); }); +} + +void LocalTrajectoryUploader::Shutdown() { + CHECK(!shutting_down_); + CHECK(upload_thread_); + shutting_down_ = true; + upload_thread_->join(); +} + +void LocalTrajectoryUploader::TryRecovery() { + if (client_channel_->GetState(false /* try_to_connect */) != + grpc_connectivity_state::GRPC_CHANNEL_READY) { + LOG(INFO) << "Trying to re-connect to uplink..."; + std::chrono::system_clock::time_point deadline = + std::chrono::system_clock::now() + + std::chrono::seconds(kConnectionRecoveryTimeoutInSeconds); + if (!client_channel_->WaitForConnected(deadline)) { + LOG(ERROR) << "Failed to re-connect to uplink prior to recovery attempt."; + return; + } + } + LOG(INFO) << "Uplink channel ready, trying recovery."; + + // Wind the sensor_data_queue forward to the next new submap. + LOG(INFO) << "LocalTrajectoryUploader tries to recover with next submap."; + while (true) { + if (shutting_down_) { + return; + } + proto::SensorData* sensor_data = + send_queue_.PeekWithTimeout(kPopTimeout); + if (sensor_data) { + CHECK_GE(sensor_data->local_slam_result_data().submaps_size(), 0); + if (sensor_data->sensor_data_case() == + proto::SensorData::kLocalSlamResultData && + sensor_data->local_slam_result_data().submaps_size() > 0 && + IsNewSubmap(sensor_data->local_slam_result_data().submaps( + sensor_data->local_slam_result_data().submaps_size() - 1))) { + break; + } else { + send_queue_.Pop(); + } + } + } + + // Because the trajectories may be interrupted on the uplink side, we can no + // longer upload to those. + for (auto& entry : local_trajectory_id_to_trajectory_info_) { + entry.second.uplink_trajectory_id.reset(); + } + // TODO(gaschler): If the uplink did not restart but only the connection was + // interrupted, this leaks trajectories in the uplink. + + // Attempt to recreate the trajectories. + for (const auto& entry : local_trajectory_id_to_trajectory_info_) { + grpc::Status status = RegisterTrajectory(entry.first); + if (!status.ok()) { + LOG(ERROR) << "Failed to create trajectory. Aborting recovery attempt. " + << status.error_message(); + return; + } + } + LOG(INFO) << "LocalTrajectoryUploader recovered."; +} + +void LocalTrajectoryUploader::ProcessSendQueue() { + LOG(INFO) << "Starting uploader thread."; + proto::AddSensorDataBatchRequest batch_request; + while (!shutting_down_) { + auto sensor_data = send_queue_.PopWithTimeout(kPopTimeout); + if (sensor_data) { + if (!TranslateTrajectoryId(sensor_data->mutable_sensor_metadata())) { + batch_request.clear_sensor_data(); + TryRecovery(); + continue; + } + proto::SensorData* added_sensor_data = batch_request.add_sensor_data(); + *added_sensor_data = *sensor_data; + + // A submap also holds a trajectory id that must be translated to uplink's + // trajectory id. + if (added_sensor_data->has_local_slam_result_data()) { + for (mapping::proto::Submap& mutable_submap : + *added_sensor_data->mutable_local_slam_result_data() + ->mutable_submaps()) { + mutable_submap.mutable_submap_id()->set_trajectory_id( + added_sensor_data->sensor_metadata().trajectory_id()); + } + } + + if (batch_request.sensor_data_size() == batch_size_) { + async_grpc::Client client( + client_channel_, common::FromSeconds(kConnectionTimeoutInSeconds), + async_grpc::CreateUnlimitedConstantDelayStrategy( + common::FromSeconds(1), kUnrecoverableStatusCodes)); + if (client.Write(batch_request)) { + LOG(INFO) << "Uploaded " << batch_request.ByteSize() + << " bytes of sensor data."; + batch_request.clear_sensor_data(); + continue; + } + // Unrecoverable error occurred. Attempt recovery. + batch_request.clear_sensor_data(); + TryRecovery(); + } + } + } +} + +bool LocalTrajectoryUploader::TranslateTrajectoryId( + proto::SensorMetadata* sensor_metadata) { + auto it = local_trajectory_id_to_trajectory_info_.find( + sensor_metadata->trajectory_id()); + if (it == local_trajectory_id_to_trajectory_info_.end()) { + return false; + } + if (!it->second.uplink_trajectory_id.has_value()) { + // Could not yet register trajectory with uplink server. + return false; + } + int cloud_trajectory_id = it->second.uplink_trajectory_id.value(); + sensor_metadata->set_trajectory_id(cloud_trajectory_id); + return true; +} + +grpc::Status LocalTrajectoryUploader::AddTrajectory( + const std::string& client_id, int local_trajectory_id, + const std::set& expected_sensor_ids, + const mapping::proto::TrajectoryBuilderOptions& trajectory_options) { + CHECK_EQ(local_trajectory_id_to_trajectory_info_.count(local_trajectory_id), + 0); + local_trajectory_id_to_trajectory_info_.emplace( + local_trajectory_id, + TrajectoryInfo{{}, expected_sensor_ids, trajectory_options, client_id}); + return RegisterTrajectory(local_trajectory_id); +} + +grpc::Status LocalTrajectoryUploader::RegisterTrajectory( + int local_trajectory_id) { + TrajectoryInfo& trajectory_info = + local_trajectory_id_to_trajectory_info_.at(local_trajectory_id); + proto::AddTrajectoryRequest request; + request.set_client_id(trajectory_info.client_id); + *request.mutable_trajectory_builder_options() = + trajectory_info.trajectory_options; + for (const SensorId& sensor_id : trajectory_info.expected_sensor_ids) { + // Range sensors are not forwarded, but combined into a LocalSlamResult. + if (sensor_id.type != SensorId::SensorType::RANGE) { + *request.add_expected_sensor_ids() = cloud::ToProto(sensor_id); + } + } + *request.add_expected_sensor_ids() = + cloud::ToProto(GetLocalSlamResultSensorId(local_trajectory_id)); + async_grpc::Client client( + client_channel_, common::FromSeconds(kConnectionTimeoutInSeconds)); + ::grpc::Status status; + if (!client.Write(request, &status)) { + LOG(ERROR) << "Failed to register local_trajectory_id " + << local_trajectory_id << " with uplink server. " + << status.error_message(); + return status; + } + LOG(INFO) << "Created trajectory for client_id: " << trajectory_info.client_id + << " local trajectory_id: " << local_trajectory_id + << " uplink trajectory_id: " << client.response().trajectory_id(); + trajectory_info.uplink_trajectory_id = client.response().trajectory_id(); + return status; +} + +grpc::Status LocalTrajectoryUploader::FinishTrajectory( + const std::string& client_id, int local_trajectory_id) { + auto it = local_trajectory_id_to_trajectory_info_.find(local_trajectory_id); + if (it == local_trajectory_id_to_trajectory_info_.end()) { + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "local_trajectory_id has not been" + " registered with AddTrajectory."); + } + auto cloud_trajectory_id = it->second.uplink_trajectory_id; + if (!cloud_trajectory_id.has_value()) { + return grpc::Status( + grpc::StatusCode::UNAVAILABLE, + "trajectory_id has not been created in uplink, ignoring."); + } + proto::FinishTrajectoryRequest request; + request.set_client_id(client_id); + request.set_trajectory_id(cloud_trajectory_id.value()); + async_grpc::Client client( + client_channel_, common::FromSeconds(kConnectionTimeoutInSeconds)); + grpc::Status status; + client.Write(request, &status); + return status; +} + +void LocalTrajectoryUploader::EnqueueSensorData( + std::unique_ptr sensor_data) { + send_queue_.Push(std::move(sensor_data)); +} + +} // namespace + +std::unique_ptr CreateLocalTrajectoryUploader( + const std::string& uplink_server_address, int batch_size, + bool enable_ssl_encryption, bool enable_google_auth) { + return make_unique(uplink_server_address, batch_size, + enable_ssl_encryption, + enable_google_auth); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader.h new file mode 100644 index 0000000..323e2c2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader.h @@ -0,0 +1,72 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_LOCAL_TRAJECTORY_UPLOADER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_LOCAL_TRAJECTORY_UPLOADER_H + +#include +#include +#include + +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "grpc++/support/status.h" + +namespace cartographer { +namespace cloud { + +// Uploads sensor data batches to uplink server. +// Gracefully handles interruptions of the connection. +class LocalTrajectoryUploaderInterface { + public: + using SensorId = mapping::TrajectoryBuilderInterface::SensorId; + + virtual ~LocalTrajectoryUploaderInterface() = default; + + // Starts the upload thread. + virtual void Start() = 0; + + // Shuts down the upload thread. This method blocks until the shutdown is + // complete. + virtual void Shutdown() = 0; + + // Enqueue an Add*DataRequest message to be uploaded. + virtual void EnqueueSensorData( + std::unique_ptr sensor_data) = 0; + + // Creates a new trajectory with the specified settings in the uplink. A + // return 'value' with '!value.ok()' indicates that the creation failed. + virtual grpc::Status AddTrajectory( + const std::string& client_id, int local_trajectory_id, + const std::set& expected_sensor_ids, + const mapping::proto::TrajectoryBuilderOptions& trajectory_options) = 0; + + virtual grpc::Status FinishTrajectory(const std::string& client_id, + int local_trajectory_id) = 0; + virtual SensorId GetLocalSlamResultSensorId( + int local_trajectory_id) const = 0; +}; + +// Returns LocalTrajectoryUploader with the actual implementation. +std::unique_ptr CreateLocalTrajectoryUploader( + const std::string& uplink_server_address, int batch_size, + bool enable_ssl_encryption, bool enable_google_auth); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_LOCAL_TRAJECTORY_UPLOADER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader_test.cc new file mode 100644 index 0000000..b333dc9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/local_trajectory_uploader_test.cc @@ -0,0 +1,57 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/local_trajectory_uploader.h" + +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using SensorId = ::cartographer::mapping::TrajectoryBuilderInterface::SensorId; + +namespace cartographer { +namespace cloud { +namespace { + +constexpr char kClientId[] = "CLIENT_ID"; +const SensorId kImuSensorId{SensorId::SensorType::IMU, "imu"}; +const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"}; +const int kLocalTrajectoryId = 3; + +TEST(LocalTrajectoryUploaderTest, HandlesInvalidUplink) { + auto uploader = CreateLocalTrajectoryUploader("invalid-uplink-address:50051", + /*batch_size=*/1, false, false); + uploader->Start(); + mapping::proto::TrajectoryBuilderOptions options; + auto status = uploader->AddTrajectory( + kClientId, kLocalTrajectoryId, {kRangeSensorId, kImuSensorId}, options); + EXPECT_FALSE(status.ok()); + auto sensor_data = absl::make_unique(); + sensor_data->mutable_sensor_metadata()->set_client_id(kClientId); + sensor_data->mutable_sensor_metadata()->set_sensor_id(kImuSensorId.id); + sensor_data->mutable_sensor_metadata()->set_trajectory_id(kLocalTrajectoryId); + sensor_data->mutable_imu_data()->set_timestamp(1); + uploader->EnqueueSensorData(std::move(sensor_data)); + auto sensor_id = uploader->GetLocalSlamResultSensorId(kLocalTrajectoryId); + EXPECT_THAT(sensor_id.id, ::testing::Not(::testing::IsEmpty())); + status = uploader->FinishTrajectory(kClientId, kLocalTrajectoryId); + EXPECT_FALSE(status.ok()); + uploader->Shutdown(); +} + +} // namespace +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_impl.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_impl.cc new file mode 100644 index 0000000..3f3c462 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_impl.cc @@ -0,0 +1,45 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/map_builder_server.h" +#include "cartographer/mapping/internal/2d/local_slam_result_2d.h" +#include "cartographer/mapping/internal/3d/local_slam_result_3d.h" + +namespace cartographer { +namespace cloud { + +template <> +void MapBuilderContext::EnqueueLocalSlamResultData( + int trajectory_id, const std::string& sensor_id, + const mapping::proto::LocalSlamResultData& local_slam_result_data) { + map_builder_server_->incoming_data_queue_.Push(absl::make_unique( + Data{trajectory_id, + absl::make_unique( + sensor_id, local_slam_result_data, &submap_controller_)})); +} + +template <> +void MapBuilderContext::EnqueueLocalSlamResultData( + int trajectory_id, const std::string& sensor_id, + const mapping::proto::LocalSlamResultData& local_slam_result_data) { + map_builder_server_->incoming_data_queue_.Push(absl::make_unique( + Data{trajectory_id, + absl::make_unique( + sensor_id, local_slam_result_data, &submap_controller_)})); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_impl.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_impl.h new file mode 100644 index 0000000..186e1e7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_impl.h @@ -0,0 +1,138 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_IMPL_H +#define CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_IMPL_H + +namespace cartographer { +namespace cloud { + +template +MapBuilderContext::MapBuilderContext( + MapBuilderServer* map_builder_server) + : map_builder_server_(map_builder_server) {} + +template +mapping::MapBuilderInterface& MapBuilderContext::map_builder() { + return *map_builder_server_->map_builder_; +} + +template +common::BlockingQueue>& +MapBuilderContext::sensor_data_queue() { + return map_builder_server_->incoming_data_queue_; +} + +template +mapping::TrajectoryBuilderInterface::LocalSlamResultCallback +MapBuilderContext::GetLocalSlamResultCallbackForSubscriptions() { + return [this](int trajectory_id, common::Time time, + transform::Rigid3d local_pose, sensor::RangeData range_data, + std::unique_ptr< + const mapping::TrajectoryBuilderInterface::InsertionResult> + insertion_result) { + auto it = client_ids_.find(trajectory_id); + if (it == client_ids_.end()) { + LOG(ERROR) << "Unknown trajectory_id " << trajectory_id << ". Ignoring."; + return; + } + map_builder_server_->OnLocalSlamResult(trajectory_id, it->second, time, + local_pose, std::move(range_data), + std::move(insertion_result)); + }; +} + +template +void MapBuilderContext::AddSensorDataToTrajectory( + const Data& sensor_data) { + sensor_data.data->AddToTrajectoryBuilder( + map_builder_server_->map_builder_->GetTrajectoryBuilder( + sensor_data.trajectory_id)); +} + +template +MapBuilderContextInterface::LocalSlamSubscriptionId +MapBuilderContext::SubscribeLocalSlamResults( + int trajectory_id, LocalSlamSubscriptionCallback callback) { + return map_builder_server_->SubscribeLocalSlamResults(trajectory_id, + callback); +} + +template +void MapBuilderContext::UnsubscribeLocalSlamResults( + const LocalSlamSubscriptionId& subscription_id) { + map_builder_server_->UnsubscribeLocalSlamResults(subscription_id); +} + +template +int MapBuilderContext::SubscribeGlobalSlamOptimizations( + GlobalSlamOptimizationCallback callback) { + return map_builder_server_->SubscribeGlobalSlamOptimizations(callback); +} + +template +void MapBuilderContext::UnsubscribeGlobalSlamOptimizations( + int subscription_index) { + map_builder_server_->UnsubscribeGlobalSlamOptimizations(subscription_index); +} + +template +void MapBuilderContext::NotifyFinishTrajectory(int trajectory_id) { + map_builder_server_->NotifyFinishTrajectory(trajectory_id); +} + +template +LocalTrajectoryUploaderInterface* +MapBuilderContext::local_trajectory_uploader() { + return map_builder_server_->local_trajectory_uploader_.get(); +} + +template +void MapBuilderContext::EnqueueSensorData( + int trajectory_id, std::unique_ptr data) { + map_builder_server_->incoming_data_queue_.Push( + absl::make_unique(Data{trajectory_id, std::move(data)})); +} + +template +void MapBuilderContext::RegisterClientIdForTrajectory( + const std::string& client_id, int trajectory_id) { + CHECK_EQ(client_ids_.count(trajectory_id), 0u); + LOG(INFO) << "Registering trajectory_id " << trajectory_id << " to client_id " + << client_id; + client_ids_[trajectory_id] = client_id; +} + +template +bool MapBuilderContext::CheckClientIdForTrajectory( + const std::string& client_id, int trajectory_id) { + return (client_ids_.count(trajectory_id) > 0 && + client_ids_[trajectory_id] == client_id); +} + +template <> +void MapBuilderContext::EnqueueLocalSlamResultData( + int trajectory_id, const std::string& sensor_id, + const mapping::proto::LocalSlamResultData& local_slam_result_data); +template <> +void MapBuilderContext::EnqueueLocalSlamResultData( + int trajectory_id, const std::string& sensor_id, + const mapping::proto::LocalSlamResultData& local_slam_result_data); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_IMPL_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_interface.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_interface.h new file mode 100644 index 0000000..bbcd111 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_context_interface.h @@ -0,0 +1,104 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_MAP_BUILDER_CONTEXT_INTERFACE_H +#define CARTOGRAPHER_CLOUD_MAP_BUILDER_CONTEXT_INTERFACE_H + +#include "async_grpc/execution_context.h" +#include "cartographer/cloud/internal/local_trajectory_uploader.h" +#include "cartographer/common/internal/blocking_queue.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/sensor/data.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace cloud { + +class MapBuilderServer; +class MapBuilderContextInterface : public async_grpc::ExecutionContext { + public: + struct LocalSlamResult { + int trajectory_id; + common::Time time; + transform::Rigid3d local_pose; + std::shared_ptr range_data; + std::unique_ptr + insertion_result; + }; + // Calling with 'nullptr' signals subscribers that the subscription has ended, + // e.g. this happens when the corresponding trajectory was finished and hence + // no more local SLAM updates will occur. + // The callback can return 'false' to indicate that the client is not + // interested in more local SLAM updates and 'MapBuilderServer' will end the + // subscription. + using LocalSlamSubscriptionCallback = + std::function)>; + + // The callback can return 'false' to indicate that the client is not + // interested in more global SLAM runs and 'MapBuilderServer' will end the + // subscription. + using GlobalSlamOptimizationCallback = std::function&, + const std::map&)>; + + struct Data { + int trajectory_id; + std::unique_ptr data; + }; + struct LocalSlamSubscriptionId { + const int trajectory_id; + const int subscription_index; + }; + + MapBuilderContextInterface() = default; + ~MapBuilderContextInterface() = default; + + MapBuilderContextInterface(const MapBuilderContextInterface&) = delete; + MapBuilderContextInterface& operator=(const MapBuilderContextInterface&) = + delete; + + virtual mapping::MapBuilderInterface& map_builder() = 0; + virtual common::BlockingQueue>& sensor_data_queue() = 0; + virtual mapping::TrajectoryBuilderInterface::LocalSlamResultCallback + GetLocalSlamResultCallbackForSubscriptions() = 0; + virtual void AddSensorDataToTrajectory(const Data& sensor_data) = 0; + virtual LocalSlamSubscriptionId SubscribeLocalSlamResults( + int trajectory_id, LocalSlamSubscriptionCallback callback) = 0; + virtual void UnsubscribeLocalSlamResults( + const LocalSlamSubscriptionId& subscription_id) = 0; + virtual int SubscribeGlobalSlamOptimizations( + GlobalSlamOptimizationCallback callback) = 0; + virtual void UnsubscribeGlobalSlamOptimizations(int subscription_index) = 0; + virtual void NotifyFinishTrajectory(int trajectory_id) = 0; + virtual LocalTrajectoryUploaderInterface* local_trajectory_uploader() = 0; + virtual void EnqueueSensorData(int trajectory_id, + std::unique_ptr data) = 0; + virtual void EnqueueLocalSlamResultData( + int trajectory_id, const std::string& sensor_id, + const mapping::proto::LocalSlamResultData& local_slam_result_data) = 0; + virtual void RegisterClientIdForTrajectory(const std::string& client_id, + int trajectory_id) = 0; + virtual bool CheckClientIdForTrajectory(const std::string& client_id, + int trajectory_id) = 0; +}; + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_MAP_BUILDER_CONTEXT_INTERFACE_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_server.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_server.cc new file mode 100644 index 0000000..6972bf5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_server.cc @@ -0,0 +1,297 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/cloud/internal/map_builder_server.h" + +#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" +#include "cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h" +#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/delete_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" +#include "cartographer/cloud/internal/handlers/get_all_submap_poses.h" +#include "cartographer/cloud/internal/handlers/get_constraints_handler.h" +#include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" +#include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" +#include "cartographer/cloud/internal/handlers/get_submap_handler.h" +#include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" +#include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" +#include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" +#include "cartographer/cloud/internal/handlers/load_state_handler.h" +#include "cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.h" +#include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" +#include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" +#include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" +#include "cartographer/cloud/internal/handlers/write_state_handler.h" +#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h" +#include "cartographer/cloud/internal/sensor/serialization.h" +#include "glog/logging.h" + +namespace cartographer { +namespace cloud { +namespace { + +static auto* kIncomingDataQueueMetric = metrics::Gauge::Null(); +constexpr int kMaxMessageSize = 100 * 1024 * 1024; // 100 MB +const common::Duration kPopTimeout = common::FromMilliseconds(100); + +} // namespace + +MapBuilderServer::MapBuilderServer( + const proto::MapBuilderServerOptions& map_builder_server_options, + std::unique_ptr map_builder) + : map_builder_(std::move(map_builder)) { + async_grpc::Server::Builder server_builder; + server_builder.SetServerAddress(map_builder_server_options.server_address()); + server_builder.SetNumGrpcThreads( + map_builder_server_options.num_grpc_threads()); + server_builder.SetNumEventThreads( + map_builder_server_options.num_event_threads()); + server_builder.SetMaxSendMessageSize(kMaxMessageSize); + server_builder.SetMaxReceiveMessageSize(kMaxMessageSize); + if (!map_builder_server_options.uplink_server_address().empty()) { + local_trajectory_uploader_ = CreateLocalTrajectoryUploader( + map_builder_server_options.uplink_server_address(), + map_builder_server_options.upload_batch_size(), + map_builder_server_options.enable_ssl_encryption(), + map_builder_server_options.enable_google_auth()); + } + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder + .RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + grpc_server_ = server_builder.Build(); + if (map_builder_server_options.map_builder_options() + .use_trajectory_builder_2d()) { + grpc_server_->SetExecutionContext( + absl::make_unique>(this)); + } else if (map_builder_server_options.map_builder_options() + .use_trajectory_builder_3d()) { + grpc_server_->SetExecutionContext( + absl::make_unique>(this)); + } else { + LOG(FATAL) + << "Set either use_trajectory_builder_2d or use_trajectory_builder_3d"; + } + map_builder_->pose_graph()->SetGlobalSlamOptimizationCallback( + [this](const std::map& last_optimized_submap_ids, + const std::map& last_optimized_node_ids) { + OnGlobalSlamOptimizations(last_optimized_submap_ids, + last_optimized_node_ids); + }); +} + +void MapBuilderServer::Start() { + shutting_down_ = false; + if (local_trajectory_uploader_) { + local_trajectory_uploader_->Start(); + } + StartSlamThread(); + grpc_server_->Start(); +} + +void MapBuilderServer::WaitForShutdown() { + grpc_server_->WaitForShutdown(); + if (slam_thread_) { + slam_thread_->join(); + } + if (local_trajectory_uploader_) { + local_trajectory_uploader_->Shutdown(); + } +} + +void MapBuilderServer::Shutdown() { + shutting_down_ = true; + grpc_server_->Shutdown(); + if (slam_thread_) { + slam_thread_->join(); + slam_thread_.reset(); + } + if (local_trajectory_uploader_) { + local_trajectory_uploader_->Shutdown(); + local_trajectory_uploader_.reset(); + } +} + +void MapBuilderServer::ProcessSensorDataQueue() { + LOG(INFO) << "Starting SLAM thread."; + while (!shutting_down_) { + kIncomingDataQueueMetric->Set(incoming_data_queue_.Size()); + std::unique_ptr sensor_data = + incoming_data_queue_.PopWithTimeout(kPopTimeout); + if (sensor_data) { + grpc_server_->GetContext() + ->AddSensorDataToTrajectory(*sensor_data); + } + } +} + +void MapBuilderServer::StartSlamThread() { + CHECK(!slam_thread_); + + // Start the SLAM processing thread. + slam_thread_ = absl::make_unique( + [this]() { this->ProcessSensorDataQueue(); }); +} + +void MapBuilderServer::OnLocalSlamResult( + int trajectory_id, const std::string client_id, common::Time time, + transform::Rigid3d local_pose, sensor::RangeData range_data, + std::unique_ptr + insertion_result) { + auto shared_range_data = + std::make_shared(std::move(range_data)); + + // If there is an uplink server and a submap insertion happened, enqueue this + // local SLAM result for uploading. + if (insertion_result && + grpc_server_->GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto sensor_data = absl::make_unique(); + auto sensor_id = + grpc_server_->GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->GetLocalSlamResultSensorId(trajectory_id); + CreateSensorDataForLocalSlamResult(sensor_id.id, trajectory_id, client_id, + time, starting_submap_index_, + *insertion_result, sensor_data.get()); + // TODO(cschuet): Make this more robust. + if (insertion_result->insertion_submaps.front()->insertion_finished()) { + ++starting_submap_index_; + } + grpc_server_->GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueSensorData(std::move(sensor_data)); + } + + absl::MutexLock locker(&subscriptions_lock_); + for (auto& entry : local_slam_subscriptions_[trajectory_id]) { + auto copy_of_insertion_result = + insertion_result + ? absl::make_unique< + const mapping::TrajectoryBuilderInterface::InsertionResult>( + *insertion_result) + : nullptr; + MapBuilderContextInterface::LocalSlamSubscriptionCallback callback = + entry.second; + if (!callback( + absl::make_unique( + MapBuilderContextInterface::LocalSlamResult{ + trajectory_id, time, local_pose, shared_range_data, + std::move(copy_of_insertion_result)}))) { + LOG(INFO) << "Removing subscription with index: " << entry.first; + CHECK_EQ(local_slam_subscriptions_[trajectory_id].erase(entry.first), 1u); + } + } +} + +void MapBuilderServer::OnGlobalSlamOptimizations( + const std::map& last_optimized_submap_ids, + const std::map& last_optimized_node_ids) { + absl::MutexLock locker(&subscriptions_lock_); + for (auto& entry : global_slam_subscriptions_) { + if (!entry.second(last_optimized_submap_ids, last_optimized_node_ids)) { + LOG(INFO) << "Removing subscription with index: " << entry.first; + CHECK_EQ(global_slam_subscriptions_.erase(entry.first), 1u); + } + } +} + +MapBuilderContextInterface::LocalSlamSubscriptionId +MapBuilderServer::SubscribeLocalSlamResults( + int trajectory_id, + MapBuilderContextInterface::LocalSlamSubscriptionCallback callback) { + absl::MutexLock locker(&subscriptions_lock_); + local_slam_subscriptions_[trajectory_id].emplace(current_subscription_index_, + callback); + return MapBuilderContextInterface::LocalSlamSubscriptionId{ + trajectory_id, current_subscription_index_++}; +} + +void MapBuilderServer::UnsubscribeLocalSlamResults( + const MapBuilderContextInterface::LocalSlamSubscriptionId& + subscription_id) { + absl::MutexLock locker(&subscriptions_lock_); + CHECK_EQ(local_slam_subscriptions_[subscription_id.trajectory_id].erase( + subscription_id.subscription_index), + 1u); +} + +int MapBuilderServer::SubscribeGlobalSlamOptimizations( + MapBuilderContextInterface::GlobalSlamOptimizationCallback callback) { + absl::MutexLock locker(&subscriptions_lock_); + global_slam_subscriptions_.emplace(current_subscription_index_, callback); + return current_subscription_index_++; +} + +void MapBuilderServer::UnsubscribeGlobalSlamOptimizations( + int subscription_index) { + absl::MutexLock locker(&subscriptions_lock_); + CHECK_EQ(global_slam_subscriptions_.erase(subscription_index), 1u); +} + +void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) { + absl::MutexLock locker(&subscriptions_lock_); + for (auto& entry : local_slam_subscriptions_[trajectory_id]) { + MapBuilderContextInterface::LocalSlamSubscriptionCallback callback = + entry.second; + // 'nullptr' signals subscribers that the trajectory finished. + callback(nullptr); + } +} + +void MapBuilderServer::WaitUntilIdle() { + incoming_data_queue_.WaitUntilEmpty(); + map_builder_->pose_graph()->RunFinalOptimization(); +} + +void MapBuilderServer::RegisterMetrics(metrics::FamilyFactory* factory) { + auto* queue_length = factory->NewGaugeFamily( + "cloud_internal_map_builder_server_incoming_data_queue_length", + "Incoming SLAM Data Queue length"); + kIncomingDataQueueMetric = queue_length->Add({}); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_server.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_server.h new file mode 100644 index 0000000..03a70aa --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/map_builder_server.h @@ -0,0 +1,153 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_INTERNAL_MAP_BUILDER_SERVER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_MAP_BUILDER_SERVER_H + +#include "async_grpc/execution_context.h" +#include "async_grpc/server.h" +#include "cartographer/cloud/internal/local_trajectory_uploader.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/map_builder_server_interface.h" +#include "cartographer/cloud/proto/map_builder_server_options.pb.h" +#include "cartographer/common/internal/blocking_queue.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/internal/submap_controller.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/internal/dispatchable.h" + +namespace cartographer { +namespace cloud { + +class MapBuilderServer; + +template +class MapBuilderContext : public MapBuilderContextInterface { + public: + MapBuilderContext(MapBuilderServer* map_builder_server); + mapping::MapBuilderInterface& map_builder() override; + common::BlockingQueue>& + sensor_data_queue() override; + mapping::TrajectoryBuilderInterface::LocalSlamResultCallback + GetLocalSlamResultCallbackForSubscriptions() override; + void AddSensorDataToTrajectory(const Data& sensor_data) override; + MapBuilderContextInterface::LocalSlamSubscriptionId SubscribeLocalSlamResults( + int trajectory_id, LocalSlamSubscriptionCallback callback) override; + void UnsubscribeLocalSlamResults( + const LocalSlamSubscriptionId& subscription_id) override; + int SubscribeGlobalSlamOptimizations( + GlobalSlamOptimizationCallback callback) override; + void UnsubscribeGlobalSlamOptimizations(int subscription_index) override; + void NotifyFinishTrajectory(int trajectory_id) override; + LocalTrajectoryUploaderInterface* local_trajectory_uploader() override; + void EnqueueSensorData(int trajectory_id, + std::unique_ptr data) override; + void EnqueueLocalSlamResultData(int trajectory_id, + const std::string& sensor_id, + const mapping::proto::LocalSlamResultData& + local_slam_result_data) override; + void RegisterClientIdForTrajectory(const std::string& client_id, + int trajectory_id) override; + bool CheckClientIdForTrajectory(const std::string& client_id, + int trajectory_id) override; + + private: + MapBuilderServer* map_builder_server_; + mapping::SubmapController submap_controller_; + std::map client_ids_; +}; + +class MapBuilderServer : public MapBuilderServerInterface { + public: + friend MapBuilderContext; + friend MapBuilderContext; + + MapBuilderServer( + const proto::MapBuilderServerOptions& map_builder_server_options, + std::unique_ptr map_builder); + ~MapBuilderServer() {} + + // Starts the gRPC server, the 'LocalTrajectoryUploader' and the SLAM thread. + void Start() final; + + // Waits for the 'MapBuilderServer' to shut down. Note: The server must be + // either shutting down or some other thread must call 'Shutdown()' for this + // function to ever return. + void WaitForShutdown() final; + + // Waits until all computation is finished (for testing). + void WaitUntilIdle() final; + + // Shuts down the gRPC server, the 'LocalTrajectoryUploader' and the SLAM + // thread. + void Shutdown() final; + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + using LocalSlamResultHandlerSubscriptions = + std::map; + + void ProcessSensorDataQueue(); + void StartSlamThread(); + void OnLocalSlamResult( + int trajectory_id, const std::string client_id, common::Time time, + transform::Rigid3d local_pose, sensor::RangeData range_data, + std::unique_ptr< + const mapping::TrajectoryBuilderInterface::InsertionResult> + insertion_result); + void OnGlobalSlamOptimizations( + const std::map& last_optimized_submap_ids, + const std::map& last_optimized_node_ids); + MapBuilderContextInterface::LocalSlamSubscriptionId SubscribeLocalSlamResults( + int trajectory_id, + MapBuilderContextInterface::LocalSlamSubscriptionCallback callback); + void UnsubscribeLocalSlamResults( + const MapBuilderContextInterface::LocalSlamSubscriptionId& + subscription_id); + int SubscribeGlobalSlamOptimizations( + MapBuilderContextInterface::GlobalSlamOptimizationCallback callback); + void UnsubscribeGlobalSlamOptimizations(int subscription_index); + void NotifyFinishTrajectory(int trajectory_id); + + bool shutting_down_ = false; + std::unique_ptr slam_thread_; + std::unique_ptr grpc_server_; + std::unique_ptr map_builder_; + common::BlockingQueue> + incoming_data_queue_; + absl::Mutex subscriptions_lock_; + int current_subscription_index_ = 0; + std::map + local_slam_subscriptions_ GUARDED_BY(subscriptions_lock_); + std::map + global_slam_subscriptions_ GUARDED_BY(subscriptions_lock_); + std::unique_ptr local_trajectory_uploader_; + int starting_submap_index_ = 0; +}; + +} // namespace cloud +} // namespace cartographer + +#include "cartographer/cloud/internal/map_builder_context_impl.h" + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_MAP_BUILDER_SERVER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/mapping/serialization.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/mapping/serialization.cc new file mode 100644 index 0000000..b6a56e9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/mapping/serialization.cc @@ -0,0 +1,76 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/mapping/serialization.h" + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace cloud { +namespace { + +using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + +} // namespace + +proto::TrajectoryState ToProto(const TrajectoryState& trajectory_state) { + switch (trajectory_state) { + case TrajectoryState::ACTIVE: + return proto::TrajectoryState::ACTIVE; + case TrajectoryState::FINISHED: + return proto::TrajectoryState::FINISHED; + case TrajectoryState::FROZEN: + return proto::TrajectoryState::FROZEN; + case TrajectoryState::DELETED: + return proto::TrajectoryState::DELETED; + default: + LOG(FATAL) << "Unknown TrajectoryState"; + } +} + +TrajectoryState FromProto(const proto::TrajectoryState& proto) { + switch (proto) { + case proto::TrajectoryState::ACTIVE: + return TrajectoryState::ACTIVE; + case proto::TrajectoryState::FINISHED: + return TrajectoryState::FINISHED; + case proto::TrajectoryState::FROZEN: + return TrajectoryState::FROZEN; + case proto::TrajectoryState::DELETED: + return TrajectoryState::DELETED; + default: + LOG(FATAL) << "Unknown proto::TrajectoryState"; + } +} + +proto::TrajectoryRemapping ToProto( + const std::map& trajectory_remapping) { + proto::TrajectoryRemapping proto; + *proto.mutable_serialized_trajectories_to_trajectories() = + google::protobuf::Map(trajectory_remapping.begin(), + trajectory_remapping.end()); + return proto; +} + +std::map FromProto(const proto::TrajectoryRemapping& proto) { + return std::map( + proto.serialized_trajectories_to_trajectories().begin(), + proto.serialized_trajectories_to_trajectories().end()); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/mapping/serialization.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/mapping/serialization.h new file mode 100644 index 0000000..2dd9ac4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/mapping/serialization.h @@ -0,0 +1,38 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H +#define CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H + +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/pose_graph_interface.h" + +namespace cartographer { +namespace cloud { + +proto::TrajectoryState ToProto( + const mapping::PoseGraphInterface::TrajectoryState& trajectory_state); +mapping::PoseGraphInterface::TrajectoryState FromProto( + const proto::TrajectoryState& proto); + +proto::TrajectoryRemapping ToProto( + const std::map& trajectory_remapping); +std::map FromProto(const proto::TrajectoryRemapping& proto); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/sensor/serialization.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/sensor/serialization.cc new file mode 100644 index 0000000..429ba68 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/sensor/serialization.cc @@ -0,0 +1,165 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/sensor/serialization.h" + +namespace cartographer { +namespace cloud { + +void CreateSensorMetadata(const std::string& sensor_id, const int trajectory_id, + const std::string& client_id, + proto::SensorMetadata* proto) { + proto->set_sensor_id(sensor_id); + proto->set_trajectory_id(trajectory_id); + proto->set_client_id(client_id); +} + +void CreateAddFixedFramePoseDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::FixedFramePoseData& fixed_frame_pose_data, + proto::AddFixedFramePoseDataRequest* proto) { + CreateSensorMetadata(sensor_id, trajectory_id, client_id, + proto->mutable_sensor_metadata()); + *proto->mutable_fixed_frame_pose_data() = fixed_frame_pose_data; +} + +void CreateAddImuDataRequest(const std::string& sensor_id, + const int trajectory_id, + const std::string& client_id, + const sensor::proto::ImuData& imu_data, + proto::AddImuDataRequest* proto) { + CreateSensorMetadata(sensor_id, trajectory_id, client_id, + proto->mutable_sensor_metadata()); + *proto->mutable_imu_data() = imu_data; +} + +void CreateAddOdometryDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::OdometryData& odometry_data, + proto::AddOdometryDataRequest* proto) { + CreateSensorMetadata(sensor_id, trajectory_id, client_id, + proto->mutable_sensor_metadata()); + *proto->mutable_odometry_data() = odometry_data; +} + +void CreateAddRangeFinderDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::TimedPointCloudData& timed_point_cloud_data, + proto::AddRangefinderDataRequest* proto) { + CreateSensorMetadata(sensor_id, trajectory_id, client_id, + proto->mutable_sensor_metadata()); + *proto->mutable_timed_point_cloud_data() = timed_point_cloud_data; +} + +void CreateAddLandmarkDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::LandmarkData& landmark_data, + proto::AddLandmarkDataRequest* proto) { + CreateSensorMetadata(sensor_id, trajectory_id, client_id, + proto->mutable_sensor_metadata()); + *proto->mutable_landmark_data() = landmark_data; +} + +void CreateSensorDataForLocalSlamResult( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, common::Time time, int starting_submap_index, + const mapping::TrajectoryBuilderInterface::InsertionResult& + insertion_result, + proto::SensorData* proto) { + CreateSensorMetadata(sensor_id, trajectory_id, client_id, + proto->mutable_sensor_metadata()); + proto->mutable_local_slam_result_data()->set_timestamp( + common::ToUniversal(time)); + *proto->mutable_local_slam_result_data()->mutable_node_data() = + mapping::ToProto(*insertion_result.constant_data); + for (const auto& insertion_submap : insertion_result.insertion_submaps) { + // We only send the probability grid up if the submap is finished. + auto* submap = proto->mutable_local_slam_result_data()->add_submaps(); + *submap = insertion_submap->ToProto(insertion_submap->insertion_finished()); + submap->mutable_submap_id()->set_trajectory_id(trajectory_id); + submap->mutable_submap_id()->set_submap_index(starting_submap_index); + ++starting_submap_index; + } +} + +proto::SensorId ToProto( + const mapping::TrajectoryBuilderInterface::SensorId& sensor_id) { + using SensorType = mapping::TrajectoryBuilderInterface::SensorId::SensorType; + proto::SensorType type; + switch (sensor_id.type) { + case SensorType::RANGE: + type = proto::SensorType::RANGE; + break; + case SensorType::IMU: + type = proto::SensorType::IMU; + break; + case SensorType::ODOMETRY: + type = proto::SensorType::ODOMETRY; + break; + case SensorType::FIXED_FRAME_POSE: + type = proto::SensorType::FIXED_FRAME_POSE; + break; + case SensorType::LANDMARK: + type = proto::SensorType::LANDMARK; + break; + case SensorType::LOCAL_SLAM_RESULT: + type = proto::SensorType::LOCAL_SLAM_RESULT; + break; + default: + LOG(FATAL) << "unknown SensorType"; + } + proto::SensorId proto; + proto.set_type(type); + proto.set_id(sensor_id.id); + return proto; +} + +mapping::TrajectoryBuilderInterface::SensorId FromProto( + const proto::SensorId& proto) { + using SensorId = mapping::TrajectoryBuilderInterface::SensorId; + using SensorType = SensorId::SensorType; + SensorType type; + switch (proto.type()) { + case proto::SensorType::RANGE: + type = SensorType::RANGE; + break; + case proto::SensorType::IMU: + type = SensorType::IMU; + break; + case proto::SensorType::ODOMETRY: + type = SensorType::ODOMETRY; + break; + case proto::SensorType::FIXED_FRAME_POSE: + type = SensorType::FIXED_FRAME_POSE; + break; + case proto::SensorType::LANDMARK: + type = SensorType::LANDMARK; + break; + case proto::SensorType::LOCAL_SLAM_RESULT: + type = SensorType::LOCAL_SLAM_RESULT; + break; + default: + LOG(FATAL) << "unknown proto::SensorType"; + } + return SensorId{type, proto.id()}; +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/sensor/serialization.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/sensor/serialization.h new file mode 100644 index 0000000..9da7fe2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/sensor/serialization.h @@ -0,0 +1,75 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_SENSOR_SERIALIZATION_H +#define CARTOGRAPHER_CLOUD_INTERNAL_SENSOR_SERIALIZATION_H + +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" + +namespace cartographer { +namespace cloud { + +void CreateSensorMetadata(const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + proto::SensorMetadata* proto); + +void CreateAddFixedFramePoseDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::FixedFramePoseData& fixed_frame_pose_data, + proto::AddFixedFramePoseDataRequest* proto); +void CreateAddImuDataRequest(const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::ImuData& imu_data, + proto::AddImuDataRequest* proto); +void CreateAddOdometryDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::OdometryData& odometry_data, + proto::AddOdometryDataRequest* proto); +void CreateAddRangeFinderDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::TimedPointCloudData& timed_point_cloud_data, + proto::AddRangefinderDataRequest* proto); +void CreateAddLandmarkDataRequest( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, + const sensor::proto::LandmarkData& landmark_data, + proto::AddLandmarkDataRequest* proto); +void CreateSensorDataForLocalSlamResult( + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, common::Time time, int starting_submap_index, + const mapping::TrajectoryBuilderInterface::InsertionResult& + insertion_result, + proto::SensorData* proto); + +proto::SensorId ToProto( + const mapping::TrajectoryBuilderInterface::SensorId& sensor_id); +mapping::TrajectoryBuilderInterface::SensorId FromProto( + const proto::SensorId& proto); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_SENSOR_SERIALIZATION_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/handler_test.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/handler_test.h new file mode 100644 index 0000000..b114a6a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/handler_test.h @@ -0,0 +1,82 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_TESTING_HANDLER_TEST_H +#define CARTOGRAPHER_CLOUD_INTERNAL_TESTING_HANDLER_TEST_H + +#include "absl/memory/memory.h" +#include "async_grpc/testing/rpc_handler_test_server.h" +#include "cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h" +#include "cartographer/cloud/internal/testing/mock_map_builder_context.h" +#include "cartographer/mapping/internal/testing/mock_map_builder.h" +#include "cartographer/mapping/internal/testing/mock_pose_graph.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace testing { + +using ::testing::Return; +using ::testing::Test; + +template +class HandlerTest : public Test { + public: + void SetUp() override { + test_server_ = absl::make_unique< + async_grpc::testing::RpcHandlerTestServer>( + absl::make_unique()); + mock_map_builder_context_ = + test_server_ + ->template GetUnsynchronizedContext(); + mock_local_trajectory_uploader_ = + absl::make_unique(); + mock_map_builder_ = absl::make_unique(); + mock_pose_graph_ = absl::make_unique(); + + EXPECT_CALL(*mock_map_builder_context_, map_builder()) + .Times(::testing::AnyNumber()) + .WillRepeatedly(::testing::ReturnPointee(mock_map_builder_.get())); + EXPECT_CALL(*mock_map_builder_, pose_graph()) + .Times(::testing::AnyNumber()) + .WillRepeatedly(Return(mock_pose_graph_.get())); + } + + void SetNoLocalTrajectoryUploader() { + EXPECT_CALL(*mock_map_builder_context_, local_trajectory_uploader()) + .WillOnce(Return(nullptr)); + } + + void SetMockLocalTrajectoryUploader() { + EXPECT_CALL(*mock_map_builder_context_, local_trajectory_uploader()) + .WillRepeatedly(Return(mock_local_trajectory_uploader_.get())); + } + + protected: + std::unique_ptr< + async_grpc::testing::RpcHandlerTestServer> + test_server_; + MockMapBuilderContext *mock_map_builder_context_; + std::unique_ptr mock_local_trajectory_uploader_; + std::unique_ptr mock_map_builder_; + std::unique_ptr mock_pose_graph_; +}; + +} // namespace testing +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_TESTING_HANDLER_TEST_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h new file mode 100644 index 0000000..ba24cf0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h @@ -0,0 +1,50 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_LOCAL_TRAJECTORY_UPLOADER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_LOCAL_TRAJECTORY_UPLOADER_H + +#include "cartographer/cloud/internal/local_trajectory_uploader.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace testing { + +class MockLocalTrajectoryUploader : public LocalTrajectoryUploaderInterface { + public: + MOCK_METHOD1(DoEnqueueSensorData, void(proto::SensorData *)); + void EnqueueSensorData( + std::unique_ptr data_request) override { + DoEnqueueSensorData(data_request.get()); + } + MOCK_METHOD0(Start, void()); + MOCK_METHOD0(Shutdown, void()); + MOCK_METHOD4(AddTrajectory, + grpc::Status(const std::string &, int, + const std::set &, + const mapping::proto::TrajectoryBuilderOptions &)); + MOCK_METHOD2(FinishTrajectory, grpc::Status(const std::string &, int)); + MOCK_CONST_METHOD1(GetLocalSlamResultSensorId, SensorId(int)); +}; + +} // namespace testing +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_LOCAL_TRAJECTORY_UPLOADER_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/mock_map_builder_context.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/mock_map_builder_context.h new file mode 100644 index 0000000..bb0e1d7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/mock_map_builder_context.h @@ -0,0 +1,70 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_MAP_BUILDER_CONTEXT_H +#define CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_MAP_BUILDER_CONTEXT_H + +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace cloud { +namespace testing { + +class MockMapBuilderContext : public MapBuilderContextInterface { + public: + MOCK_METHOD0(map_builder, mapping::MapBuilderInterface &()); + MOCK_METHOD0( + sensor_data_queue, + common::BlockingQueue> + &()); + MOCK_METHOD0(GetLocalSlamResultCallbackForSubscriptions, + mapping::TrajectoryBuilderInterface::LocalSlamResultCallback()); + MOCK_METHOD1(AddSensorDataToTrajectory, + void(const MapBuilderContextInterface::Data &)); + MOCK_METHOD2(SubscribeLocalSlamResults, + MapBuilderContextInterface::LocalSlamSubscriptionId( + int, + MapBuilderContextInterface::LocalSlamSubscriptionCallback)); + MOCK_METHOD1( + UnsubscribeLocalSlamResults, + void(const MapBuilderContextInterface::LocalSlamSubscriptionId &)); + MOCK_METHOD1(SubscribeGlobalSlamOptimizations, + int(GlobalSlamOptimizationCallback)); + MOCK_METHOD1(UnsubscribeGlobalSlamOptimizations, void(int)); + MOCK_METHOD1(NotifyFinishTrajectory, void(int)); + MOCK_METHOD0(local_trajectory_uploader, LocalTrajectoryUploaderInterface *()); + + MOCK_METHOD2(DoEnqueueSensorData, void(int, sensor::Data *)); + void EnqueueSensorData(int trajectory_id, + std::unique_ptr data) override { + DoEnqueueSensorData(trajectory_id, data.get()); + } + MOCK_METHOD3(EnqueueLocalSlamResultData, + void(int, const std::string &, + const mapping::proto::LocalSlamResultData &)); + MOCK_METHOD2(RegisterClientIdForTrajectory, void(const std::string &, int)); + MOCK_METHOD2(CheckClientIdForTrajectory, bool(const std::string &, int)); +}; + +} // namespace testing +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_MAP_BUILDER_CONTEXT_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/test_helpers.cc b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/test_helpers.cc new file mode 100644 index 0000000..1d84c4d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/test_helpers.cc @@ -0,0 +1,101 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/internal/testing/test_helpers.h" + +namespace cartographer { +namespace cloud { +namespace testing { + +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddImuDataRequest &proto) { + return [proto](const sensor::Data &data) { + const auto *dispatchable = + dynamic_cast *>(&data); + CHECK_NOTNULL(dispatchable); + return google::protobuf::util::MessageDifferencer::Equals( + sensor::ToProto(dispatchable->data()), proto.imu_data()) && + dispatchable->GetSensorId() == proto.sensor_metadata().sensor_id(); + }; +} + +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddFixedFramePoseDataRequest &proto) { + return [proto](const sensor::Data &data) { + const auto *dispatchable = + dynamic_cast *>( + &data); + CHECK_NOTNULL(dispatchable); + return google::protobuf::util::MessageDifferencer::Equals( + sensor::ToProto(dispatchable->data()), + proto.fixed_frame_pose_data()) && + dispatchable->GetSensorId() == proto.sensor_metadata().sensor_id(); + }; +} + +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddOdometryDataRequest &proto) { + return [proto](const sensor::Data &data) { + const auto *dispatchable = + dynamic_cast *>(&data); + CHECK_NOTNULL(dispatchable); + return google::protobuf::util::MessageDifferencer::Equals( + sensor::ToProto(dispatchable->data()), proto.odometry_data()) && + dispatchable->GetSensorId() == proto.sensor_metadata().sensor_id(); + }; +} + +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddLandmarkDataRequest &proto) { + return [proto](const sensor::Data &data) { + const auto *dispatchable = + dynamic_cast *>(&data); + CHECK_NOTNULL(dispatchable); + return google::protobuf::util::MessageDifferencer::Equals( + sensor::ToProto(dispatchable->data()), proto.landmark_data()) && + dispatchable->GetSensorId() == proto.sensor_metadata().sensor_id(); + }; +} + +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddRangefinderDataRequest &proto) { + return [proto](const sensor::Data &data) { + const auto *dispatchable = + dynamic_cast *>( + &data); + CHECK_NOTNULL(dispatchable); + return google::protobuf::util::MessageDifferencer::Equals( + sensor::ToProto(dispatchable->data()), + proto.timed_point_cloud_data()) && + dispatchable->GetSensorId() == proto.sensor_metadata().sensor_id(); + }; +} + +ProtoPredicateType BuildProtoPredicateEquals( + const google::protobuf::Message *proto) { + return [proto](const google::protobuf::Message &message) { + return google::protobuf::util::MessageDifferencer::Equals(*proto, message); + }; +} + +} // namespace testing +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/test_helpers.h b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/test_helpers.h new file mode 100644 index 0000000..cac0a52 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/internal/testing/test_helpers.h @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_INTERNAL_TESTING_TEST_HELPERS_H +#define CARTOGRAPHER_CLOUD_INTERNAL_TESTING_TEST_HELPERS_H + +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "google/protobuf/util/message_differencer.h" + +namespace cartographer { +namespace cloud { +namespace testing { + +using DataPredicateType = std::function; +using ProtoPredicateType = + std::function; + +template +DataPredicateType BuildDataPredicateEquals(const T &proto); + +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddImuDataRequest &proto); +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddFixedFramePoseDataRequest &proto); +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddOdometryDataRequest &proto); +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddLandmarkDataRequest &proto); +template <> +DataPredicateType BuildDataPredicateEquals( + const proto::AddRangefinderDataRequest &proto); + +ProtoPredicateType BuildProtoPredicateEquals( + const google::protobuf::Message *proto); + +} // namespace testing +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_TESTING_TEST_HELPERS_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_interface.cc b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_interface.cc new file mode 100644 index 0000000..07e204b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_interface.cc @@ -0,0 +1,21 @@ +#include "cartographer/cloud/map_builder_server_interface.h" + +#include "absl/memory/memory.h" +#include "cartographer/cloud/internal/map_builder_server.h" + +namespace cartographer { +namespace cloud { + +void RegisterMapBuilderServerMetrics(metrics::FamilyFactory* factory) { + MapBuilderServer::RegisterMetrics(factory); +} + +std::unique_ptr CreateMapBuilderServer( + const proto::MapBuilderServerOptions& map_builder_server_options, + std::unique_ptr map_builder) { + return absl::make_unique(map_builder_server_options, + std::move(map_builder)); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_interface.h b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_interface.h new file mode 100644 index 0000000..bf0d971 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_interface.h @@ -0,0 +1,60 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_MAP_BUILDER_SERVER_INTERFACE_H +#define CARTOGRAPHER_CLOUD_MAP_BUILDER_SERVER_INTERFACE_H + +#include + +#include "cartographer/cloud/proto/map_builder_server_options.pb.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/metrics/family_factory.h" + +namespace cartographer { +namespace cloud { + +class MapBuilderServerInterface { + public: + virtual ~MapBuilderServerInterface() {} + + // Starts the gRPC server, the 'LocalTrajectoryUploader' and the SLAM thread. + virtual void Start() = 0; + + // Waits for the 'MapBuilderServer' to shut down. Note: The server must be + // either shutting down or some other thread must call 'Shutdown()' for + // this function to ever return. + virtual void WaitForShutdown() = 0; + + // Waits until all computation is finished (for testing). + virtual void WaitUntilIdle() = 0; + + // Shuts down the gRPC server, the 'LocalTrajectoryUploader' and the SLAM + // thread. + virtual void Shutdown() = 0; +}; + +// Registers all metrics for the MapBuilderServer. +void RegisterMapBuilderServerMetrics(metrics::FamilyFactory* factory); + +// Returns MapBuilderServer with the actual implementation. +std::unique_ptr CreateMapBuilderServer( + const proto::MapBuilderServerOptions& map_builder_server_options, + std::unique_ptr map_builder); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_MAP_BUILDER_SERVER_INTERFACE_H diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_main.cc b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_main.cc new file mode 100644 index 0000000..81bf87e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_main.cc @@ -0,0 +1,79 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/cloud/map_builder_server_interface.h" +#include "cartographer/cloud/map_builder_server_options.h" +#include "cartographer/mapping/map_builder.h" +#include "cartographer/metrics/register.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#if USE_PROMETHEUS +#include "cartographer/cloud/metrics/prometheus/family_factory.h" +#include "prometheus/exposer.h" +#endif + +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); + +namespace cartographer { +namespace cloud { + +void Run(const std::string& configuration_directory, + const std::string& configuration_basename) { +#if USE_PROMETHEUS + metrics::prometheus::FamilyFactory registry; + ::cartographer::metrics::RegisterAllMetrics(®istry); + RegisterMapBuilderServerMetrics(®istry); + ::prometheus::Exposer exposer("0.0.0.0:9100"); + exposer.RegisterCollectable(registry.GetCollectable()); + LOG(INFO) << "Exposing metrics at http://localhost:9100/metrics"; +#endif + + proto::MapBuilderServerOptions map_builder_server_options = + LoadMapBuilderServerOptions(configuration_directory, + configuration_basename); + auto map_builder = mapping::CreateMapBuilder( + map_builder_server_options.map_builder_options()); + std::unique_ptr map_builder_server = + CreateMapBuilderServer(map_builder_server_options, + std::move(map_builder)); + map_builder_server->Start(); + map_builder_server->WaitForShutdown(); +} + +} // namespace cloud +} // namespace cartographer + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + google::SetUsageMessage( + "\n\n" + "This program offers a MapBuilder service via a gRPC interface.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + if (FLAGS_configuration_directory.empty() || + FLAGS_configuration_basename.empty()) { + google::ShowUsageWithFlagsRestrict(argv[0], "map_builder_server"); + return EXIT_FAILURE; + } + cartographer::cloud::Run(FLAGS_configuration_directory, + FLAGS_configuration_basename); +} diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_options.cc b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_options.cc new file mode 100644 index 0000000..84042f3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_options.cc @@ -0,0 +1,62 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/cloud/map_builder_server_options.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/mapping/map_builder_interface.h" + +namespace cartographer { +namespace cloud { + +proto::MapBuilderServerOptions CreateMapBuilderServerOptions( + common::LuaParameterDictionary* lua_parameter_dictionary) { + proto::MapBuilderServerOptions map_builder_server_options; + map_builder_server_options.set_server_address( + lua_parameter_dictionary->GetString("server_address")); + map_builder_server_options.set_num_grpc_threads( + lua_parameter_dictionary->GetInt("num_grpc_threads")); + map_builder_server_options.set_num_event_threads( + lua_parameter_dictionary->GetInt("num_event_threads")); + *map_builder_server_options.mutable_map_builder_options() = + mapping::CreateMapBuilderOptions( + lua_parameter_dictionary->GetDictionary("map_builder").get()); + map_builder_server_options.set_uplink_server_address( + lua_parameter_dictionary->GetString("uplink_server_address")); + map_builder_server_options.set_upload_batch_size( + lua_parameter_dictionary->GetInt("upload_batch_size")); + map_builder_server_options.set_enable_ssl_encryption( + lua_parameter_dictionary->GetBool("enable_ssl_encryption")); + map_builder_server_options.set_enable_google_auth( + lua_parameter_dictionary->GetBool("enable_google_auth")); + return map_builder_server_options; +} + +proto::MapBuilderServerOptions LoadMapBuilderServerOptions( + const std::string& configuration_directory, + const std::string& configuration_basename) { + auto file_resolver = absl::make_unique( + std::vector{configuration_directory}); + const std::string code = + file_resolver->GetFileContentOrDie(configuration_basename); + common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver)); + return CreateMapBuilderServerOptions(&lua_parameter_dictionary); +} + +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_options.h b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_options.h new file mode 100644 index 0000000..838ceae --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/map_builder_server_options.h @@ -0,0 +1,38 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_CLOUD_MAP_BUILDER_SERVER_OPTIONS_H_ +#define CARTOGRAPHER_CLOUD_MAP_BUILDER_SERVER_OPTIONS_H_ + +#include + +#include "cartographer/cloud/proto/map_builder_server_options.pb.h" +#include "cartographer/common/lua_parameter_dictionary.h" + +namespace cartographer { +namespace cloud { + +proto::MapBuilderServerOptions CreateMapBuilderServerOptions( + common::LuaParameterDictionary* lua_parameter_dictionary); + +proto::MapBuilderServerOptions LoadMapBuilderServerOptions( + const std::string& configuration_directory, + const std::string& configuration_basename); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_MAP_BUILDER_SERVER_OPTIONS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/family_factory.cc b/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/family_factory.cc new file mode 100644 index 0000000..d9e1329 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/family_factory.cc @@ -0,0 +1,196 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/metrics/prometheus/family_factory.h" + +#include "absl/container/flat_hash_map.h" +#include "absl/memory/memory.h" +#include "prometheus/counter.h" +#include "prometheus/family.h" +#include "prometheus/gauge.h" +#include "prometheus/histogram.h" + +namespace cartographer { +namespace cloud { +namespace metrics { +namespace prometheus { +namespace { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +// Creates or looks up already existing objects from a wrapper map. +template +Wrapper* GetOrCreateWrapper(ObjectPtr object_ptr, WrapperMap* wrapper_map, + std::mutex* wrapper_mutex) { + std::lock_guard lock(*wrapper_mutex); + auto wrappers_itr = wrapper_map->find(object_ptr); + if (wrappers_itr == wrapper_map->end()) { + auto wrapper = absl::make_unique(object_ptr); + auto* ptr = wrapper.get(); + (*wrapper_map)[object_ptr] = std::unique_ptr(std::move(wrapper)); + return ptr; + } + return wrappers_itr->second.get(); +} + +class Counter : public ::cartographer::metrics::Counter { + public: + explicit Counter(::prometheus::Counter* prometheus) + : prometheus_(prometheus) {} + + void Increment() override { prometheus_->Increment(); } + void Increment(double by_value) override { prometheus_->Increment(by_value); } + + private: + ::prometheus::Counter* prometheus_; +}; + +class CounterFamily + : public ::cartographer::metrics::Family<::cartographer::metrics::Counter> { + public: + explicit CounterFamily( + ::prometheus::Family<::prometheus::Counter>* prometheus) + : prometheus_(prometheus) {} + + Counter* Add(const std::map& labels) override { + ::prometheus::Counter* counter = &prometheus_->Add(labels); + return GetOrCreateWrapper<>(counter, &wrappers_, &wrappers_mutex_); + } + + private: + ::prometheus::Family<::prometheus::Counter>* prometheus_; + std::mutex wrappers_mutex_; + absl::flat_hash_map<::prometheus::Counter*, std::unique_ptr> + wrappers_; +}; + +class Gauge : public ::cartographer::metrics::Gauge { + public: + explicit Gauge(::prometheus::Gauge* prometheus) : prometheus_(prometheus) {} + + void Decrement() override { prometheus_->Decrement(); } + void Decrement(double by_value) override { prometheus_->Decrement(by_value); } + void Increment() override { prometheus_->Increment(); } + void Increment(double by_value) override { prometheus_->Increment(by_value); } + void Set(double value) override { prometheus_->Set(value); } + + private: + ::prometheus::Gauge* prometheus_; +}; + +class GaugeFamily + : public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> { + public: + explicit GaugeFamily(::prometheus::Family<::prometheus::Gauge>* prometheus) + : prometheus_(prometheus) {} + + Gauge* Add(const std::map& labels) override { + ::prometheus::Gauge* gauge = &prometheus_->Add(labels); + return GetOrCreateWrapper<>(gauge, &wrappers_, &wrappers_mutex_); + } + + private: + ::prometheus::Family<::prometheus::Gauge>* prometheus_; + std::mutex wrappers_mutex_; + absl::flat_hash_map<::prometheus::Gauge*, std::unique_ptr> wrappers_; +}; + +class Histogram : public ::cartographer::metrics::Histogram { + public: + explicit Histogram(::prometheus::Histogram* prometheus) + : prometheus_(prometheus) {} + + void Observe(double value) override { prometheus_->Observe(value); } + + private: + ::prometheus::Histogram* prometheus_; +}; + +class HistogramFamily : public ::cartographer::metrics::Family< + ::cartographer::metrics::Histogram> { + public: + HistogramFamily(::prometheus::Family<::prometheus::Histogram>* prometheus, + const BucketBoundaries& boundaries) + : prometheus_(prometheus), boundaries_(boundaries) {} + + Histogram* Add(const std::map& labels) override { + ::prometheus::Histogram* histogram = &prometheus_->Add(labels, boundaries_); + return GetOrCreateWrapper<>(histogram, &wrappers_, &wrappers_mutex_); + } + + private: + ::prometheus::Family<::prometheus::Histogram>* prometheus_; + std::mutex wrappers_mutex_; + absl::flat_hash_map<::prometheus::Histogram*, std::unique_ptr> + wrappers_; + const BucketBoundaries boundaries_; +}; + +} // namespace + +FamilyFactory::FamilyFactory() + : registry_(std::make_shared<::prometheus::Registry>()) {} + +::cartographer::metrics::Family<::cartographer::metrics::Counter>* +FamilyFactory::NewCounterFamily(const std::string& name, + const std::string& description) { + auto& family = ::prometheus::BuildCounter() + .Name(name) + .Help(description) + .Register(*registry_); + auto wrapper = absl::make_unique(&family); + auto* ptr = wrapper.get(); + counters_.emplace_back(std::move(wrapper)); + return ptr; +} + +::cartographer::metrics::Family<::cartographer::metrics::Gauge>* +FamilyFactory::NewGaugeFamily(const std::string& name, + const std::string& description) { + auto& family = ::prometheus::BuildGauge() + .Name(name) + .Help(description) + .Register(*registry_); + auto wrapper = absl::make_unique(&family); + auto* ptr = wrapper.get(); + gauges_.emplace_back(std::move(wrapper)); + return ptr; +} + +::cartographer::metrics::Family<::cartographer::metrics::Histogram>* +FamilyFactory::NewHistogramFamily(const std::string& name, + const std::string& description, + const BucketBoundaries& boundaries) { + auto& family = ::prometheus::BuildHistogram() + .Name(name) + .Help(description) + .Register(*registry_); + auto wrapper = absl::make_unique(&family, boundaries); + auto* ptr = wrapper.get(); + histograms_.emplace_back(std::move(wrapper)); + return ptr; +} + +std::weak_ptr<::prometheus::Collectable> FamilyFactory::GetCollectable() const { + return registry_; +} + +} // namespace prometheus +} // namespace metrics +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/family_factory.h b/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/family_factory.h new file mode 100644 index 0000000..dea1275 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/family_factory.h @@ -0,0 +1,66 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_CLOUD_METRICS_PROMETHEUS_FAMILY_FACTORY_H_ +#define CARTOGRAPHER_CLOUD_METRICS_PROMETHEUS_FAMILY_FACTORY_H_ + +#include +#include + +#include "cartographer/metrics/family_factory.h" +#include "prometheus/registry.h" + +namespace cartographer { +namespace cloud { +namespace metrics { +namespace prometheus { + +class FamilyFactory : public ::cartographer::metrics::FamilyFactory { + public: + FamilyFactory(); + + ::cartographer::metrics::Family<::cartographer::metrics::Counter>* + NewCounterFamily(const std::string& name, + const std::string& description) override; + ::cartographer::metrics::Family<::cartographer::metrics::Gauge>* + NewGaugeFamily(const std::string& name, + const std::string& description) override; + ::cartographer::metrics::Family<::cartographer::metrics::Histogram>* + NewHistogramFamily(const std::string& name, const std::string& description, + const ::cartographer::metrics::Histogram::BucketBoundaries& + boundaries) override; + + std::weak_ptr<::prometheus::Collectable> GetCollectable() const; + + private: + std::vector>> + counters_; + std::vector>> + gauges_; + std::vector>> + histograms_; + std::shared_ptr<::prometheus::Registry> registry_; +}; + +} // namespace prometheus +} // namespace metrics +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_METRICS_PROMETHEUS_FAMILY_FACTORY_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/metrics_test.cc b/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/metrics_test.cc new file mode 100644 index 0000000..a285cab --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/metrics/prometheus/metrics_test.cc @@ -0,0 +1,148 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/metrics/prometheus/family_factory.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/metrics/register.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "prometheus/exposer.h" +#include "prometheus/metric_family.h" + +namespace cartographer { +namespace cloud { +namespace metrics { +namespace prometheus { +namespace { + +using Label = ::prometheus::ClientMetric::Label; + +static auto* kCounter = ::cartographer::metrics::Counter::Null(); +static auto* kGauge = ::cartographer::metrics::Gauge::Null(); +static auto* kScoresMetric = ::cartographer::metrics::Histogram::Null(); + +const char kLabelKey[] = "kind"; +const char kLabelValue[] = "score"; +const std::array kObserveScores = {{-1, 0.11, 0.2, 0.5, 2}}; + +class Algorithm { + public: + static void RegisterMetrics(::cartographer::metrics::FamilyFactory* factory) { + auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(0.05, 20); + auto* scores_family = factory->NewHistogramFamily( + "/algorithm/scores", "Scores achieved", boundaries); + kScoresMetric = scores_family->Add({{kLabelKey, kLabelValue}}); + } + void Run() { + for (double score : kObserveScores) { + kScoresMetric->Observe(score); + } + } +}; + +TEST(MetricsTest, CollectCounter) { + FamilyFactory factory; + auto* counter_family = factory.NewCounterFamily("/test/hits", "Hits"); + kCounter = counter_family->Add({{kLabelKey, kLabelValue}}); + kCounter->Increment(); + kCounter->Increment(5); + double expected_value = 1 + 5; + std::vector<::prometheus::MetricFamily> collected; + { + std::shared_ptr<::prometheus::Collectable> collectable; + CHECK(collectable = factory.GetCollectable().lock()); + collected = collectable->Collect(); + } + ASSERT_EQ(collected.size(), 1); + ASSERT_EQ(collected[0].metric.size(), 1); + EXPECT_THAT( + collected[0].metric.at(0).label, + testing::AllOf( + testing::ElementsAre(testing::Field(&Label::name, kLabelKey)), + testing::ElementsAre(testing::Field(&Label::value, kLabelValue)))); + EXPECT_THAT(collected[0].metric.at(0).counter.value, + testing::DoubleEq(expected_value)); +} + +TEST(MetricsTest, CollectGauge) { + FamilyFactory factory; + auto* gauge_family = + factory.NewGaugeFamily("/test/queue/length", "Length of some queue"); + kGauge = gauge_family->Add({{kLabelKey, kLabelValue}}); + kGauge->Increment(); + kGauge->Increment(5); + kGauge->Decrement(); + kGauge->Decrement(2); + double expected_value = 1 + 5 - 1 - 2; + std::vector<::prometheus::MetricFamily> collected; + { + std::shared_ptr<::prometheus::Collectable> collectable; + CHECK(collectable = factory.GetCollectable().lock()); + collected = collectable->Collect(); + } + ASSERT_EQ(collected.size(), 1); + ASSERT_EQ(collected[0].metric.size(), 1); + EXPECT_THAT( + collected[0].metric.at(0).label, + testing::AllOf( + testing::ElementsAre(testing::Field(&Label::name, kLabelKey)), + testing::ElementsAre(testing::Field(&Label::value, kLabelValue)))); + EXPECT_THAT(collected[0].metric.at(0).gauge.value, + testing::DoubleEq(expected_value)); +} + +TEST(MetricsTest, CollectHistogram) { + FamilyFactory registry; + Algorithm::RegisterMetrics(®istry); + + Algorithm algorithm; + algorithm.Run(); + std::vector<::prometheus::MetricFamily> collected; + { + std::shared_ptr<::prometheus::Collectable> collectable; + CHECK(collectable = registry.GetCollectable().lock()); + collected = collectable->Collect(); + } + ASSERT_EQ(collected.size(), 1); + ASSERT_EQ(collected[0].metric.size(), 1); + EXPECT_THAT( + collected[0].metric.at(0).label, + testing::AllOf( + testing::ElementsAre(testing::Field(&Label::name, kLabelKey)), + testing::ElementsAre(testing::Field(&Label::value, kLabelValue)))); + EXPECT_THAT(collected[0].metric.at(0).histogram.sample_count, + testing::Eq(kObserveScores.size())); + EXPECT_EQ(collected[0].metric.at(0).histogram.bucket.at(0).cumulative_count, + 1); +} + +TEST(MetricsTest, RunExposerServer) { + FamilyFactory registry; + Algorithm::RegisterMetrics(®istry); + ::cartographer::metrics::RegisterAllMetrics(®istry); + ::prometheus::Exposer exposer("0.0.0.0:9100"); + exposer.RegisterCollectable(registry.GetCollectable()); + + Algorithm algorithm; + algorithm.Run(); +} + +} // namespace +} // namespace prometheus +} // namespace metrics +} // namespace cloud +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/proto/map_builder_server_options.proto b/carto_ws/src/cartographer-master/cartographer/cloud/proto/map_builder_server_options.proto new file mode 100644 index 0000000..747ee8a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/proto/map_builder_server_options.proto @@ -0,0 +1,30 @@ +// Copyright 2017 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +import "cartographer/mapping/proto/map_builder_options.proto"; + +package cartographer.cloud.proto; + +message MapBuilderServerOptions { + string server_address = 1; + int32 num_grpc_threads = 2; + int32 num_event_threads = 3; + cartographer.mapping.proto.MapBuilderOptions map_builder_options = 4; + string uplink_server_address = 5; + int32 upload_batch_size = 6; + bool enable_ssl_encryption = 7; + bool enable_google_auth = 9; +} diff --git a/carto_ws/src/cartographer-master/cartographer/cloud/proto/map_builder_service.proto b/carto_ws/src/cartographer-master/cartographer/cloud/proto/map_builder_service.proto new file mode 100644 index 0000000..4938e3f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/cloud/proto/map_builder_service.proto @@ -0,0 +1,354 @@ +// Copyright 2017 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +import "cartographer/mapping/proto/pose_graph.proto"; +import "cartographer/mapping/proto/serialization.proto"; +import "cartographer/mapping/proto/submap_visualization.proto"; +import "cartographer/mapping/proto/trajectory_builder_options.proto"; +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/transform.proto"; +import "google/protobuf/empty.proto"; + +package cartographer.cloud.proto; + +enum SensorType { + RANGE = 0; + IMU = 1; + ODOMETRY = 2; + FIXED_FRAME_POSE = 3; + LANDMARK = 4; + LOCAL_SLAM_RESULT = 5; +} + +enum TrajectoryState { + ACTIVE = 0; + FINISHED = 1; + FROZEN = 2; + DELETED = 3; +} + +message SensorId { + string id = 1; + SensorType type = 2; +} + +message AddTrajectoryRequest { + repeated SensorId expected_sensor_ids = 3; + cartographer.mapping.proto.TrajectoryBuilderOptions + trajectory_builder_options = 2; + string client_id = 4; +} + +message SensorMetadata { + int32 trajectory_id = 1; + string sensor_id = 2; + string client_id = 3; +} + +message SensorData { + SensorMetadata sensor_metadata = 1; + oneof sensor_data { + cartographer.sensor.proto.OdometryData odometry_data = 2; + cartographer.sensor.proto.ImuData imu_data = 3; + cartographer.sensor.proto.TimedPointCloudData timed_point_cloud_data = 4; + cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 5; + cartographer.sensor.proto.LandmarkData landmark_data = 6; + cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 7; + } +} + +message AddTrajectoryResponse { + int32 trajectory_id = 1; +} + +message AddSensorDataBatchRequest { + repeated SensorData sensor_data = 1; +} + +message AddOdometryDataRequest { + SensorMetadata sensor_metadata = 1; + cartographer.sensor.proto.OdometryData odometry_data = 2; +} + +message AddImuDataRequest { + SensorMetadata sensor_metadata = 1; + cartographer.sensor.proto.ImuData imu_data = 2; +} + +message AddRangefinderDataRequest { + SensorMetadata sensor_metadata = 1; + cartographer.sensor.proto.TimedPointCloudData timed_point_cloud_data = 2; +} + +message AddFixedFramePoseDataRequest { + SensorMetadata sensor_metadata = 1; + cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2; +} + +message AddLandmarkDataRequest { + SensorMetadata sensor_metadata = 1; + cartographer.sensor.proto.LandmarkData landmark_data = 2; +} + +message FinishTrajectoryRequest { + int32 trajectory_id = 1; + string client_id = 2; +} + +message DeleteTrajectoryRequest { + int32 trajectory_id = 1; + string client_id = 2; +} + +message ReceiveLocalSlamResultsRequest { + int32 trajectory_id = 1; +} + +message LocalSlamInsertionResult { + cartographer.mapping.proto.NodeId node_id = 1; +} + +message ReceiveLocalSlamResultsResponse { + int32 trajectory_id = 1; + int64 timestamp = 2; + cartographer.transform.proto.Rigid3d local_pose = 3; + cartographer.sensor.proto.RangeData range_data = 4; + LocalSlamInsertionResult insertion_result = 5; +} + +message ReceiveGlobalSlamOptimizationsResponse { + map + last_optimized_node_ids = 1; + map + last_optimized_submap_ids = 2; +} + +message GetSubmapRequest { + cartographer.mapping.proto.SubmapId submap_id = 1; +} + +message LoadStateRequest { + oneof state_chunk { + cartographer.mapping.proto.SerializedData serialized_data = 1; + cartographer.mapping.proto.SerializationHeader serialization_header = 2; + string client_id = 3; + } + bool load_frozen_state = 4; +} + +message TrajectoryRemapping { + map + serialized_trajectories_to_trajectories = 1; +} + +message LoadStateResponse { + TrajectoryRemapping trajectory_remapping = 1; +} + +message LoadStateFromFileRequest { + string file_path = 1; + string client_id = 2; + bool load_frozen_state = 3; +} + +message LoadStateFromFileResponse { + TrajectoryRemapping trajectory_remapping = 1; +} + +message GetSubmapResponse { + cartographer.mapping.proto.SubmapQuery.Response submap_query_response = 1; + string error_msg = 2; +} + +message TrajectoryNodePose { + message ConstantPoseData { + int64 timestamp = 1; + cartographer.transform.proto.Rigid3d local_pose = 2; + } + cartographer.mapping.proto.NodeId node_id = 1; + cartographer.transform.proto.Rigid3d global_pose = 2; + ConstantPoseData constant_pose_data = 3; +} + +message GetTrajectoryNodePosesResponse { + repeated TrajectoryNodePose node_poses = 1; +} + +message GetTrajectoryStatesResponse { + map trajectories_state = 1; +} + +message GetLandmarkPosesResponse { + repeated cartographer.mapping.proto.PoseGraph.LandmarkPose landmark_poses = 1; +} + +message SetLandmarkPoseRequest { + cartographer.mapping.proto.PoseGraph.LandmarkPose landmark_pose = 1; +} + +message SubmapPose { + cartographer.mapping.proto.SubmapId submap_id = 1; + int32 submap_version = 2; + cartographer.transform.proto.Rigid3d global_pose = 3; +} + +message GetAllSubmapPosesResponse { + repeated SubmapPose submap_poses = 1; +} + +message GetLocalToGlobalTransformRequest { + int32 trajectory_id = 1; +} + +message GetLocalToGlobalTransformResponse { + cartographer.transform.proto.Rigid3d local_to_global = 1; +} + +message GetConstraintsResponse { + repeated cartographer.mapping.proto.PoseGraph.Constraint constraints = 1; +} + +message WriteStateResponse { + oneof state_chunk { + cartographer.mapping.proto.SerializationHeader header = 1; + cartographer.mapping.proto.SerializedData serialized_data = 2; + } +} + +message WriteStateToFileRequest { + string filename = 1; +} + +message WriteStateToFileResponse { + bool success = 1; +} + +message IsTrajectoryFinishedRequest { + int32 trajectory_id = 1; +} + +message IsTrajectoryFinishedResponse { + bool is_finished = 1; +} + +message IsTrajectoryFrozenRequest { + int32 trajectory_id = 1; +} + +message IsTrajectoryFrozenResponse { + bool is_frozen = 1; +} + +service MapBuilderService { + // Starts a new trajectory and returns its index. + rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse); + + // Adds a batch of sensor data to a trajectory. + rpc AddSensorDataBatch(AddSensorDataBatchRequest) + returns (google.protobuf.Empty); + + // Adds odometry data from the sensor with id 'sensor_metadata.sensor_id' to + // the trajectory corresponding to 'sensor_metadata.trajectory_id'. + rpc AddOdometryData(stream AddOdometryDataRequest) + returns (google.protobuf.Empty); + + // Same for IMU data. + rpc AddImuData(stream AddImuDataRequest) returns (google.protobuf.Empty); + + // Same for range-finder data. + rpc AddRangefinderData(stream AddRangefinderDataRequest) + returns (google.protobuf.Empty); + + // Same for fixed-frame pose data. + rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest) + returns (google.protobuf.Empty); + + // Same for landmark data. + rpc AddLandmarkData(stream AddLandmarkDataRequest) + returns (google.protobuf.Empty); + + // Requests the server to send a stream of local SLAM results for the given + // 'trajectory_id'. + rpc ReceiveLocalSlamResults(ReceiveLocalSlamResultsRequest) + returns (stream ReceiveLocalSlamResultsResponse); + + // Requests the server to send a stream of global SLAM notifications. + rpc ReceiveGlobalSlamOptimizations(google.protobuf.Empty) + returns (stream ReceiveGlobalSlamOptimizationsResponse); + + // Marks a trajectory corresponding to 'trajectory_id' as finished, + // i.e. no further sensor data is expected. + rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty); + + // Deletes a trajectory asynchronously. + rpc DeleteTrajectory(DeleteTrajectoryRequest) returns (google.protobuf.Empty); + + // Retrieves a single submap. + rpc GetSubmap(GetSubmapRequest) returns (GetSubmapResponse); + + // Returns the current optimized trajectory poses. + rpc GetTrajectoryNodePoses(google.protobuf.Empty) + returns (GetTrajectoryNodePosesResponse); + + // Returns the states of trajectories. + rpc GetTrajectoryStates(google.protobuf.Empty) + returns (GetTrajectoryStatesResponse); + + // Returns the current optimized landmark poses. + rpc GetLandmarkPoses(google.protobuf.Empty) + returns (GetLandmarkPosesResponse); + + // Returns the current optimized submap poses. + rpc GetAllSubmapPoses(google.protobuf.Empty) + returns (GetAllSubmapPosesResponse); + + // Returns the current local-to-global transform for the trajectory. + rpc GetLocalToGlobalTransform(GetLocalToGlobalTransformRequest) + returns (GetLocalToGlobalTransformResponse); + + // Returns the list of constraints in the current optimization problem. + rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse); + + // Sets a pose for a landmark. + rpc SetLandmarkPose(SetLandmarkPoseRequest) returns (google.protobuf.Empty); + + // Checks whether the trajectory is finished. + rpc IsTrajectoryFinished(IsTrajectoryFinishedRequest) + returns (IsTrajectoryFinishedResponse); + + // Checks whether the trajectory is frozen. + rpc IsTrajectoryFrozen(IsTrajectoryFrozenRequest) + returns (IsTrajectoryFrozenResponse); + + // Requests a PoseGraph to call RunFinalOptimization. + rpc RunFinalOptimization(google.protobuf.Empty) + returns (google.protobuf.Empty); + + // Adds serialized SLAM state data in the order defined by ProtoStreamReader. + rpc LoadState(stream LoadStateRequest) returns (LoadStateResponse); + + // Loads a serialized SLAM state data from the host file system. + rpc LoadStateFromFile(LoadStateFromFileRequest) + returns (LoadStateFromFileResponse); + + // Receives serialized SLAM state data in the order defined by + // ProtoStreamWriter. + rpc WriteState(google.protobuf.Empty) returns (stream WriteStateResponse); + + // Writes the serialized SLAM state to the host file system. + rpc WriteStateToFile(WriteStateToFileRequest) + returns (WriteStateToFileResponse); +} diff --git a/carto_ws/src/cartographer-master/cartographer/common/config.h.cmake b/carto_ws/src/cartographer-master/cartographer/common/config.h.cmake new file mode 100644 index 0000000..5089306 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/config.h.cmake @@ -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. + */ + +#ifndef CARTOGRAPHER_COMMON_CONFIG_H_ +#define CARTOGRAPHER_COMMON_CONFIG_H_ + +namespace cartographer { +namespace common { + +constexpr char kConfigurationFilesDirectory[] = + "@CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY@"; +constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@"; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_CONFIG_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/configuration_file_resolver.cc b/carto_ws/src/cartographer-master/cartographer/common/configuration_file_resolver.cc new file mode 100644 index 0000000..3c2fd10 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/configuration_file_resolver.cc @@ -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. + */ + +#include "cartographer/common/configuration_file_resolver.h" + +#include +#include +#include + +#include "cartographer/common/config.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +ConfigurationFileResolver::ConfigurationFileResolver( + const std::vector& configuration_files_directories) + : configuration_files_directories_(configuration_files_directories) { + configuration_files_directories_.push_back(kConfigurationFilesDirectory); +} + +std::string ConfigurationFileResolver::GetFullPathOrDie( + const std::string& basename) { + for (const auto& path : configuration_files_directories_) { + const std::string filename = path + "/" + basename; + std::ifstream stream(filename.c_str()); + if (stream.good()) { + LOG(INFO) << "Found '" << filename << "' for '" << basename << "'."; + return filename; + } + } + LOG(FATAL) << "File '" << basename << "' was not found."; +} + +std::string ConfigurationFileResolver::GetFileContentOrDie( + const std::string& basename) { + CHECK(!basename.empty()) << "File basename cannot be empty." << basename; + const std::string filename = GetFullPathOrDie(basename); + std::ifstream stream(filename.c_str()); + return std::string((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/configuration_file_resolver.h b/carto_ws/src/cartographer-master/cartographer/common/configuration_file_resolver.h new file mode 100644 index 0000000..5005cdf --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/configuration_file_resolver.h @@ -0,0 +1,49 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ +#define CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" + +namespace cartographer { +namespace common { + +// A 'FileResolver' for the 'LuaParameterDictionary' that reads files from disk. +// It searches the 'configuration_files_directories' in order to find the +// requested filename. The last place searched is always the +// 'configuration_files/' directory installed with Cartographer. It contains +// reasonable configuration for the various Cartographer components which +// provide a good starting ground for new platforms. +class ConfigurationFileResolver : public FileResolver { + public: + explicit ConfigurationFileResolver( + const std::vector& configuration_files_directories); + + std::string GetFullPathOrDie(const std::string& basename) override; + std::string GetFileContentOrDie(const std::string& basename) override; + + private: + std::vector configuration_files_directories_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/configuration_files_test.cc b/carto_ws/src/cartographer-master/cartographer/common/configuration_files_test.cc new file mode 100644 index 0000000..d3c451a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/configuration_files_test.cc @@ -0,0 +1,65 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer/common/config.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace { + +TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) { + const std::string kCode = R"text( + include "map_builder.lua" + MAP_BUILDER.use_trajectory_builder_2d = true + return MAP_BUILDER)text"; + EXPECT_NO_FATAL_FAILURE({ + auto file_resolver = + ::absl::make_unique< ::cartographer::common::ConfigurationFileResolver>( + std::vector{ + std::string(::cartographer::common::kSourceDirectory) + + "/configuration_files"}); + ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + kCode, std::move(file_resolver)); + ::cartographer::mapping::CreateMapBuilderOptions(&lua_parameter_dictionary); + }); +} + +TEST(ConfigurationFilesTest, ValidateTrajectoryBuilderOptions) { + const std::string kCode = R"text( + include "trajectory_builder.lua" + TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false + return TRAJECTORY_BUILDER)text"; + EXPECT_NO_FATAL_FAILURE({ + auto file_resolver = + ::absl::make_unique< ::cartographer::common::ConfigurationFileResolver>( + std::vector{ + std::string(::cartographer::common::kSourceDirectory) + + "/configuration_files"}); + ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + kCode, std::move(file_resolver)); + ::cartographer::mapping::CreateTrajectoryBuilderOptions( + &lua_parameter_dictionary); + }); +} + +} // namespace +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler.cc b/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler.cc new file mode 100644 index 0000000..690854d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler.cc @@ -0,0 +1,47 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/fixed_ratio_sampler.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) { + CHECK_GE(ratio, 0.); + LOG_IF(WARNING, ratio == 0.) << "FixedRatioSampler is dropping all data."; + CHECK_LE(ratio, 1.); +} + +FixedRatioSampler::~FixedRatioSampler() {} + +bool FixedRatioSampler::Pulse() { + ++num_pulses_; + if (static_cast(num_samples_) / num_pulses_ < ratio_) { + ++num_samples_; + return true; + } + return false; +} + +std::string FixedRatioSampler::DebugString() { + return std::to_string(num_samples_) + " (" + + std::to_string(100. * num_samples_ / num_pulses_) + "%)"; +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler.h b/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler.h new file mode 100644 index 0000000..3e00f9e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler.h @@ -0,0 +1,55 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ +#define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ + +#include + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace common { + +// Signals when a sample should be taken from a stream of data to select a +// uniformly distributed fraction of the data. +class FixedRatioSampler { + public: + explicit FixedRatioSampler(double ratio); + ~FixedRatioSampler(); + + FixedRatioSampler(const FixedRatioSampler&) = delete; + FixedRatioSampler& operator=(const FixedRatioSampler&) = delete; + + // Returns true if this pulse should result in an sample. + bool Pulse(); + + // Returns a debug string describing the current ratio of samples to pulses. + std::string DebugString(); + + private: + // Sampling occurs if the proportion of samples to pulses drops below this + // number. + const double ratio_; + + int64 num_pulses_ = 0; + int64 num_samples_ = 0; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler_test.cc b/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler_test.cc new file mode 100644 index 0000000..ccd716f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/fixed_ratio_sampler_test.cc @@ -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. + */ + +#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, NonSensicalRatio) { + EXPECT_DEATH(FixedRatioSampler(2.), "ratio"); + EXPECT_DEATH(FixedRatioSampler(-0.1), "ratio"); +} + +TEST(FixedRatioSamplerTest, SometimesTrue) { + FixedRatioSampler fixed_ratio_sampler(0.5); + for (int i = 0; i < 100; ++i) { + EXPECT_EQ(i % 2 == 0, fixed_ratio_sampler.Pulse()); + } +} + +TEST(FixedRatioSamplerTest, FirstPulseIsTrue) { + // Choose a very very small positive number for the ratio. + FixedRatioSampler fixed_ratio_sampler(1e-20); + EXPECT_TRUE(fixed_ratio_sampler.Pulse()); + for (int i = 0; i < 100; ++i) { + EXPECT_FALSE(fixed_ratio_sampler.Pulse()); + } +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/histogram.cc b/carto_ws/src/cartographer-master/cartographer/common/histogram.cc new file mode 100644 index 0000000..d54a9b5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/histogram.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/histogram.h" + +#include +#include +#include + +#include "absl/strings/str_cat.h" +#include "absl/strings/str_format.h" +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +void Histogram::Add(const float value) { values_.push_back(value); } + +std::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(); + std::string result = absl::StrCat("Count: ", values_.size(), " Min: ", min, + " Max: ", max, " Mean: ", 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; + absl::StrAppendFormat(&result, "\n[%f, %f%c", lower_bound, 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)) ? " " : "#"; + } + absl::StrAppend(&result, "\tCount: ", count, " (", + count * 1e2f / values_.size(), "%)", + "\tTotal: ", total_count, " (", + total_count * 1e2f / values_.size(), "%)"); + lower_bound = upper_bound; + } + return result; +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/histogram.h b/carto_ws/src/cartographer-master/cartographer/common/histogram.h new file mode 100644 index 0000000..59df2f7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/histogram.h @@ -0,0 +1,40 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_ +#define CARTOGRAPHER_COMMON_HISTOGRAM_H_ + +#include +#include + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace common { + +class Histogram { + public: + void Add(float value); + std::string ToString(int buckets) const; + + private: + std::vector values_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/blocking_queue.h b/carto_ws/src/cartographer-master/cartographer/common/internal/blocking_queue.h new file mode 100644 index 0000000..cba91c0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/blocking_queue.h @@ -0,0 +1,160 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ +#define CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ + +#include +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +// A thread-safe blocking queue that is useful for producer/consumer patterns. +// 'T' must be movable. +template +class BlockingQueue { + public: + static constexpr size_t kInfiniteQueueSize = 0; + + // Constructs a blocking queue with infinite queue size. + BlockingQueue() : BlockingQueue(kInfiniteQueueSize) {} + + BlockingQueue(const BlockingQueue&) = delete; + BlockingQueue& operator=(const BlockingQueue&) = delete; + + // Constructs a blocking queue with a size of 'queue_size'. + explicit BlockingQueue(const size_t queue_size) : queue_size_(queue_size) {} + + // Pushes a value onto the queue. Blocks if the queue is full. + void Push(T t) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return QueueNotFullCondition(); + }; + absl::MutexLock lock(&mutex_); + mutex_.Await(absl::Condition(&predicate)); + deque_.push_back(std::move(t)); + } + + // Like push, but returns false if 'timeout' is reached. + bool PushWithTimeout(T t, const common::Duration timeout) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return QueueNotFullCondition(); + }; + absl::MutexLock lock(&mutex_); + if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(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() { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + mutex_.Await(absl::Condition(&predicate)); + + 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) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(timeout))) { + return nullptr; + } + T t = std::move(deque_.front()); + deque_.pop_front(); + return t; + } + + // Like Peek, but can timeout. Returns nullptr in this case. + template + R* PeekWithTimeout(const common::Duration timeout) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(timeout))) { + return nullptr; + } + return deque_.front().get(); + } + + // Returns the next value in the queue or nullptr if the queue is empty. + // Maintains ownership. This assumes a member function get() that returns + // a pointer to the given type R. + template + const R* Peek() { + absl::MutexLock lock(&mutex_); + if (deque_.empty()) { + return nullptr; + } + return deque_.front().get(); + } + + // Returns the number of items currently in the queue. + size_t Size() { + absl::MutexLock lock(&mutex_); + return deque_.size(); + } + + // Blocks until the queue is empty. + void WaitUntilEmpty() { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + mutex_.Await(absl::Condition(&predicate)); + } + + private: + // Returns true iff the queue is empty. + bool QueueEmptyCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return deque_.empty(); + } + + // Returns true iff the queue is not full. + bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_; + } + + absl::Mutex mutex_; + const size_t queue_size_ GUARDED_BY(mutex_); + std::deque deque_ GUARDED_BY(mutex_); +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/blocking_queue_test.cc b/carto_ws/src/cartographer-master/cartographer/common/internal/blocking_queue_test.cc new file mode 100644 index 0000000..47f961e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/blocking_queue_test.cc @@ -0,0 +1,119 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/internal/blocking_queue.h" + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/time.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +TEST(BlockingQueueTest, testPushPeekPop) { + BlockingQueue> blocking_queue; + blocking_queue.Push(absl::make_unique(42)); + ASSERT_EQ(1, blocking_queue.Size()); + blocking_queue.Push(absl::make_unique(24)); + ASSERT_EQ(2, blocking_queue.Size()); + EXPECT_EQ(42, *blocking_queue.Peek()); + ASSERT_EQ(2, blocking_queue.Size()); + EXPECT_EQ(42, *blocking_queue.Pop()); + ASSERT_EQ(1, blocking_queue.Size()); + EXPECT_EQ(24, *blocking_queue.Pop()); + ASSERT_EQ(0, blocking_queue.Size()); + EXPECT_EQ(nullptr, blocking_queue.Peek()); + ASSERT_EQ(0, blocking_queue.Size()); +} + +TEST(BlockingQueueTest, testPushPopSharedPtr) { + BlockingQueue> blocking_queue; + blocking_queue.Push(std::make_shared(42)); + blocking_queue.Push(std::make_shared(24)); + EXPECT_EQ(42, *blocking_queue.Pop()); + EXPECT_EQ(24, *blocking_queue.Pop()); +} + +TEST(BlockingQueueTest, testPopWithTimeout) { + BlockingQueue> blocking_queue; + EXPECT_EQ(nullptr, + blocking_queue.PopWithTimeout(common::FromMilliseconds(150))); +} + +TEST(BlockingQueueTest, testPushWithTimeout) { + BlockingQueue> blocking_queue(1); + EXPECT_EQ(true, + blocking_queue.PushWithTimeout(absl::make_unique(42), + common::FromMilliseconds(150))); + EXPECT_EQ(false, + blocking_queue.PushWithTimeout(absl::make_unique(15), + common::FromMilliseconds(150))); + EXPECT_EQ(42, *blocking_queue.Pop()); + EXPECT_EQ(0, blocking_queue.Size()); +} + +TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) { + BlockingQueue> blocking_queue; + EXPECT_EQ(true, + blocking_queue.PushWithTimeout(absl::make_unique(42), + common::FromMilliseconds(150))); + EXPECT_EQ(true, + blocking_queue.PushWithTimeout(absl::make_unique(45), + common::FromMilliseconds(150))); + EXPECT_EQ(42, *blocking_queue.Pop()); + EXPECT_EQ(45, *blocking_queue.Pop()); + EXPECT_EQ(0, blocking_queue.Size()); +} + +TEST(BlockingQueueTest, testBlockingPop) { + BlockingQueue> blocking_queue; + ASSERT_EQ(0, blocking_queue.Size()); + + int pop = 0; + + std::thread thread([&blocking_queue, &pop] { pop = *blocking_queue.Pop(); }); + + std::this_thread::sleep_for(common::FromMilliseconds(100)); + blocking_queue.Push(absl::make_unique(42)); + thread.join(); + ASSERT_EQ(0, blocking_queue.Size()); + EXPECT_EQ(42, pop); +} + +TEST(BlockingQueueTest, testBlockingPopWithTimeout) { + BlockingQueue> blocking_queue; + ASSERT_EQ(0, blocking_queue.Size()); + + int pop = 0; + + std::thread thread([&blocking_queue, &pop] { + pop = *blocking_queue.PopWithTimeout(common::FromMilliseconds(2500)); + }); + + std::this_thread::sleep_for(common::FromMilliseconds(100)); + blocking_queue.Push(absl::make_unique(42)); + thread.join(); + ASSERT_EQ(0, blocking_queue.Size()); + EXPECT_EQ(42, pop); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/ceres_solver_options.cc b/carto_ws/src/cartographer-master/cartographer/common/internal/ceres_solver_options.cc new file mode 100644 index 0000000..fd13e02 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/ceres_solver_options.cc @@ -0,0 +1,45 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/internal/ceres_solver_options.h" + +namespace cartographer { +namespace common { + +proto::CeresSolverOptions CreateCeresSolverOptionsProto( + common::LuaParameterDictionary* parameter_dictionary) { + proto::CeresSolverOptions proto; + proto.set_use_nonmonotonic_steps( + parameter_dictionary->GetBool("use_nonmonotonic_steps")); + proto.set_max_num_iterations( + parameter_dictionary->GetNonNegativeInt("max_num_iterations")); + proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads")); + CHECK_GT(proto.max_num_iterations(), 0); + CHECK_GT(proto.num_threads(), 0); + return proto; +} + +ceres::Solver::Options CreateCeresSolverOptions( + const proto::CeresSolverOptions& proto) { + ceres::Solver::Options options; + options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps(); + options.max_num_iterations = proto.max_num_iterations(); + options.num_threads = proto.num_threads(); + return options; +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/ceres_solver_options.h b/carto_ws/src/cartographer-master/cartographer/common/internal/ceres_solver_options.h new file mode 100644 index 0000000..cb5f35f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/ceres_solver_options.h @@ -0,0 +1,36 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ +#define CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/proto/ceres_solver_options.pb.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace common { + +proto::CeresSolverOptions CreateCeresSolverOptionsProto( + common::LuaParameterDictionary* parameter_dictionary); + +ceres::Solver::Options CreateCeresSolverOptions( + const proto::CeresSolverOptions& proto); + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/rate_timer.h b/carto_ws/src/cartographer-master/cartographer/common/internal/rate_timer.h new file mode 100644 index 0000000..afa27ef --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/rate_timer.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. + */ + +#ifndef CARTOGRAPHER_COMMON_RATE_TIMER_H_ +#define CARTOGRAPHER_COMMON_RATE_TIMER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" + +namespace cartographer { +namespace common { + +// Computes the rate at which pulses come in. +template +class RateTimer { + public: + // Computes the rate at which pulses come in over 'window_duration' in wall + // time. + explicit RateTimer(const common::Duration window_duration) + : window_duration_(window_duration) {} + ~RateTimer() {} + + RateTimer(const RateTimer&) = delete; + RateTimer& operator=(const RateTimer&) = delete; + + // Returns the pulse rate in Hz. + double ComputeRate() const { + if (events_.empty()) { + return 0.; + } + return static_cast(events_.size() - 1) / + common::ToSeconds((events_.back().time - events_.front().time)); + } + + // Returns the ratio of the pulse rate (with supplied times) to the wall time + // rate. For example, if a sensor produces pulses at 10 Hz, but we call Pulse + // at 20 Hz wall time, this will return 2. + double ComputeWallTimeRateRatio() const { + if (events_.empty()) { + return 0.; + } + return common::ToSeconds((events_.back().time - events_.front().time)) / + common::ToSeconds(events_.back().wall_time - + events_.front().wall_time); + } + + // Records an event that will contribute to the computed rate. + void Pulse(common::Time time) { + events_.push_back(Event{time, ClockType::now()}); + while (events_.size() > 2 && + (events_.back().wall_time - events_.front().wall_time) > + window_duration_) { + events_.pop_front(); + } + } + + // Returns a debug string representation. + std::string DebugString() const { + if (events_.size() < 2) { + return "unknown"; + } + std::ostringstream out; + out << std::fixed << std::setprecision(2) << ComputeRate() << " Hz " + << DeltasDebugString() << " (pulsed at " + << ComputeWallTimeRateRatio() * 100. << "% real time)"; + return out.str(); + } + + private: + struct Event { + common::Time time; + typename ClockType::time_point wall_time; + }; + + // Computes all differences in seconds between consecutive pulses. + std::vector ComputeDeltasInSeconds() const { + CHECK_GT(events_.size(), 1); + const size_t count = events_.size() - 1; + std::vector result; + result.reserve(count); + for (size_t i = 0; i != count; ++i) { + result.push_back( + common::ToSeconds(events_[i + 1].time - events_[i].time)); + } + return result; + } + + // Returns the average and standard deviation of the deltas. + std::string DeltasDebugString() const { + const auto deltas = ComputeDeltasInSeconds(); + const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.); + const double mean = sum / deltas.size(); + + double squared_sum = 0.; + for (const double x : deltas) { + squared_sum += common::Pow2(x - mean); + } + const double sigma = std::sqrt(squared_sum / (deltas.size() - 1)); + + std::ostringstream out; + out << std::scientific << std::setprecision(2) << mean << " s +/- " << sigma + << " s"; + return out.str(); + } + + std::deque events_; + const common::Duration window_duration_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_RATE_TIMER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/rate_timer_test.cc b/carto_ws/src/cartographer-master/cartographer/common/internal/rate_timer_test.cc new file mode 100644 index 0000000..8c73bdd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/rate_timer_test.cc @@ -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. + */ + +#include "cartographer/common/internal/rate_timer.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +TEST(RateTimerTest, ComputeRate) { + RateTimer<> rate_timer(common::FromSeconds(1.)); + common::Time time = common::FromUniversal(42); + for (int i = 0; i < 100; ++i) { + rate_timer.Pulse(time); + time += common::FromSeconds(0.1); + } + EXPECT_NEAR(10., rate_timer.ComputeRate(), 1e-3); +} + +struct SimulatedClock { + using rep = std::chrono::steady_clock::rep; + using period = std::chrono::steady_clock::period; + using duration = std::chrono::steady_clock::duration; + using time_point = std::chrono::steady_clock::time_point; + + static time_point time; + static time_point now() noexcept { return time; } +}; + +SimulatedClock::time_point SimulatedClock::time; + +TEST(RateTimerTest, ComputeWallTimeRateRatio) { + common::Time time = common::FromUniversal(42); + RateTimer rate_timer(common::FromSeconds(1.)); + for (int i = 0; i < 100; ++i) { + rate_timer.Pulse(time); + time += common::FromSeconds(0.1); + SimulatedClock::time += + std::chrono::duration_cast( + std::chrono::duration(0.05)); + } + EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h b/carto_ws/src/cartographer-master/cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h new file mode 100644 index 0000000..4326029 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ +#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.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 {} + + std::string GetFileContentOrDie(const std::string& unused_basename) override { + LOG(FATAL) << "Not implemented"; + } + + std::string GetFullPathOrDie(const std::string& unused_basename) override { + LOG(FATAL) << "Not implemented"; + } +}; + +std::unique_ptr MakeDictionary( + const std::string& code) { + return absl::make_unique( + code, absl::make_unique()); +} + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/testing/thread_pool_for_testing.cc b/carto_ws/src/cartographer-master/cartographer/common/internal/testing/thread_pool_for_testing.cc new file mode 100644 index 0000000..89744ba --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/testing/thread_pool_for_testing.cc @@ -0,0 +1,114 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/internal/testing/thread_pool_for_testing.h" + +#ifndef WIN32 +#include +#endif +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/task.h" +#include "cartographer/common/time.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { +namespace testing { + +ThreadPoolForTesting::ThreadPoolForTesting() + : thread_([this]() { ThreadPoolForTesting::DoWork(); }) {} + +ThreadPoolForTesting::~ThreadPoolForTesting() { + { + absl::MutexLock locker(&mutex_); + CHECK(running_); + running_ = false; + CHECK_EQ(task_queue_.size(), 0); + CHECK_EQ(tasks_not_ready_.size(), 0); + } + thread_.join(); +} + +void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) { + absl::MutexLock locker(&mutex_); + CHECK(running_); + auto it = tasks_not_ready_.find(task); + CHECK(it != tasks_not_ready_.end()); + task_queue_.push_back(it->second); + tasks_not_ready_.erase(it); +} + +std::weak_ptr ThreadPoolForTesting::Schedule(std::unique_ptr task) { + std::shared_ptr shared_task; + { + absl::MutexLock locker(&mutex_); + idle_ = false; + CHECK(running_); + auto insert_result = + tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task))); + CHECK(insert_result.second) << "ScheduleWhenReady called twice"; + shared_task = insert_result.first->second; + } + SetThreadPool(shared_task.get()); + return shared_task; +} + +void ThreadPoolForTesting::WaitUntilIdle() { + const auto predicate = [this]() + EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return idle_; }; + for (;;) { + { + absl::MutexLock locker(&mutex_); + if (mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(0.1)))) { + return; + } + } + } +} + +void ThreadPoolForTesting::DoWork() { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !task_queue_.empty() || !running_; + }; + for (;;) { + std::shared_ptr task; + { + absl::MutexLock locker(&mutex_); + mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(0.1))); + if (!task_queue_.empty()) { + task = task_queue_.front(); + task_queue_.pop_front(); + } + if (!running_) { + return; + } + if (tasks_not_ready_.empty() && task_queue_.empty() && !task) { + idle_ = true; + } + } + if (task) Execute(task.get()); + } +} + +} // namespace testing +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/internal/testing/thread_pool_for_testing.h b/carto_ws/src/cartographer-master/cartographer/common/internal/testing/thread_pool_for_testing.h new file mode 100644 index 0000000..f733d0f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/internal/testing/thread_pool_for_testing.h @@ -0,0 +1,61 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_INTERNAL_TESTING_THREAD_POOL_FOR_TESTING_H_ +#define CARTOGRAPHER_COMMON_INTERNAL_TESTING_THREAD_POOL_FOR_TESTING_H_ + +#include +#include +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/common/thread_pool.h" + +namespace cartographer { +namespace common { +namespace testing { + +class ThreadPoolForTesting : public ThreadPoolInterface { + public: + ThreadPoolForTesting(); + ~ThreadPoolForTesting(); + + std::weak_ptr Schedule(std::unique_ptr task) + LOCKS_EXCLUDED(mutex_) override; + + void WaitUntilIdle(); + + private: + friend class Task; + + void DoWork(); + + void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override; + + absl::Mutex mutex_; + bool running_ GUARDED_BY(mutex_) = true; + bool idle_ GUARDED_BY(mutex_) = true; + std::deque> task_queue_ GUARDED_BY(mutex_); + std::map> tasks_not_ready_ GUARDED_BY(mutex_); + std::thread thread_ GUARDED_BY(mutex_); +}; + +} // namespace testing +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_INTERNAL_TESTING_THREAD_POOL_FOR_TESTING_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/lua.h b/carto_ws/src/cartographer-master/cartographer/common/lua.h new file mode 100644 index 0000000..a40f4f0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/lua.h @@ -0,0 +1,22 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_LUA_H_ +#define CARTOGRAPHER_COMMON_LUA_H_ + +#include + +#endif // CARTOGRAPHER_COMMON_LUA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary.cc b/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary.cc new file mode 100644 index 0000000..55c3cc7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary.cc @@ -0,0 +1,473 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// When a LuaParameterDictionary is constructed, a new Lua state (i.e. an +// independent Lua interpreter) is fired up to evaluate the Lua code. The code +// is expected to return a Lua table that contains key/value pairs that are the +// key/value pairs of our parameter dictionary. +// +// We keep the Lua interpreter around and the table on the stack and reference +// it in the Get* methods instead of moving its contents from Lua into a C++ map +// since we can only know in the Get* methods what data type to expect in the +// table. +// +// Some of the methods below documentation the current stack with the following +// notation: S: ... + +#include "cartographer/common/lua_parameter_dictionary.h" + +#include +#include +#include +#include + +namespace cartographer { +namespace common { + +namespace { + +// Replace the string at the top of the stack through a quoted version that Lua +// can read back. +void QuoteStringOnStack(lua_State* L) { + CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value."; + int current_index = lua_gettop(L); + + // S: ... string + lua_pushglobaltable(L); // S: ... string globals + lua_getfield(L, -1, "string"); // S: ... string globals + lua_getfield(L, -1, + "format"); // S: ... string globals format + lua_pushstring(L, "%q"); // S: ... string globals format "%q" + lua_pushvalue(L, current_index); // S: ... string globals + // format "%q" string + + lua_call(L, 2, 1); // S: ... string globals quoted + lua_replace(L, current_index); // S: ... quoted globals + + lua_pop(L, 2); // S: ... quoted +} + +// Sets the given 'dictionary' as the value of the "this" key in Lua's registry +// table. +void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) { + lua_pushstring(L, "this"); + lua_pushlightuserdata(L, dictionary); + lua_settable(L, LUA_REGISTRYINDEX); +} + +// Gets the 'dictionary' from the "this" key in Lua's registry table. +LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) { + lua_pushstring(L, "this"); + lua_gettable(L, LUA_REGISTRYINDEX); + void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1); + lua_pop(L, 1); + CHECK(return_value != nullptr); + return reinterpret_cast(return_value); +} + +// CHECK()s if a Lua method returned an error. +void CheckForLuaErrors(lua_State* L, int status) { + CHECK_EQ(status, 0) << lua_tostring(L, -1); +} + +// Returns 'a' if 'condition' is true, else 'b'. +int LuaChoose(lua_State* L) { + CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b)."; + CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value."; + + const bool condition = lua_toboolean(L, 1); + if (condition) { + lua_pushvalue(L, 2); + } else { + lua_pushvalue(L, 3); + } + return 1; +} + +// Pushes a value to the Lua stack. +void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); } +void PushValue(lua_State* L, const std::string& key) { + lua_pushstring(L, key.c_str()); +} + +// Reads the value with the given 'key' from the Lua dictionary and pushes it to +// the top of the stack. +template +void GetValueFromLuaTable(lua_State* L, const T& key) { + PushValue(L, key); + lua_rawget(L, -2); +} + +// CHECK() that the topmost parameter on the Lua stack is a table. +void CheckTableIsAtTopOfStack(lua_State* L) { + CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; +} + +// Returns true if 'key' is in the table at the top of the Lua stack. +template +bool HasKeyOfType(lua_State* L, const T& key) { + CheckTableIsAtTopOfStack(L); + PushValue(L, key); + lua_rawget(L, -2); + const bool key_not_found = lua_isnil(L, -1); + lua_pop(L, 1); // Pop the item again. + return !key_not_found; +} + +// Iterates over the integer keys of the table at the top of the stack of 'L• +// and pushes the values one by one. 'pop_value' is expected to pop a value and +// put them into a C++ container. +void GetArrayValues(lua_State* L, const std::function& pop_value) { + int idx = 1; + while (true) { + GetValueFromLuaTable(L, idx); + if (lua_isnil(L, -1)) { + lua_pop(L, 1); + break; + } + pop_value(); + ++idx; + } +} + +} // namespace + +std::unique_ptr +LuaParameterDictionary::NonReferenceCounted( + const std::string& code, std::unique_ptr file_resolver) { + return std::unique_ptr(new LuaParameterDictionary( + code, ReferenceCount::NO, std::move(file_resolver))); +} + +LuaParameterDictionary::LuaParameterDictionary( + const std::string& code, std::unique_ptr file_resolver) + : LuaParameterDictionary(code, ReferenceCount::YES, + std::move(file_resolver)) {} + +LuaParameterDictionary::LuaParameterDictionary( + const std::string& code, ReferenceCount reference_count, + std::unique_ptr file_resolver) + : 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); + + CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str())); + CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0)); + CheckTableIsAtTopOfStack(L_); +} + +LuaParameterDictionary::LuaParameterDictionary( + lua_State* const L, ReferenceCount reference_count, + std::shared_ptr file_resolver) + : L_(lua_newthread(L)), + file_resolver_(std::move(file_resolver)), + reference_count_(reference_count) { + CHECK_NOTNULL(L_); + + // Make sure this is never garbage collected. + CHECK(lua_isthread(L, -1)); + index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX); + + CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; + lua_xmove(L, L_, 1); // Moves the table and the coroutine over. + CheckTableIsAtTopOfStack(L_); +} + +LuaParameterDictionary::~LuaParameterDictionary() { + if (reference_count_ == ReferenceCount::YES) { + CheckAllKeysWereUsedExactlyOnceAndReset(); + } + if (index_into_reference_table_ > 0) { + luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_); + } else { + lua_close(L_); + } +} + +std::vector LuaParameterDictionary::GetKeys() const { + CheckTableIsAtTopOfStack(L_); + std::vector keys; + + lua_pushnil(L_); // Push the first key + while (lua_next(L_, -2) != 0) { + lua_pop(L_, 1); // Pop value, keep key. + if (!lua_isnumber(L_, -1)) { + keys.emplace_back(lua_tostring(L_, -1)); + } + } + return keys; +} + +bool LuaParameterDictionary::HasKey(const std::string& key) const { + return HasKeyOfType(L_, key); +} + +std::string LuaParameterDictionary::GetString(const std::string& key) { + CheckHasKeyAndReference(key); + GetValueFromLuaTable(L_, key); + return PopString(Quoted::NO); +} + +std::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 std::string value = lua_tostring(L_, -1); + lua_pop(L_, 1); + return value; +} + +double LuaParameterDictionary::GetDouble(const std::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 std::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 std::string& key) { + CheckHasKeyAndReference(key); + GetValueFromLuaTable(L_, key); + return PopBool(); +} + +bool LuaParameterDictionary::PopBool() const { + CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value."; + const bool value = lua_toboolean(L_, -1); + lua_pop(L_, 1); + return value; +} + +std::unique_ptr LuaParameterDictionary::GetDictionary( + const std::string& key) { + CheckHasKeyAndReference(key); + GetValueFromLuaTable(L_, key); + return PopDictionary(reference_count_); +} + +std::unique_ptr LuaParameterDictionary::PopDictionary( + ReferenceCount reference_count) const { + CheckTableIsAtTopOfStack(L_); + std::unique_ptr value( + new LuaParameterDictionary(L_, reference_count, file_resolver_)); + // The constructor lua_xmove()s the value, no need to pop it. + CheckTableIsAtTopOfStack(L_); + return value; +} + +std::string LuaParameterDictionary::DoToString( + const std::string& indent) const { + std::string result = "{"; + bool dictionary_is_empty = true; + + const auto top_of_stack_to_string = [this, indent, + &dictionary_is_empty]() -> std::string { + dictionary_is_empty = false; + + const int value_type = lua_type(L_, -1); + switch (value_type) { + case LUA_TBOOLEAN: + return PopBool() ? "true" : "false"; + break; + case LUA_TSTRING: + return PopString(Quoted::YES); + break; + case LUA_TNUMBER: { + const double value = PopDouble(); + if (std::isinf(value)) { + return value < 0 ? "-math.huge" : "math.huge"; + } else { + return std::to_string(value); + } + } break; + case LUA_TTABLE: { + std::unique_ptr subdict( + PopDictionary(ReferenceCount::NO)); + return subdict->DoToString(indent + " "); + } break; + default: + LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type); + } + }; + + // Integer (array) keys. + for (int i = 1; i; ++i) { + GetValueFromLuaTable(L_, i); + if (lua_isnil(L_, -1)) { + lua_pop(L_, 1); + break; + } + result.append("\n"); + result.append(indent); + result.append(" "); + result.append(top_of_stack_to_string()); + result.append(","); + } + + // String keys. + std::vector keys = GetKeys(); + if (!keys.empty()) { + std::sort(keys.begin(), keys.end()); + for (const std::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; +} + +std::string LuaParameterDictionary::ToString() const { return DoToString(""); } + +std::vector LuaParameterDictionary::GetArrayValuesAsDoubles() { + std::vector values; + GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); }); + return values; +} + +std::vector> +LuaParameterDictionary::GetArrayValuesAsDictionaries() { + std::vector> values; + GetArrayValues(L_, [&values, this] { + values.push_back(PopDictionary(reference_count_)); + }); + return values; +} + +std::vector LuaParameterDictionary::GetArrayValuesAsStrings() { + std::vector values; + GetArrayValues(L_, + [&values, this] { values.push_back(PopString(Quoted::NO)); }); + return values; +} + +void LuaParameterDictionary::CheckHasKey(const std::string& key) const { + CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n" + << ToString(); +} + +void LuaParameterDictionary::CheckHasKeyAndReference(const std::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 std::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 std::string basename = lua_tostring(L, -1); + const std::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()) { + std::string error_msg = + "Tried to include " + filename + + " twice. Already included files in order of inclusion: "; + for (const std::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 std::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 std::string file_content = + parameter_dictionary->file_resolver_->GetFileContentOrDie( + lua_tostring(L, -1)); + lua_pushstring(L, file_content.c_str()); + return 1; +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary.h b/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary.h new file mode 100644 index 0000000..52eaf87 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary.h @@ -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. + */ + +#ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ +#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ + +#include +#include +#include +#include + +#include "cartographer/common/lua.h" +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +// Resolves file paths and file content for the Lua 'read' and 'include' +// functions. Use this to configure where those functions load other files from. +class FileResolver { + public: + virtual ~FileResolver() {} + virtual std::string GetFullPathOrDie(const std::string& basename) = 0; + virtual std::string GetFileContentOrDie(const std::string& basename) = 0; +}; + +// A parameter dictionary that gets loaded from Lua code. +class LuaParameterDictionary { + public: + // Constructs the dictionary from a Lua Table specification. + LuaParameterDictionary(const std::string& code, + std::unique_ptr file_resolver); + + LuaParameterDictionary(const LuaParameterDictionary&) = delete; + LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete; + + // Constructs a LuaParameterDictionary without reference counting. + static std::unique_ptr NonReferenceCounted( + const std::string& code, std::unique_ptr file_resolver); + + ~LuaParameterDictionary(); + + // Returns all available keys. + std::vector GetKeys() const; + + // Returns true if the key is in this dictionary. + bool HasKey(const std::string& key) const; + + // These methods CHECK() that the 'key' exists. + std::string GetString(const std::string& key); + double GetDouble(const std::string& key); + int GetInt(const std::string& key); + bool GetBool(const std::string& key); + std::unique_ptr GetDictionary(const std::string& key); + + // Gets an int from the dictionary and CHECK()s that it is non-negative. + int GetNonNegativeInt(const std::string& key); + + // Returns a string representation for this LuaParameterDictionary. + std::string ToString() const; + + // Returns the values of the keys '1', '2', '3' as the given types. + std::vector GetArrayValuesAsDoubles(); + std::vector GetArrayValuesAsStrings(); + std::vector> + GetArrayValuesAsDictionaries(); + + private: + enum class ReferenceCount { YES, NO }; + LuaParameterDictionary(const std::string& code, + ReferenceCount reference_count, + std::unique_ptr file_resolver); + + // For GetDictionary(). + LuaParameterDictionary(lua_State* L, ReferenceCount reference_count, + std::shared_ptr file_resolver); + + // Function that recurses to keep track of indent for ToString(). + std::string DoToString(const std::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 }; + std::string PopString(Quoted quoted) const; + + // Creates a LuaParameterDictionary from the Lua table at the top of the + // stack, either with or without reference counting. + std::unique_ptr PopDictionary( + ReferenceCount reference_count) const; + + // CHECK() that 'key' is in the dictionary. + void CheckHasKey(const std::string& key) const; + + // CHECK() that 'key' is in this dictionary and reference it as being used. + void CheckHasKeyAndReference(const std::string& key); + + // If desired, this can be called in the destructor of a derived class. It + // will CHECK() that all keys defined in the configuration have been used + // exactly once and resets the reference counter. + void CheckAllKeysWereUsedExactlyOnceAndReset(); + + // Reads a file into a Lua string. + static int LuaRead(lua_State* L); + + // Handles inclusion of other Lua files and prevents double inclusion. + static int LuaInclude(lua_State* L); + + lua_State* L_; // The name is by convention in the Lua World. + int index_into_reference_table_; + + // This is shared with all the sub dictionaries. + const std::shared_ptr file_resolver_; + + // If true will check that all keys were used on destruction. + const ReferenceCount reference_count_; + + // This is modified with every call to Get* in order to verify that all + // parameters are read exactly once. + std::map reference_counts_; + + // List of all included files in order of inclusion. Used to prevent double + // inclusion. + std::vector included_files_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary_test.cc b/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary_test.cc new file mode 100644 index 0000000..ada3b7b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/lua_parameter_dictionary_test.cc @@ -0,0 +1,223 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/lua_parameter_dictionary.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +std::unique_ptr MakeNonReferenceCounted( + const std::string& code) { + return LuaParameterDictionary::NonReferenceCounted( + code, absl::make_unique()); +} + +class LuaParameterDictionaryTest : public ::testing::Test { + protected: + void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) { + for (const std::string& key : dict->GetKeys()) { + dict->GetInt(key); + } + } +}; + +TEST_F(LuaParameterDictionaryTest, GetInt) { + auto dict = MakeDictionary("return { blah = 100 }"); + ASSERT_EQ(dict->GetInt("blah"), 100); +} + +TEST_F(LuaParameterDictionaryTest, GetString) { + auto dict = MakeDictionary("return { blah = 'is_a_string' }\n"); + ASSERT_EQ(dict->GetString("blah"), "is_a_string"); +} + +TEST_F(LuaParameterDictionaryTest, GetDouble) { + auto dict = MakeDictionary("return { blah = 3.1415 }"); + ASSERT_DOUBLE_EQ(dict->GetDouble("blah"), 3.1415); +} + +TEST_F(LuaParameterDictionaryTest, GetBoolTrue) { + auto dict = MakeDictionary("return { blah = true }"); + ASSERT_TRUE(dict->GetBool("blah")); +} + +TEST_F(LuaParameterDictionaryTest, GetBoolFalse) { + auto dict = MakeDictionary("return { blah = false }"); + ASSERT_FALSE(dict->GetBool("blah")); +} + +TEST_F(LuaParameterDictionaryTest, GetDictionary) { + auto dict = + MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }"); + + std::unique_ptr sub_dict(dict->GetDictionary("blah")); + std::vector keys = sub_dict->GetKeys(); + ASSERT_EQ(keys.size(), 2); + std::sort(keys.begin(), keys.end()); + ASSERT_EQ(keys[0], "blue"); + ASSERT_EQ(keys[1], "red"); + ASSERT_TRUE(sub_dict->HasKey("blue")); + ASSERT_TRUE(sub_dict->HasKey("red")); + ASSERT_EQ(sub_dict->GetInt("blue"), 100); + ASSERT_EQ(sub_dict->GetInt("red"), 200); + + ASSERT_EQ(dict->GetString("fasel"), "10"); +} + +TEST_F(LuaParameterDictionaryTest, GetKeys) { + auto dict = MakeDictionary("return { blah = 100, blah1 = 200}"); + + std::vector keys = dict->GetKeys(); + ASSERT_EQ(keys.size(), 2); + std::sort(keys.begin(), keys.end()); + ASSERT_EQ(keys[0], "blah"); + ASSERT_EQ(keys[1], "blah1"); + + ReferenceAllKeysAsIntegers(dict.get()); +} + +TEST_F(LuaParameterDictionaryTest, NonExistingKey) { + auto dict = MakeDictionary("return { blah = 100 }"); + ReferenceAllKeysAsIntegers(dict.get()); + ASSERT_DEATH(dict->GetInt("blah_fasel"), "Key.* not in dictionary."); +} + +TEST_F(LuaParameterDictionaryTest, UintNegative) { + auto dict = MakeDictionary("return { blah = -100}"); + ASSERT_DEATH(dict->GetNonNegativeInt("blah"), ".*-100 is negative."); + ReferenceAllKeysAsIntegers(dict.get()); +} + +TEST_F(LuaParameterDictionaryTest, ToString) { + auto dict = MakeDictionary(R"(return { + ceta = { yolo = "hurray" }, + fasel = 1234.456786, + fasel1 = -math.huge, + fasel2 = math.huge, + blubber = 123, + blub = 'hello', + alpha = true, + alpha1 = false, + })"); + + const std::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 std::string golden = R"({ + "blub", + 3.000000, + 3.100000, + foo = "ups", +})"; + EXPECT_EQ(golden, dict->ToString()); +} + +TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) { + auto dict = MakeDictionary("return { 'a', 'b', 'c' }"); + EXPECT_EQ(0, dict->GetKeys().size()); + const std::vector values = dict->GetArrayValuesAsStrings(); + EXPECT_EQ(3, values.size()); + EXPECT_EQ("a", values[0]); + EXPECT_EQ("b", values[1]); + EXPECT_EQ("c", values[2]); +} + +TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDoubles) { + auto dict = MakeDictionary("return { 1., 2., 3. }"); + EXPECT_EQ(0, dict->GetKeys().size()); + const std::vector values = dict->GetArrayValuesAsDoubles(); + EXPECT_EQ(3, values.size()); + EXPECT_NEAR(1., values[0], 1e-7); + EXPECT_NEAR(2., values[1], 1e-7); + EXPECT_NEAR(3., values[2], 1e-7); +} + +TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDictionaries) { + auto dict = MakeDictionary("return { { a = 1 }, { b = 3 } }"); + EXPECT_EQ(0, dict->GetKeys().size()); + const std::vector> values = + dict->GetArrayValuesAsDictionaries(); + EXPECT_EQ(2, values.size()); + EXPECT_EQ(1., values[0]->GetInt("a")); + EXPECT_EQ(3., values[1]->GetInt("b")); +} + +TEST_F(LuaParameterDictionaryTest, TestChooseTrue) { + auto dict = MakeDictionary("return { a = choose(true, 1, 0) }"); + EXPECT_EQ(1, dict->GetInt("a")); +} + +TEST_F(LuaParameterDictionaryTest, TestChoseFalse) { + auto dict = MakeDictionary("return { a = choose(false, 1, 0) }"); + EXPECT_EQ(0, dict->GetInt("a")); +} + +TEST_F(LuaParameterDictionaryTest, TestChooseInvalidArgument) { + EXPECT_DEATH(MakeDictionary("return { a = choose('truish', 1, 0) }"), + "condition is not a boolean value."); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/math.h b/carto_ws/src/cartographer-master/cartographer/common/math.h new file mode 100644 index 0000000..c4a77ef --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/math.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. + */ + +#ifndef CARTOGRAPHER_COMMON_MATH_H_ +#define CARTOGRAPHER_COMMON_MATH_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace common { + +// Clamps 'value' to be in the range ['min', 'max']. +template +T Clamp(const T value, const T min, const T max) { + if (value > max) { + return max; + } + if (value < min) { + return min; + } + return value; +} + +// Calculates 'base'^'exponent'. +template +constexpr T Power(T base, int exponent) { + return (exponent != 0) ? base * Power(base, exponent - 1) : T(1); +} + +// Calculates a^2. +template +constexpr T Pow2(T a) { + return Power(a, 2); +} + +// Converts from degrees to radians. +constexpr double DegToRad(double deg) { return M_PI * deg / 180.; } + +// Converts form radians to degrees. +constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; } + +// Bring the 'difference' between two angles into [-pi; pi]. +template +T NormalizeAngleDifference(T difference) { + const T kPi = T(M_PI); + while (difference > kPi) difference -= 2. * kPi; + while (difference < -kPi) difference += 2. * kPi; + return difference; +} + +template +T atan2(const Eigen::Matrix& vector) { + return ceres::atan2(vector.y(), vector.x()); +} + +template +inline void QuaternionProduct(const double* const z, const T* const w, + T* const zw) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_MATH_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/math_test.cc b/carto_ws/src/cartographer-master/cartographer/common/math_test.cc new file mode 100644 index 0000000..cad7c1a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/math_test.cc @@ -0,0 +1,61 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/math.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +TEST(MathTest, testPower) { + EXPECT_EQ(0., Power(0, 42)); + EXPECT_EQ(1., Power(0, 0)); + EXPECT_EQ(1., Power(1, 0)); + EXPECT_EQ(1., Power(1, 42)); + EXPECT_EQ(4., Power(2, 2)); +} + +TEST(MathTest, testPow2) { + EXPECT_EQ(0., Pow2(0)); + EXPECT_EQ(1., Pow2(1)); + EXPECT_EQ(4., Pow2(2)); + EXPECT_EQ(49., Pow2(7)); +} + +TEST(MathTest, testDeg2rad) { + EXPECT_NEAR(M_PI, DegToRad(180.), 1e-9); + EXPECT_NEAR(2. * M_PI, DegToRad(360. - 1e-9), 1e-6); +} + +TEST(MathTest, testRad2deg) { + EXPECT_NEAR(180., RadToDeg(M_PI), 1e-9); + EXPECT_NEAR(360., RadToDeg(2. * M_PI - 1e-9), 1e-6); +} + +TEST(MathTest, testNormalizeAngleDifference) { + EXPECT_NEAR(0., NormalizeAngleDifference(0.), 1e-9); + EXPECT_NEAR(M_PI, NormalizeAngleDifference(M_PI), 1e-9); + EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-M_PI), 1e-9); + EXPECT_NEAR(0., NormalizeAngleDifference(2. * M_PI), 1e-9); + EXPECT_NEAR(M_PI, NormalizeAngleDifference(5. * M_PI), 1e-9); + EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-5. * M_PI), 1e-9); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/port.h b/carto_ws/src/cartographer-master/cartographer/common/port.h new file mode 100644 index 0000000..eec8469 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/port.h @@ -0,0 +1,71 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_PORT_H_ +#define CARTOGRAPHER_COMMON_PORT_H_ + +#include +#include +#include +#include +#include +#include + +namespace cartographer { + +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; + +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 std::string& uncompressed, + std::string* compressed) { + boost::iostreams::filtering_ostream out; + out.push( + boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed)); + out.push(boost::iostreams::back_inserter(*compressed)); + boost::iostreams::write(out, + reinterpret_cast(uncompressed.data()), + uncompressed.size()); +} + +inline void FastGunzipString(const std::string& compressed, + std::string* decompressed) { + boost::iostreams::filtering_ostream out; + out.push(boost::iostreams::gzip_decompressor()); + out.push(boost::iostreams::back_inserter(*decompressed)); + boost::iostreams::write(out, reinterpret_cast(compressed.data()), + compressed.size()); +} + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_PORT_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/print_configuration_main.cc b/carto_ws/src/cartographer-master/cartographer/common/print_configuration_main.cc new file mode 100644 index 0000000..189f633 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/print_configuration_main.cc @@ -0,0 +1,102 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "absl/memory/memory.h" +#include "absl/strings/str_split.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string(configuration_directories, "", + "Comma separated list of directories in which configuration files" + " are searched, the last is always the Cartographer installation" + " to allow including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); +DEFINE_string(subdictionary, "", + "Only print a subdictionary referenced by its Lua ID, e.g.: " + "'--subdictionary trajectory_builder.trajectory_builder_3d'"); + +namespace cartographer { +namespace common { + +std::unique_ptr LoadLuaDictionary( + const std::vector& configuration_directories, + const std::string& configuration_basename) { + auto file_resolver = + absl::make_unique(configuration_directories); + const std::string code = + file_resolver->GetFileContentOrDie(configuration_basename); + // We use no reference count because we just want to print the configuration. + return LuaParameterDictionary::NonReferenceCounted(code, + std::move(file_resolver)); +} + +void PrintSubdictionaryById(LuaParameterDictionary* lua_dictionary, + const std::string& subdictionary_id) { + const std::vector subdictionary_keys = + absl::StrSplit(subdictionary_id, '.', absl::SkipEmpty()); + CHECK(!subdictionary_keys.empty()) << "Failed to parse 'subdictionary_id'."; + // Keep a stack to avoid memory glitches due to unique_ptr deletion. + std::vector> stack; + for (const auto& key : subdictionary_keys) { + if (stack.empty()) { + stack.push_back(lua_dictionary->GetDictionary(key)); + continue; + } + stack.push_back(stack.back()->GetDictionary(key)); + } + std::cout << subdictionary_id << " = " << stack.back()->ToString() + << std::endl; +} + +} // namespace common +} // namespace cartographer + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "Resolves and compiles a Lua configuration and prints it to stdout.\n" + "The output can be restricted to a subdictionary using the optional " + "'--subdictionary' parameter, which can be given in Lua syntax.\n" + "The logs of the configuration file resolver are written to stderr if " + "'--logtostderr' is given."); + google::ParseCommandLineFlags(&argc, &argv, true); + + if (FLAGS_configuration_directories.empty() || + FLAGS_configuration_basename.empty()) { + google::ShowUsageWithFlagsRestrict(argv[0], "print_configuration_main"); + return EXIT_FAILURE; + } + + const std::vector configuration_directories = + absl::StrSplit(FLAGS_configuration_directories, ',', absl::SkipEmpty()); + + auto lua_dictionary = ::cartographer::common::LoadLuaDictionary( + configuration_directories, FLAGS_configuration_basename); + + if (FLAGS_subdictionary.empty()) { + std::cout << "return " << lua_dictionary->ToString() << std::endl; + return EXIT_SUCCESS; + } + ::cartographer::common::PrintSubdictionaryById(lua_dictionary.get(), + FLAGS_subdictionary); +} diff --git a/carto_ws/src/cartographer-master/cartographer/common/proto/ceres_solver_options.proto b/carto_ws/src/cartographer-master/cartographer/common/proto/ceres_solver_options.proto new file mode 100644 index 0000000..60a00d1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/proto/ceres_solver_options.proto @@ -0,0 +1,25 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.common.proto; + +message CeresSolverOptions { + // Configure the Ceres solver. See the Ceres documentation for more + // information: https://code.google.com/p/ceres-solver/ + bool use_nonmonotonic_steps = 1; + int32 max_num_iterations = 2; + int32 num_threads = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/common/task.cc b/carto_ws/src/cartographer-master/cartographer/common/task.cc new file mode 100644 index 0000000..4085c97 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/task.cc @@ -0,0 +1,107 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/task.h" + +namespace cartographer { +namespace common { + +Task::~Task() { + // TODO(gaschler): Relax some checks after testing. + if (state_ != NEW && state_ != COMPLETED) { + LOG(WARNING) << "Delete Task between dispatch and completion."; + } +} + +Task::State Task::GetState() { + absl::MutexLock locker(&mutex_); + return state_; +} + +void Task::SetWorkItem(const WorkItem& work_item) { + absl::MutexLock locker(&mutex_); + CHECK_EQ(state_, NEW); + work_item_ = work_item; +} + +void Task::AddDependency(std::weak_ptr dependency) { + std::shared_ptr shared_dependency; + { + absl::MutexLock locker(&mutex_); + CHECK_EQ(state_, NEW); + if ((shared_dependency = dependency.lock())) { + ++uncompleted_dependencies_; + } + } + if (shared_dependency) { + shared_dependency->AddDependentTask(this); + } +} + +void Task::SetThreadPool(ThreadPoolInterface* thread_pool) { + absl::MutexLock locker(&mutex_); + CHECK_EQ(state_, NEW); + state_ = DISPATCHED; + thread_pool_to_notify_ = thread_pool; + if (uncompleted_dependencies_ == 0) { + state_ = DEPENDENCIES_COMPLETED; + CHECK(thread_pool_to_notify_); + thread_pool_to_notify_->NotifyDependenciesCompleted(this); + } +} + +void Task::AddDependentTask(Task* dependent_task) { + absl::MutexLock locker(&mutex_); + if (state_ == COMPLETED) { + dependent_task->OnDependenyCompleted(); + return; + } + bool inserted = dependent_tasks_.insert(dependent_task).second; + CHECK(inserted) << "Given dependency is already a dependency."; +} + +void Task::OnDependenyCompleted() { + absl::MutexLock locker(&mutex_); + CHECK(state_ == NEW || state_ == DISPATCHED); + --uncompleted_dependencies_; + if (uncompleted_dependencies_ == 0 && state_ == DISPATCHED) { + state_ = DEPENDENCIES_COMPLETED; + CHECK(thread_pool_to_notify_); + thread_pool_to_notify_->NotifyDependenciesCompleted(this); + } +} + +void Task::Execute() { + { + absl::MutexLock locker(&mutex_); + CHECK_EQ(state_, DEPENDENCIES_COMPLETED); + state_ = RUNNING; + } + + // Execute the work item. + if (work_item_) { + work_item_(); + } + + absl::MutexLock locker(&mutex_); + state_ = COMPLETED; + for (Task* dependent_task : dependent_tasks_) { + dependent_task->OnDependenyCompleted(); + } +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/task.h b/carto_ws/src/cartographer-master/cartographer/common/task.h new file mode 100644 index 0000000..ae44fb1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/task.h @@ -0,0 +1,76 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_TASK_H_ +#define CARTOGRAPHER_COMMON_TASK_H_ + +#include + +#include "absl/synchronization/mutex.h" +#include "glog/logging.h" +#include "thread_pool.h" + +namespace cartographer { +namespace common { + +class ThreadPoolInterface; + +class Task { + public: + friend class ThreadPoolInterface; + + using WorkItem = std::function; + enum State { NEW, DISPATCHED, DEPENDENCIES_COMPLETED, RUNNING, COMPLETED }; + + Task() = default; + ~Task(); + + State GetState() LOCKS_EXCLUDED(mutex_); + + // State must be 'NEW'. + void SetWorkItem(const WorkItem& work_item) LOCKS_EXCLUDED(mutex_); + + // State must be 'NEW'. 'dependency' may be nullptr, in which case it is + // assumed completed. + void AddDependency(std::weak_ptr dependency) LOCKS_EXCLUDED(mutex_); + + private: + // Allowed in all states. + void AddDependentTask(Task* dependent_task); + + // State must be 'DEPENDENCIES_COMPLETED' and becomes 'COMPLETED'. + void Execute() LOCKS_EXCLUDED(mutex_); + + // State must be 'NEW' and becomes 'DISPATCHED' or 'DEPENDENCIES_COMPLETED'. + void SetThreadPool(ThreadPoolInterface* thread_pool) LOCKS_EXCLUDED(mutex_); + + // State must be 'NEW' or 'DISPATCHED'. If 'DISPATCHED', may become + // 'DEPENDENCIES_COMPLETED'. + void OnDependenyCompleted(); + + WorkItem work_item_ GUARDED_BY(mutex_); + ThreadPoolInterface* thread_pool_to_notify_ GUARDED_BY(mutex_) = nullptr; + State state_ GUARDED_BY(mutex_) = NEW; + unsigned int uncompleted_dependencies_ GUARDED_BY(mutex_) = 0; + std::set dependent_tasks_ GUARDED_BY(mutex_); + + absl::Mutex mutex_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_TASK_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/task_test.cc b/carto_ws/src/cartographer-master/cartographer/common/task_test.cc new file mode 100644 index 0000000..390a9c0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/task_test.cc @@ -0,0 +1,187 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/task.h" + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/thread_pool.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +class MockCallback { + public: + MOCK_METHOD0(Run, void()); +}; + +class FakeThreadPool : public ThreadPoolInterface { + public: + void NotifyDependenciesCompleted(Task* task) override { + auto it = tasks_not_ready_.find(task); + ASSERT_NE(it, tasks_not_ready_.end()); + task_queue_.push_back(it->second); + tasks_not_ready_.erase(it); + } + + std::weak_ptr Schedule(std::unique_ptr task) override { + std::shared_ptr shared_task; + auto it = + tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task))); + EXPECT_TRUE(it.second); + shared_task = it.first->second; + SetThreadPool(shared_task.get()); + return shared_task; + } + + void RunNext() { + ASSERT_GE(task_queue_.size(), 1); + Execute(task_queue_.front().get()); + task_queue_.pop_front(); + } + + bool IsEmpty() { return task_queue_.empty(); } + + private: + std::deque> task_queue_; + std::map> tasks_not_ready_; +}; + +class TaskTest : public ::testing::Test { + protected: + FakeThreadPool* thread_pool() { return &thread_pool_; } + FakeThreadPool thread_pool_; +}; + +TEST_F(TaskTest, RunTask) { + auto a = absl::make_unique(); + MockCallback callback; + a->SetWorkItem([&callback]() { callback.Run(); }); + EXPECT_EQ(a->GetState(), Task::NEW); + auto shared_a = thread_pool()->Schedule(std::move(a)).lock(); + EXPECT_NE(shared_a, nullptr); + EXPECT_EQ(shared_a->GetState(), Task::DEPENDENCIES_COMPLETED); + EXPECT_CALL(callback, Run()).Times(1); + thread_pool()->RunNext(); + EXPECT_EQ(shared_a->GetState(), Task::COMPLETED); + EXPECT_TRUE(thread_pool()->IsEmpty()); +} + +TEST_F(TaskTest, RunTaskWithDependency) { + auto a = absl::make_unique(); + auto b = absl::make_unique(); + MockCallback callback_a; + a->SetWorkItem([&callback_a]() { callback_a.Run(); }); + MockCallback callback_b; + b->SetWorkItem([&callback_b]() { callback_b.Run(); }); + EXPECT_EQ(a->GetState(), Task::NEW); + EXPECT_EQ(b->GetState(), Task::NEW); + { + ::testing::InSequence dummy; + EXPECT_CALL(callback_a, Run()).Times(1); + EXPECT_CALL(callback_b, Run()).Times(1); + } + auto shared_a = thread_pool()->Schedule(std::move(a)).lock(); + EXPECT_NE(shared_a, nullptr); + b->AddDependency(shared_a); + auto shared_b = thread_pool()->Schedule(std::move(b)).lock(); + EXPECT_NE(shared_b, nullptr); + EXPECT_EQ(shared_b->GetState(), Task::DISPATCHED); + EXPECT_EQ(shared_a->GetState(), Task::DEPENDENCIES_COMPLETED); + thread_pool()->RunNext(); + EXPECT_EQ(shared_b->GetState(), Task::DEPENDENCIES_COMPLETED); + thread_pool()->RunNext(); + EXPECT_EQ(shared_a->GetState(), Task::COMPLETED); + EXPECT_EQ(shared_b->GetState(), Task::COMPLETED); +} + +TEST_F(TaskTest, RunTaskWithTwoDependency) { + /* c \ + * a --> b --> d + */ + auto a = absl::make_unique(); + auto b = absl::make_unique(); + auto c = absl::make_unique(); + auto d = absl::make_unique(); + MockCallback callback_a; + a->SetWorkItem([&callback_a]() { callback_a.Run(); }); + MockCallback callback_b; + b->SetWorkItem([&callback_b]() { callback_b.Run(); }); + MockCallback callback_c; + c->SetWorkItem([&callback_c]() { callback_c.Run(); }); + MockCallback callback_d; + d->SetWorkItem([&callback_d]() { callback_d.Run(); }); + EXPECT_CALL(callback_a, Run()).Times(1); + EXPECT_CALL(callback_b, Run()).Times(1); + EXPECT_CALL(callback_c, Run()).Times(1); + EXPECT_CALL(callback_d, Run()).Times(1); + auto shared_a = thread_pool()->Schedule(std::move(a)).lock(); + EXPECT_NE(shared_a, nullptr); + b->AddDependency(shared_a); + auto shared_b = thread_pool()->Schedule(std::move(b)).lock(); + EXPECT_NE(shared_b, nullptr); + auto shared_c = thread_pool()->Schedule(std::move(c)).lock(); + EXPECT_NE(shared_c, nullptr); + d->AddDependency(shared_b); + d->AddDependency(shared_c); + auto shared_d = thread_pool()->Schedule(std::move(d)).lock(); + EXPECT_NE(shared_d, nullptr); + EXPECT_EQ(shared_b->GetState(), Task::DISPATCHED); + EXPECT_EQ(shared_d->GetState(), Task::DISPATCHED); + thread_pool()->RunNext(); + EXPECT_EQ(shared_a->GetState(), Task::COMPLETED); + EXPECT_EQ(shared_b->GetState(), Task::DEPENDENCIES_COMPLETED); + EXPECT_EQ(shared_c->GetState(), Task::DEPENDENCIES_COMPLETED); + thread_pool()->RunNext(); + thread_pool()->RunNext(); + EXPECT_EQ(shared_b->GetState(), Task::COMPLETED); + EXPECT_EQ(shared_c->GetState(), Task::COMPLETED); + EXPECT_EQ(shared_d->GetState(), Task::DEPENDENCIES_COMPLETED); + thread_pool()->RunNext(); + EXPECT_EQ(shared_d->GetState(), Task::COMPLETED); +} + +TEST_F(TaskTest, RunWithCompletedDependency) { + auto a = absl::make_unique(); + MockCallback callback_a; + a->SetWorkItem([&callback_a]() { callback_a.Run(); }); + auto shared_a = thread_pool()->Schedule(std::move(a)).lock(); + EXPECT_NE(shared_a, nullptr); + EXPECT_EQ(shared_a->GetState(), Task::DEPENDENCIES_COMPLETED); + EXPECT_CALL(callback_a, Run()).Times(1); + thread_pool()->RunNext(); + EXPECT_EQ(shared_a->GetState(), Task::COMPLETED); + auto b = absl::make_unique(); + MockCallback callback_b; + b->SetWorkItem([&callback_b]() { callback_b.Run(); }); + b->AddDependency(shared_a); + EXPECT_EQ(b->GetState(), Task::NEW); + auto shared_b = thread_pool()->Schedule(std::move(b)).lock(); + EXPECT_NE(shared_b, nullptr); + EXPECT_EQ(shared_b->GetState(), Task::DEPENDENCIES_COMPLETED); + EXPECT_CALL(callback_b, Run()).Times(1); + thread_pool()->RunNext(); + EXPECT_EQ(shared_b->GetState(), Task::COMPLETED); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/thread_pool.cc b/carto_ws/src/cartographer-master/cartographer/common/thread_pool.cc new file mode 100644 index 0000000..2457152 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/thread_pool.cc @@ -0,0 +1,108 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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" + +#ifndef WIN32 +#include +#endif +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/task.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +void ThreadPoolInterface::Execute(Task* task) { task->Execute(); } + +void ThreadPoolInterface::SetThreadPool(Task* task) { + task->SetThreadPool(this); +} + +ThreadPool::ThreadPool(int num_threads) { + CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!"; + absl::MutexLock locker(&mutex_); + for (int i = 0; i != num_threads; ++i) { + pool_.emplace_back([this]() { ThreadPool::DoWork(); }); + } +} + +ThreadPool::~ThreadPool() { + { + absl::MutexLock locker(&mutex_); + CHECK(running_); + running_ = false; + } + for (std::thread& thread : pool_) { + thread.join(); + } +} + +void ThreadPool::NotifyDependenciesCompleted(Task* task) { + absl::MutexLock locker(&mutex_); + auto it = tasks_not_ready_.find(task); + CHECK(it != tasks_not_ready_.end()); + task_queue_.push_back(it->second); + tasks_not_ready_.erase(it); +} + +std::weak_ptr ThreadPool::Schedule(std::unique_ptr task) { + std::shared_ptr shared_task; + { + absl::MutexLock locker(&mutex_); + auto insert_result = + tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task))); + CHECK(insert_result.second) << "Schedule called twice"; + shared_task = insert_result.first->second; + } + SetThreadPool(shared_task.get()); + return shared_task; +} + +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 + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !task_queue_.empty() || !running_; + }; + for (;;) { + std::shared_ptr task; + { + absl::MutexLock locker(&mutex_); + mutex_.Await(absl::Condition(&predicate)); + if (!task_queue_.empty()) { + task = std::move(task_queue_.front()); + task_queue_.pop_front(); + } else if (!running_) { + return; + } + } + CHECK(task); + CHECK_EQ(task->GetState(), common::Task::DEPENDENCIES_COMPLETED); + Execute(task.get()); + } +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/thread_pool.h b/carto_ws/src/cartographer-master/cartographer/common/thread_pool.h new file mode 100644 index 0000000..3f6b94c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/thread_pool.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. + */ + +#ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_ +#define CARTOGRAPHER_COMMON_THREAD_POOL_H_ + +#include +#include +#include +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "absl/synchronization/mutex.h" +#include "cartographer/common/task.h" + +namespace cartographer { +namespace common { + +class Task; + +class ThreadPoolInterface { + public: + ThreadPoolInterface() {} + virtual ~ThreadPoolInterface() {} + virtual std::weak_ptr Schedule(std::unique_ptr task) = 0; + + protected: + void Execute(Task* task); + void SetThreadPool(Task* task); + + private: + friend class Task; + + virtual void NotifyDependenciesCompleted(Task* task) = 0; +}; + +// A fixed number of threads working on tasks. Adding a task does not block. +// Tasks may be added whether or not their dependencies are completed. +// When all dependencies of a task are completed, it is queued up for execution +// in a background thread. 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 ThreadPoolInterface { + public: + explicit ThreadPool(int num_threads); + ~ThreadPool(); + + ThreadPool(const ThreadPool&) = delete; + ThreadPool& operator=(const ThreadPool&) = delete; + + // When the returned weak pointer is expired, 'task' has certainly completed, + // so dependants no longer need to add it as a dependency. + std::weak_ptr Schedule(std::unique_ptr task) + LOCKS_EXCLUDED(mutex_) override; + + private: + void DoWork(); + + void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override; + + absl::Mutex mutex_; + bool running_ GUARDED_BY(mutex_) = true; + std::vector pool_ GUARDED_BY(mutex_); + std::deque> task_queue_ GUARDED_BY(mutex_); + absl::flat_hash_map> tasks_not_ready_ + GUARDED_BY(mutex_); +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/common/thread_pool_test.cc b/carto_ws/src/cartographer-master/cartographer/common/thread_pool_test.cc new file mode 100644 index 0000000..fec82ee --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/thread_pool_test.cc @@ -0,0 +1,177 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/thread_pool.h" + +#include + +#include "absl/memory/memory.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +class Receiver { + public: + void Receive(int number) { + absl::MutexLock locker(&mutex_); + received_numbers_.push_back(number); + } + + void WaitForNumberSequence(const std::vector& expected_numbers) { + const auto predicate = + [this, &expected_numbers]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return (received_numbers_.size() >= expected_numbers.size()); + }; + absl::MutexLock locker(&mutex_); + mutex_.Await(absl::Condition(&predicate)); + EXPECT_EQ(expected_numbers, received_numbers_); + } + + absl::Mutex mutex_; + std::vector received_numbers_ GUARDED_BY(mutex_); +}; + +TEST(ThreadPoolTest, RunTask) { + ThreadPool pool(1); + Receiver receiver; + auto task = absl::make_unique(); + task->SetWorkItem([&receiver]() { receiver.Receive(1); }); + pool.Schedule(std::move(task)); + receiver.WaitForNumberSequence({1}); +} + +TEST(ThreadPoolTest, ManyTasks) { + for (int a = 0; a < 5; ++a) { + ThreadPool pool(3); + Receiver receiver; + int kNumTasks = 10; + for (int i = 0; i < kNumTasks; ++i) { + auto task = absl::make_unique(); + task->SetWorkItem([&receiver]() { receiver.Receive(1); }); + pool.Schedule(std::move(task)); + } + receiver.WaitForNumberSequence(std::vector(kNumTasks, 1)); + } +} + +TEST(ThreadPoolTest, RunWithDependency) { + ThreadPool pool(2); + Receiver receiver; + auto task_2 = absl::make_unique(); + task_2->SetWorkItem([&receiver]() { receiver.Receive(2); }); + auto task_1 = absl::make_unique(); + task_1->SetWorkItem([&receiver]() { receiver.Receive(1); }); + auto weak_task_1 = pool.Schedule(std::move(task_1)); + task_2->AddDependency(weak_task_1); + pool.Schedule(std::move(task_2)); + receiver.WaitForNumberSequence({1, 2}); +} + +TEST(ThreadPoolTest, RunWithOutOfScopeDependency) { + ThreadPool pool(2); + Receiver receiver; + auto task_2 = absl::make_unique(); + task_2->SetWorkItem([&receiver]() { receiver.Receive(2); }); + { + auto task_1 = absl::make_unique(); + task_1->SetWorkItem([&receiver]() { receiver.Receive(1); }); + auto weak_task_1 = pool.Schedule(std::move(task_1)); + task_2->AddDependency(weak_task_1); + } + pool.Schedule(std::move(task_2)); + receiver.WaitForNumberSequence({1, 2}); +} + +TEST(ThreadPoolTest, ManyDependencies) { + for (int a = 0; a < 5; ++a) { + ThreadPool pool(5); + Receiver receiver; + int kNumDependencies = 10; + auto task = absl::make_unique(); + task->SetWorkItem([&receiver]() { receiver.Receive(1); }); + for (int i = 0; i < kNumDependencies; ++i) { + auto dependency_task = absl::make_unique(); + dependency_task->SetWorkItem([]() {}); + task->AddDependency(pool.Schedule(std::move(dependency_task))); + } + pool.Schedule(std::move(task)); + receiver.WaitForNumberSequence({1}); + } +} + +TEST(ThreadPoolTest, ManyDependants) { + for (int a = 0; a < 5; ++a) { + ThreadPool pool(5); + Receiver receiver; + int kNumDependants = 10; + auto dependency_task = absl::make_unique(); + dependency_task->SetWorkItem([]() {}); + auto dependency_handle = pool.Schedule(std::move(dependency_task)); + for (int i = 0; i < kNumDependants; ++i) { + auto task = absl::make_unique(); + task->AddDependency(dependency_handle); + task->SetWorkItem([&receiver]() { receiver.Receive(1); }); + pool.Schedule(std::move(task)); + } + receiver.WaitForNumberSequence(std::vector(kNumDependants, 1)); + } +} + +TEST(ThreadPoolTest, RunWithMultipleDependencies) { + ThreadPool pool(2); + Receiver receiver; + auto task_1 = absl::make_unique(); + task_1->SetWorkItem([&receiver]() { receiver.Receive(1); }); + auto task_2a = absl::make_unique(); + task_2a->SetWorkItem([&receiver]() { receiver.Receive(2); }); + auto task_2b = absl::make_unique(); + task_2b->SetWorkItem([&receiver]() { receiver.Receive(2); }); + auto task_3 = absl::make_unique(); + task_3->SetWorkItem([&receiver]() { receiver.Receive(3); }); + /* -> task_2a \ + * task_1 /-> task_2b --> task_3 + */ + auto weak_task_1 = pool.Schedule(std::move(task_1)); + task_2a->AddDependency(weak_task_1); + auto weak_task_2a = pool.Schedule(std::move(task_2a)); + task_3->AddDependency(weak_task_1); + task_3->AddDependency(weak_task_2a); + task_2b->AddDependency(weak_task_1); + auto weak_task_2b = pool.Schedule(std::move(task_2b)); + task_3->AddDependency(weak_task_2b); + pool.Schedule(std::move(task_3)); + receiver.WaitForNumberSequence({1, 2, 2, 3}); +} + +TEST(ThreadPoolTest, RunWithFinishedDependency) { + ThreadPool pool(2); + Receiver receiver; + auto task_1 = absl::make_unique(); + task_1->SetWorkItem([&receiver]() { receiver.Receive(1); }); + auto task_2 = absl::make_unique(); + task_2->SetWorkItem([&receiver]() { receiver.Receive(2); }); + auto weak_task_1 = pool.Schedule(std::move(task_1)); + task_2->AddDependency(weak_task_1); + receiver.WaitForNumberSequence({1}); + pool.Schedule(std::move(task_2)); + receiver.WaitForNumberSequence({1, 2}); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/time.cc b/carto_ws/src/cartographer-master/cartographer/common/time.cc new file mode 100644 index 0000000..e74eead --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/time.cc @@ -0,0 +1,71 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/time.h" + +#include + +#include +#include +#include + +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +Duration FromSeconds(const double seconds) { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); +} + +double ToSeconds(const Duration duration) { + return std::chrono::duration_cast>(duration) + .count(); +} + +double ToSeconds(const std::chrono::steady_clock::duration duration) { + return std::chrono::duration_cast>(duration) + .count(); +} + +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(const int64 milliseconds) { + return std::chrono::duration_cast( + std::chrono::milliseconds(milliseconds)); +} + +double GetThreadCpuTimeSeconds() { +#ifndef WIN32 + struct timespec thread_cpu_time; + CHECK(clock_gettime(CLOCK_THREAD_CPUTIME_ID, &thread_cpu_time) == 0) + << std::strerror(errno); + return thread_cpu_time.tv_sec + 1e-9 * thread_cpu_time.tv_nsec; +#else + return 0.; +#endif +} + +} // namespace common +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/common/time.h b/carto_ws/src/cartographer-master/cartographer/common/time.h new file mode 100644 index 0000000..39ffe71 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/common/time.h @@ -0,0 +1,69 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_COMMON_TIME_H_ +#define CARTOGRAPHER_COMMON_TIME_H_ + +#include +#include +#include + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace common { + +constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds = + (719162ll * 24ll * 60ll * 60ll); + +struct UniversalTimeScaleClock { + using rep = int64; + using period = std::ratio<1, 10000000>; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + static constexpr bool is_steady = true; +}; + +// Represents Universal Time Scale durations and timestamps which are 64-bit +// integers representing the 100 nanosecond ticks since the Epoch which is +// January 1, 1 at the start of day in UTC. +using Duration = UniversalTimeScaleClock::duration; +using Time = UniversalTimeScaleClock::time_point; + +// Convenience functions to create common::Durations. +Duration FromSeconds(double seconds); +Duration FromMilliseconds(int64 milliseconds); + +// Returns the given duration in seconds. +double ToSeconds(Duration duration); +double ToSeconds(std::chrono::steady_clock::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); + +// CPU time consumed by the thread so far, in seconds. +double GetThreadCpuTimeSeconds(); + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_TIME_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth.cc b/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth.cc new file mode 100644 index 0000000..a6a8f2d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth.cc @@ -0,0 +1,153 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/ground_truth/autogenerate_ground_truth.h" + +#include +#include + +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace ground_truth { +namespace { + +std::vector ComputeCoveredDistance( + const mapping::proto::Trajectory& trajectory) { + std::vector covered_distance; + covered_distance.push_back(0.); + CHECK_GT(trajectory.node_size(), 0) + << "Trajectory does not contain any nodes."; + for (int i = 1; i < trajectory.node_size(); ++i) { + const auto last_pose = transform::ToRigid3(trajectory.node(i - 1).pose()); + const auto this_pose = transform::ToRigid3(trajectory.node(i).pose()); + covered_distance.push_back( + covered_distance.back() + + (last_pose.inverse() * this_pose).translation().norm()); + } + return covered_distance; +} + +// We pick the representative node in the middle of the submap. +// +// TODO(whess): Should we consider all nodes inserted into the submap and +// exclude, e.g. based on large relative linear or angular distance? +std::vector ComputeSubmapRepresentativeNode( + const mapping::proto::PoseGraph& pose_graph) { + std::vector submap_to_node_index; + for (const auto& constraint : pose_graph.constraint()) { + if (constraint.tag() != + mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + CHECK_EQ(constraint.submap_id().trajectory_id(), 0); + CHECK_EQ(constraint.node_id().trajectory_id(), 0); + + const int next_submap_index = static_cast(submap_to_node_index.size()); + const int submap_index = constraint.submap_id().submap_index(); + if (submap_index <= next_submap_index) { + continue; + } + + CHECK_EQ(submap_index, next_submap_index + 1); + submap_to_node_index.push_back(constraint.node_id().node_index()); + } + return submap_to_node_index; +} + +} // namespace + +proto::GroundTruth GenerateGroundTruth( + const mapping::proto::PoseGraph& pose_graph, + const double min_covered_distance, const double outlier_threshold_meters, + const double outlier_threshold_radians) { + const mapping::proto::Trajectory& trajectory = pose_graph.trajectory(0); + const std::vector covered_distance = + ComputeCoveredDistance(trajectory); + + const std::vector submap_to_node_index = + ComputeSubmapRepresentativeNode(pose_graph); + + int num_outliers = 0; + proto::GroundTruth ground_truth; + for (const auto& constraint : pose_graph.constraint()) { + // We're only interested in loop closure constraints. + if (constraint.tag() == + mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + + // For some submaps at the very end, we have not chosen a representative + // node, but those should not be part of loop closure anyway. + CHECK_EQ(constraint.submap_id().trajectory_id(), 0); + CHECK_EQ(constraint.node_id().trajectory_id(), 0); + if (constraint.submap_id().submap_index() >= + static_cast(submap_to_node_index.size())) { + continue; + } + const int matched_node = constraint.node_id().node_index(); + const int representative_node = + submap_to_node_index.at(constraint.submap_id().submap_index()); + + // Covered distance between the two should not be too small. + double covered_distance_in_constraint = + std::abs(covered_distance.at(matched_node) - + covered_distance.at(representative_node)); + if (covered_distance_in_constraint < min_covered_distance) { + continue; + } + + // Compute the transform between the nodes according to the solution and + // the constraint. + const transform::Rigid3d solution_pose1 = + transform::ToRigid3(trajectory.node(representative_node).pose()); + const transform::Rigid3d solution_pose2 = + transform::ToRigid3(trajectory.node(matched_node).pose()); + const transform::Rigid3d solution = + solution_pose1.inverse() * solution_pose2; + + const transform::Rigid3d submap_solution = transform::ToRigid3( + trajectory.submap(constraint.submap_id().submap_index()).pose()); + const transform::Rigid3d submap_solution_to_node_solution = + solution_pose1.inverse() * submap_solution; + const transform::Rigid3d node_to_submap_constraint = + transform::ToRigid3(constraint.relative_pose()); + const transform::Rigid3d expected = + submap_solution_to_node_solution * node_to_submap_constraint; + + const transform::Rigid3d error = solution * expected.inverse(); + + if (error.translation().norm() > outlier_threshold_meters || + transform::GetAngle(error) > outlier_threshold_radians) { + ++num_outliers; + continue; + } + auto* const new_relation = ground_truth.add_relation(); + new_relation->set_timestamp1( + trajectory.node(representative_node).timestamp()); + new_relation->set_timestamp2(trajectory.node(matched_node).timestamp()); + *new_relation->mutable_expected() = transform::ToProto(expected); + new_relation->set_covered_distance(covered_distance_in_constraint); + } + LOG(INFO) << "Generated " << ground_truth.relation_size() + << " relations and ignored " << num_outliers << " outliers."; + return ground_truth; +} + +} // namespace ground_truth +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth.h b/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth.h new file mode 100644 index 0000000..cdb101f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth.h @@ -0,0 +1,37 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_GROUND_TRUTH_AUTOGENERATE_GROUND_TRUTH_H_ +#define CARTOGRAPHER_GROUND_TRUTH_AUTOGENERATE_GROUND_TRUTH_H_ + +#include "cartographer/ground_truth/proto/relations.pb.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" + +namespace cartographer { +namespace ground_truth { + +// Generates GroundTruth proto from the given pose graph using the specified +// criteria parameters. See +// 'https://google-cartographer.readthedocs.io/en/latest/evaluation.html' for +// more details. +proto::GroundTruth GenerateGroundTruth( + const mapping::proto::PoseGraph& pose_graph, double min_covered_distance, + double outlier_threshold_meters, double outlier_threshold_radians); + +} // namespace ground_truth +} // namespace cartographer + +#endif // CARTOGRAPHER_GROUND_TRUTH_AUTOGENERATE_GROUND_TRUTH_H diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth_main.cc new file mode 100644 index 0000000..aeb1684 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/autogenerate_ground_truth_main.cc @@ -0,0 +1,102 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "cartographer/common/port.h" +#include "cartographer/ground_truth/autogenerate_ground_truth.h" +#include "cartographer/ground_truth/proto/relations.pb.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/transform/transform.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string(pose_graph_filename, "", + "Proto stream file containing the pose graph used to generate " + "ground truth data."); +DEFINE_string(output_filename, "", "File to write the ground truth proto to."); +DEFINE_double(min_covered_distance, 100., + "Minimum covered distance in meters before a loop closure is " + "considered a candidate for autogenerated ground truth."); +DEFINE_double(outlier_threshold_meters, 0.15, + "Distance in meters beyond which constraints are considered " + "outliers."); +DEFINE_double(outlier_threshold_radians, 0.02, + "Distance in radians beyond which constraints are considered " + "outliers."); + +namespace cartographer { +namespace ground_truth { +namespace { + +void Run(const std::string& pose_graph_filename, + const std::string& output_filename, const double min_covered_distance, + const double outlier_threshold_meters, + const double outlier_threshold_radians) { + LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'..."; + mapping::proto::PoseGraph pose_graph = + io::DeserializePoseGraphFromFile(pose_graph_filename); + + LOG(INFO) << "Autogenerating ground truth relations..."; + const proto::GroundTruth ground_truth = + GenerateGroundTruth(pose_graph, min_covered_distance, + outlier_threshold_meters, outlier_threshold_radians); + LOG(INFO) << "Writing " << ground_truth.relation_size() << " relations to '" + << output_filename << "'."; + { + std::ofstream output_stream(output_filename, + std::ios_base::out | std::ios_base::binary); + CHECK(ground_truth.SerializeToOstream(&output_stream)) + << "Could not serialize ground truth data."; + output_stream.close(); + CHECK(output_stream) << "Could not write ground truth data."; + } +} + +} // namespace +} // namespace ground_truth +} // namespace cartographer + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + google::SetUsageMessage( + "\n\n" + "This program semi-automatically generates ground truth data from a\n" + "pose graph proto.\n" + "\n" + "The input should contain a single trajectory and should have been\n" + "manually assessed to be correctly loop closed. Small local distortions\n" + "are acceptable if they are tiny compared to the errors we want to\n" + "assess using the generated ground truth data.\n" + "\n" + "All loop closure constraints separated by long covered distance are\n" + "included in the output. Outliers are removed.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + + if (FLAGS_pose_graph_filename.empty() || FLAGS_output_filename.empty()) { + google::ShowUsageWithFlagsRestrict(argv[0], "autogenerate_ground_truth"); + return EXIT_FAILURE; + } + ::cartographer::ground_truth::Run( + FLAGS_pose_graph_filename, FLAGS_output_filename, + FLAGS_min_covered_distance, FLAGS_outlier_threshold_meters, + FLAGS_outlier_threshold_radians); +} diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/compute_relations_metrics_main.cc b/carto_ws/src/cartographer-master/cartographer/ground_truth/compute_relations_metrics_main.cc new file mode 100644 index 0000000..0aba0b7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/compute_relations_metrics_main.cc @@ -0,0 +1,238 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/ground_truth/proto/relations.pb.h" +#include "cartographer/ground_truth/relations_text_file.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string( + pose_graph_filename, "", + "Proto stream file containing the pose graph used to assess quality."); +DEFINE_string(relations_filename, "", + "Relations file containing the ground truth."); +DEFINE_bool(read_text_file_with_unix_timestamps, false, + "Enable support for the relations text files as in the paper. " + "Default is to read from a GroundTruth proto file."); +DEFINE_bool(write_relation_metrics, false, + "Enable exporting relation metrics as comma-separated values to " + "[pose_graph_filename].relation_metrics.csv"); + +namespace cartographer { +namespace ground_truth { +namespace { + +struct Error { + double translational_squared; + double rotational_squared; +}; + +// TODO(whess): This gives different results for the translational error if +// 'pose1' and 'pose2' are swapped and 'expected' is inverted. Consider a +// different way to compute translational error. Maybe just look at the +// absolute difference in translation norms of each relative transform as a +// lower bound of the translational error. +Error ComputeError(const transform::Rigid3d& pose1, + const transform::Rigid3d& pose2, + const transform::Rigid3d& expected) { + const transform::Rigid3d error = + (pose1.inverse() * pose2) * expected.inverse(); + return Error{error.translation().squaredNorm(), + common::Pow2(transform::GetAngle(error))}; +} + +std::string MeanAndStdDevString(const std::vector& values) { + CHECK_GE(values.size(), 2); + const double mean = + std::accumulate(values.begin(), values.end(), 0.) / values.size(); + double sum_of_squared_differences = 0.; + for (const double value : values) { + sum_of_squared_differences += common::Pow2(value - mean); + } + const double standard_deviation = + std::sqrt(sum_of_squared_differences / (values.size() - 1)); + std::ostringstream out; + out << std::fixed << std::setprecision(5) << mean << " +/- " + << standard_deviation; + return std::string(out.str()); +} + +std::string StatisticsString(const std::vector& errors) { + std::vector translational_errors; + std::vector squared_translational_errors; + std::vector rotational_errors_degrees; + std::vector squared_rotational_errors_degrees; + for (const Error& error : errors) { + translational_errors.push_back(std::sqrt(error.translational_squared)); + squared_translational_errors.push_back(error.translational_squared); + rotational_errors_degrees.push_back( + common::RadToDeg(std::sqrt(error.rotational_squared))); + squared_rotational_errors_degrees.push_back( + common::Pow2(rotational_errors_degrees.back())); + } + return "Abs translational error " + + MeanAndStdDevString(translational_errors) + + " m\n" + "Sqr translational error " + + MeanAndStdDevString(squared_translational_errors) + + " m^2\n" + "Abs rotational error " + + MeanAndStdDevString(rotational_errors_degrees) + + " deg\n" + "Sqr rotational error " + + MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n"; +} + +void WriteRelationMetricsToFile(const std::vector& errors, + const proto::GroundTruth& ground_truth, + const std::string& relation_metrics_filename) { + std::ofstream relation_errors_file; + std::string log_file_path; + LOG(INFO) << "Writing relation metrics to '" + relation_metrics_filename + + "'..."; + relation_errors_file.open(relation_metrics_filename); + relation_errors_file + << "translational_error,squared_translational_error,rotational_" + "errors_degree,squared_rotational_errors_degree," + "expected_translation_x,expected_translation_y,expected_" + "translation_z,expected_rotation_w,expected_rotation_x," + "expected_rotation_y,expected_rotation_z,covered_distance\n"; + for (int relation_index = 0; relation_index < ground_truth.relation_size(); + ++relation_index) { + const Error& error = errors[relation_index]; + const proto::Relation& relation = ground_truth.relation(relation_index); + double translational_error = std::sqrt(error.translational_squared); + double squared_translational_error = error.translational_squared; + double rotational_errors_degree = + common::RadToDeg(std::sqrt(error.rotational_squared)); + double squared_rotational_errors_degree = + common::Pow2(rotational_errors_degree); + relation_errors_file << translational_error << "," + << squared_translational_error << "," + << rotational_errors_degree << "," + << squared_rotational_errors_degree << "," + << relation.expected().translation().x() << "," + << relation.expected().translation().y() << "," + << relation.expected().translation().z() << "," + << relation.expected().rotation().w() << "," + << relation.expected().rotation().x() << "," + << relation.expected().rotation().y() << "," + << relation.expected().rotation().z() << "," + << relation.covered_distance() << "\n"; + } + relation_errors_file.close(); +} + +transform::Rigid3d LookupTransform( + const transform::TransformInterpolationBuffer& + transform_interpolation_buffer, + const common::Time time) { + const common::Time earliest_time = + transform_interpolation_buffer.earliest_time(); + if (transform_interpolation_buffer.Has(time)) { + return transform_interpolation_buffer.Lookup(time); + } else if (time < earliest_time) { + return transform_interpolation_buffer.Lookup(earliest_time); + } + return transform_interpolation_buffer.Lookup( + transform_interpolation_buffer.latest_time()); +} + +void Run(const std::string& pose_graph_filename, + const std::string& relations_filename, + const bool read_text_file_with_unix_timestamps, + const bool write_relation_metrics) { + LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'..."; + mapping::proto::PoseGraph pose_graph = + io::DeserializePoseGraphFromFile(pose_graph_filename); + + const transform::TransformInterpolationBuffer transform_interpolation_buffer( + pose_graph.trajectory(0)); + + proto::GroundTruth ground_truth; + if (read_text_file_with_unix_timestamps) { + LOG(INFO) << "Reading relations from '" << relations_filename << "'..."; + ground_truth = ReadRelationsTextFile(relations_filename); + } else { + LOG(INFO) << "Reading ground truth from '" << relations_filename << "'..."; + std::ifstream ground_truth_stream(relations_filename.c_str(), + std::ios::binary); + CHECK(ground_truth.ParseFromIstream(&ground_truth_stream)); + } + + std::vector errors; + for (const auto& relation : ground_truth.relation()) { + const auto pose1 = + LookupTransform(transform_interpolation_buffer, + common::FromUniversal(relation.timestamp1())); + const auto pose2 = + LookupTransform(transform_interpolation_buffer, + common::FromUniversal(relation.timestamp2())); + const transform::Rigid3d expected = + transform::ToRigid3(relation.expected()); + errors.push_back(ComputeError(pose1, pose2, expected)); + } + + const std::string relation_metrics_filename = + pose_graph_filename + ".relation_metrics.csv"; + if (write_relation_metrics) { + WriteRelationMetricsToFile(errors, ground_truth, relation_metrics_filename); + } + + LOG(INFO) << "Result:\n" << StatisticsString(errors); +} + +} // namespace +} // namespace ground_truth +} // namespace cartographer + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + google::SetUsageMessage( + "\n\n" + "This program computes the relation based metric described in:\n" + "R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti,\n" + "C. Stachniss, and A. Kleiner, \"On measuring the accuracy of SLAM\n" + "algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009."); + google::ParseCommandLineFlags(&argc, &argv, true); + + if (FLAGS_pose_graph_filename.empty() || FLAGS_relations_filename.empty()) { + google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics"); + return EXIT_FAILURE; + } + + ::cartographer::ground_truth::Run( + FLAGS_pose_graph_filename, FLAGS_relations_filename, + FLAGS_read_text_file_with_unix_timestamps, FLAGS_write_relation_metrics); +} diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/proto/relations.proto b/carto_ws/src/cartographer-master/cartographer/ground_truth/proto/relations.proto new file mode 100644 index 0000000..fec2f43 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/proto/relations.proto @@ -0,0 +1,33 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.ground_truth.proto; + +import "cartographer/transform/proto/transform.proto"; + +message Relation { + int64 timestamp1 = 1; + int64 timestamp2 = 2; + + // The 'expected' relative transform of the tracking frame from 'timestamp2' + // to 'timestamp1'. + transform.proto.Rigid3d expected = 3; + double covered_distance = 4; +} + +message GroundTruth { + repeated Relation relation = 1; +} diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/relations_text_file.cc b/carto_ws/src/cartographer-master/cartographer/ground_truth/relations_text_file.cc new file mode 100644 index 0000000..50161fb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/relations_text_file.cc @@ -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. + */ + +#include "cartographer/ground_truth/relations_text_file.h" + +#include + +#include "cartographer/common/time.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace ground_truth { + +namespace { + +common::Time UnixToCommonTime(double unix_time) { + constexpr int64 kUtsTicksPerSecond = 10000000; + return common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds * + kUtsTicksPerSecond) + + common::FromSeconds(unix_time); +} + +} // namespace + +proto::GroundTruth ReadRelationsTextFile( + const std::string& relations_filename) { + proto::GroundTruth ground_truth; + std::ifstream relations_stream(relations_filename.c_str()); + double unix_time_1, unix_time_2, x, y, z, roll, pitch, yaw; + while (relations_stream >> unix_time_1 >> unix_time_2 >> x >> y >> z >> + roll >> pitch >> yaw) { + const common::Time common_time_1 = UnixToCommonTime(unix_time_1); + const common::Time common_time_2 = UnixToCommonTime(unix_time_2); + const transform::Rigid3d expected = + transform::Rigid3d(transform::Rigid3d::Vector(x, y, z), + transform::RollPitchYaw(roll, pitch, yaw)); + auto* const new_relation = ground_truth.add_relation(); + new_relation->set_timestamp1(common::ToUniversal(common_time_1)); + new_relation->set_timestamp2(common::ToUniversal(common_time_2)); + *new_relation->mutable_expected() = transform::ToProto(expected); + } + CHECK(relations_stream.eof()); + return ground_truth; +} + +} // namespace ground_truth +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/ground_truth/relations_text_file.h b/carto_ws/src/cartographer-master/cartographer/ground_truth/relations_text_file.h new file mode 100644 index 0000000..a05033e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/ground_truth/relations_text_file.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. + */ + +#ifndef CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_ +#define CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_ + +#include + +#include "cartographer/common/port.h" +#include "cartographer/ground_truth/proto/relations.pb.h" + +namespace cartographer { +namespace ground_truth { + +// Reads a text file and converts it to a GroundTruth proto. Each line contains: +// time1 time2 x y z roll pitch yaw +// using Unix epoch timestamps. +// +// This is the format used in the relations files provided for: +// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss, +// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous +// Robots, vol. 27, no. 4, pp. 387–407, 2009. +proto::GroundTruth ReadRelationsTextFile(const std::string& relations_filename); + +} // namespace ground_truth +} // namespace cartographer + +#endif // CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/color.cc b/carto_ws/src/cartographer-master/cartographer/io/color.cc new file mode 100644 index 0000000..34a4deb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/color.cc @@ -0,0 +1,71 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/color.h" + +#include + +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +namespace { + +constexpr float kInitialHue = 0.69f; +constexpr float kSaturation = 0.85f; +constexpr float kValue = 0.77f; + +FloatColor HsvToRgb(const float h, const float s, const float v) { + const float h_6 = (h == 1.f) ? 0.f : 6 * h; + const int h_i = std::floor(h_6); + const float f = h_6 - h_i; + + const float p = v * (1.f - s); + const float q = v * (1.f - f * s); + const float t = v * (1.f - (1.f - f) * s); + + if (h_i == 0) { + return {{v, t, p}}; + } else if (h_i == 1) { + return {{q, v, p}}; + } else if (h_i == 2) { + return {{p, v, t}}; + } else if (h_i == 3) { + return {{p, q, v}}; + } else if (h_i == 4) { + return {{t, p, v}}; + } else if (h_i == 5) { + return {{v, p, q}}; + } else { + return {{0.f, 0.f, 0.f}}; + } +} + +} // namespace + +FloatColor GetColor(int id) { + CHECK_GE(id, 0); + // Uniform color sampling using the golden ratio from + // http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/ + constexpr float kGoldenRatioConjugate = 0.6180339887498949f; + const float hue = std::fmod(kInitialHue + kGoldenRatioConjugate * id, 1.f); + return HsvToRgb(hue, kSaturation, kValue); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/color.h b/carto_ws/src/cartographer-master/cartographer/io/color.h new file mode 100644 index 0000000..6e5ff21 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/color.h @@ -0,0 +1,55 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_COLOR_H_ +#define CARTOGRAPHER_IO_COLOR_H_ + +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" + +namespace cartographer { +namespace io { + +using Uint8Color = std::array; +using FloatColor = std::array; + +// A function for on-demand generation of a color palette, with every two +// direct successors having large contrast. +FloatColor GetColor(int id); + +inline uint8 FloatComponentToUint8(float c) { + return static_cast(cartographer::common::RoundToInt( + cartographer::common::Clamp(c, 0.f, 1.f) * 255)); +} + +inline float Uint8ComponentToFloat(uint8 c) { return c / 255.f; } + +inline Uint8Color ToUint8Color(const FloatColor& color) { + return {{FloatComponentToUint8(color[0]), FloatComponentToUint8(color[1]), + FloatComponentToUint8(color[2])}}; +} + +inline FloatColor ToFloatColor(const Uint8Color& color) { + return {{Uint8ComponentToFloat(color[0]), Uint8ComponentToFloat(color[1]), + Uint8ComponentToFloat(color[2])}}; +} + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_COLOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/coloring_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/coloring_points_processor.cc new file mode 100644 index 0000000..9375071 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/coloring_points_processor.cc @@ -0,0 +1,60 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/coloring_points_processor.h" + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +ColoringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const std::string frame_id = dictionary->GetString("frame_id"); + const std::vector color_values = + dictionary->GetDictionary("color")->GetArrayValuesAsDoubles(); + const Uint8Color color = {{static_cast(color_values[0]), + static_cast(color_values[1]), + static_cast(color_values[2])}}; + return absl::make_unique(ToFloatColor(color), + frame_id, next); +} + +ColoringPointsProcessor::ColoringPointsProcessor(const FloatColor& color, + const std::string& frame_id, + PointsProcessor* const next) + : color_(color), frame_id_(frame_id), next_(next) {} + +void ColoringPointsProcessor::Process(std::unique_ptr batch) { + if (batch->frame_id == frame_id_) { + batch->colors.clear(); + for (size_t i = 0; i < batch->points.size(); ++i) { + batch->colors.push_back(color_); + } + } + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult ColoringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/coloring_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/coloring_points_processor.h new file mode 100644 index 0000000..4df4dd6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/coloring_points_processor.h @@ -0,0 +1,57 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Colors points with a fixed color by frame_id. +class ColoringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "color_points"; + + ColoringPointsProcessor(const FloatColor& color, const std::string& frame_id, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~ColoringPointsProcessor() override{}; + + ColoringPointsProcessor(const ColoringPointsProcessor&) = delete; + ColoringPointsProcessor& operator=(const ColoringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const FloatColor color_; + const std::string frame_id_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/counting_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/counting_points_processor.cc new file mode 100644 index 0000000..995461d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/counting_points_processor.cc @@ -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. + */ + +#include "cartographer/io/counting_points_processor.h" + +#include "absl/memory/memory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +CountingPointsProcessor::CountingPointsProcessor(PointsProcessor* next) + : num_points_(0), next_(next) {} + +std::unique_ptr +CountingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique(next); +} + +void CountingPointsProcessor::Process(std::unique_ptr batch) { + num_points_ += batch->points.size(); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult CountingPointsProcessor::Flush() { + switch (next_->Flush()) { + case FlushResult::kFinished: + LOG(INFO) << "Processed " << num_points_ << " and finishing."; + return FlushResult::kFinished; + + case FlushResult::kRestartStream: + LOG(INFO) << "Processed " << num_points_ << " and restarting stream."; + num_points_ = 0; + return FlushResult::kRestartStream; + } + LOG(FATAL); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/counting_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/counting_points_processor.h new file mode 100644 index 0000000..4fbe8ae --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/counting_points_processor.h @@ -0,0 +1,52 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_COUNTING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Passes through points, but keeps track of how many points it saw and output +// that on Flush. +class CountingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "dump_num_points"; + explicit CountingPointsProcessor(PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~CountingPointsProcessor() override {} + + CountingPointsProcessor(const CountingPointsProcessor&) = delete; + CountingPointsProcessor& operator=(const CountingPointsProcessor&) = delete; + + void Process(std::unique_ptr points) override; + FlushResult Flush() override; + + private: + int64 num_points_; + PointsProcessor* next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/draw_trajectories.cc b/carto_ws/src/cartographer-master/cartographer/io/draw_trajectories.cc new file mode 100644 index 0000000..0af139b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/draw_trajectories.cc @@ -0,0 +1,69 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/draw_trajectories.h" + +#include "cartographer/io/image.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace io { + +void DrawTrajectory(const mapping::proto::Trajectory& trajectory, + const FloatColor& color, + const PoseToPixelFunction& pose_to_pixel, + cairo_surface_t* surface) { + if (trajectory.node_size() == 0) { + return; + } + constexpr double kTrajectoryWidth = 4.; + constexpr double kTrajectoryEndMarkers = 6.; + constexpr double kAlpha = 0.7; + + auto cr = ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface)); + + cairo_set_source_rgba(cr.get(), color[0], color[1], color[2], kAlpha); + cairo_set_line_width(cr.get(), kTrajectoryWidth); + + for (const auto& node : trajectory.node()) { + const Eigen::Array2i pixel = + pose_to_pixel(transform::ToRigid3(node.pose())); + cairo_line_to(cr.get(), pixel.x(), pixel.y()); + } + cairo_stroke(cr.get()); + + // Draw beginning and end markers. + { + const Eigen::Array2i pixel = + pose_to_pixel(transform::ToRigid3(trajectory.node(0).pose())); + cairo_set_source_rgba(cr.get(), 0., 1., 0., kAlpha); + cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0, + 2 * M_PI); + cairo_fill(cr.get()); + } + { + const Eigen::Array2i pixel = pose_to_pixel(transform::ToRigid3( + trajectory.node(trajectory.node_size() - 1).pose())); + cairo_set_source_rgba(cr.get(), 1., 0., 0., kAlpha); + cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0, + 2 * M_PI); + cairo_fill(cr.get()); + } + cairo_surface_flush(surface); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/draw_trajectories.h b/carto_ws/src/cartographer-master/cartographer/io/draw_trajectories.h new file mode 100644 index 0000000..e519698 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/draw_trajectories.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. + */ + +#ifndef CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_ +#define CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_ + +#include "cairo/cairo.h" +#include "cartographer/io/color.h" +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace io { + +using PoseToPixelFunction = + std::function; + +// Draws the 'trajectory' with the given 'color' onto 'surface'. The +// 'pose_to_pixel' function must translate a trajectory node's position into the +// pixel on 'surface'. +void DrawTrajectory(const mapping::proto::Trajectory& trajectory, + const FloatColor& color, + const PoseToPixelFunction& pose_to_pixel, + cairo_surface_t* surface); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer.cc b/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer.cc new file mode 100644 index 0000000..424a967 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer.cc @@ -0,0 +1,55 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/fake_file_writer.h" + +#include "glog/logging.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { + +FakeFileWriter::FakeFileWriter(const std::string& filename, + std::shared_ptr> content) + : is_closed_(false), content_(content), filename_(filename) { + CHECK(content != nullptr); +} + +bool FakeFileWriter::Write(const char* const data, const size_t len) { + EXPECT_FALSE(is_closed_); + content_->insert(content_->end(), data, data + len); + return true; +} + +bool FakeFileWriter::Close() { + EXPECT_FALSE(is_closed_); + is_closed_ = true; + return true; +} + +bool FakeFileWriter::WriteHeader(const char* const data, const size_t len) { + EXPECT_FALSE(is_closed_); + if (content_->size() < len) { + content_->resize(len); + } + std::copy(data, data + len, content_->begin()); + return true; +} + +std::string FakeFileWriter::GetFilename() { return filename_; } + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer.h b/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer.h new file mode 100644 index 0000000..fb3d5e4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer.h @@ -0,0 +1,50 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_FAKE_FILE_WRITER_H_ +#define CARTOGRAPHER_IO_FAKE_FILE_WRITER_H_ + +#include +#include +#include + +#include "cartographer/io/file_writer.h" + +namespace cartographer { +namespace io { + +// Fakes a FileWriter by just writing the data to a std::vector. +class FakeFileWriter : public FileWriter { + public: + FakeFileWriter(const std::string& filename, + std::shared_ptr> content); + ~FakeFileWriter() override = default; + + bool WriteHeader(const char* data, size_t len) override; + bool Write(const char* data, size_t len) override; + bool Close() override; + std::string GetFilename() override; + + private: + bool is_closed_; + std::shared_ptr> content_; + std::string filename_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FAKE_FILE_WRITER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer_test.cc b/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer_test.cc new file mode 100644 index 0000000..711e916 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/fake_file_writer_test.cc @@ -0,0 +1,130 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/fake_file_writer.h" + +#include + +#include "glog/logging.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +std::string toString(const std::vector& data) { + return std::string(data.data(), data.size()); +} + +TEST(FakeFileWriter, Filename) { + auto content = std::make_shared>(); + FakeFileWriter writer("file", content); + EXPECT_EQ("file", writer.GetFilename()); +} + +TEST(FakeFileWriter, CloseStream) { + auto content = std::make_shared>(); + FakeFileWriter writer("file", content); + EXPECT_TRUE(writer.Close()); + EXPECT_EQ("", toString(*content)); +} + +TEST(FakeFileWriter, WriteHeader) { + auto content = std::make_shared>(); + const std::string header("dummy header"); + const std::string header_2("dummy header 2"); + FakeFileWriter writer("file", content); + + EXPECT_TRUE(writer.WriteHeader(header.c_str(), header.size())); + EXPECT_EQ("dummy header", toString(*content)); + + EXPECT_TRUE(writer.WriteHeader(header_2.c_str(), header_2.size())); + EXPECT_EQ("dummy header 2", toString(*content)); + + EXPECT_TRUE(writer.Close()); + EXPECT_EQ("dummy header 2", toString(*content)); +} + +TEST(FakeFileWriter, Write) { + auto content = std::make_shared>(); + const std::vector data_stream = {"data 1", "data 2"}; + FakeFileWriter writer("file", content); + + for (const auto& data : data_stream) { + EXPECT_TRUE(writer.Write(data.c_str(), data.size())); + } + + EXPECT_EQ("data 1data 2", toString(*content)); + EXPECT_TRUE(writer.Close()); + EXPECT_EQ("data 1data 2", toString(*content)); +} + +TEST(FakeFileWriter, HeaderAndWrite) { + auto content = std::make_shared>(); + const std::string header("dummy header"); + const std::vector data_stream = {"data 1", "data 2"}; + FakeFileWriter writer("file", content); + + EXPECT_TRUE(writer.WriteHeader(header.c_str(), header.size())); + EXPECT_EQ("dummy header", toString(*content)); + + for (const auto& data : data_stream) { + EXPECT_TRUE(writer.Write(data.c_str(), data.size())); + } + + EXPECT_TRUE(writer.Close()); + EXPECT_EQ("dummy headerdata 1data 2", toString(*content)); +} + +TEST(FakeFileWriter, WriteTerminatedString) { + auto content = std::make_shared>(); + std::vector data_stream = {'d', 'a', 't', 'a', '\0', ' ', '1'}; + FakeFileWriter writer("file", content); + EXPECT_TRUE(writer.Write(data_stream.data(), data_stream.size())); + EXPECT_EQ(data_stream, *content); +} + +TEST(FakeFileWriter, WriteTerminatedHeaderString) { + auto content = std::make_shared>(); + std::vector header = {'h', 'e', 'a', 'd', '\0', ' ', 'e', 'r'}; + FakeFileWriter writer("file", content); + EXPECT_TRUE(writer.WriteHeader(header.data(), header.size())); + EXPECT_EQ(header, *content); +} + +TEST(FakeFileWriter, HeaderAndWriteTerminatedString) { + auto content = std::make_shared>(); + std::vector header = {'d', 'a', 't', 'a', '\0', ' ', '1'}; + std::vector data = {'h', 'e', 'a', 'd', '\0', ' ', 'e', 'r'}; + + FakeFileWriter writer("file", content); + EXPECT_TRUE(writer.WriteHeader(header.data(), header.size())); + EXPECT_EQ(header, *content); + + EXPECT_TRUE(writer.Write(data.data(), data.size())); + + std::vector expected_output = header; + expected_output.insert(expected_output.end(), data.begin(), data.end()); + + EXPECT_EQ(expected_output, *content); + + EXPECT_TRUE(writer.Close()); + EXPECT_EQ(expected_output, *content); +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/file_writer.cc b/carto_ws/src/cartographer-master/cartographer/io/file_writer.cc new file mode 100644 index 0000000..97d4481 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/file_writer.cc @@ -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. + */ + +#include "cartographer/io/file_writer.h" + +namespace cartographer { +namespace io { + +StreamFileWriter::StreamFileWriter(const std::string& filename) + : filename_(filename), out_(filename, std::ios::out | std::ios::binary) {} + +StreamFileWriter::~StreamFileWriter() {} + +bool StreamFileWriter::Write(const char* const data, const size_t len) { + if (out_.bad()) { + return false; + } + out_.write(data, len); + return !out_.bad(); +} + +bool StreamFileWriter::Close() { + if (out_.bad()) { + return false; + } + out_.close(); + return !out_.bad(); +} + +bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) { + if (out_.bad()) { + return false; + } + out_.flush(); + out_.seekp(0); + return Write(data, len); +} + +std::string StreamFileWriter::GetFilename() { return filename_; } + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/file_writer.h b/carto_ws/src/cartographer-master/cartographer/io/file_writer.h new file mode 100644 index 0000000..cba60da --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/file_writer.h @@ -0,0 +1,71 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_FILE_WRITER_H_ +#define CARTOGRAPHER_IO_FILE_WRITER_H_ + +#include +#include +#include + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace io { + +// Simple abstraction for a file. +class FileWriter { + public: + FileWriter() {} + FileWriter(const FileWriter&) = delete; + FileWriter& operator=(const FileWriter&) = delete; + + virtual ~FileWriter() {} + + // Write 'data' to the beginning of the file. This is required to overwrite + // fixed size headers which contain the number of points once we actually know + // how many points there are. + virtual bool WriteHeader(const char* data, size_t len) = 0; + + virtual bool Write(const char* data, size_t len) = 0; + virtual bool Close() = 0; + virtual std::string GetFilename() = 0; +}; + +// An Implementation of file using std::ofstream. +class StreamFileWriter : public FileWriter { + public: + ~StreamFileWriter() override; + + StreamFileWriter(const std::string& filename); + + bool Write(const char* data, size_t len) override; + bool WriteHeader(const char* data, size_t len) override; + bool Close() override; + std::string GetFilename() override; + + private: + const std::string filename_; + std::ofstream out_; +}; + +using FileWriterFactory = + std::function(const std::string& filename)>; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FILE_WRITER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/fixed_ratio_sampling_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/fixed_ratio_sampling_points_processor.cc new file mode 100644 index 0000000..6b07f3c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/fixed_ratio_sampling_points_processor.cc @@ -0,0 +1,68 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/fixed_ratio_sampling_points_processor.h" + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +FixedRatioSamplingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const double sampling_ratio(dictionary->GetDouble("sampling_ratio")); + CHECK_LT(0., sampling_ratio) << "Sampling ratio <= 0 makes no sense."; + CHECK_LT(sampling_ratio, 1.) << "Sampling ratio >= 1 makes no sense."; + return absl::make_unique(sampling_ratio, + next); +} + +FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor( + const double sampling_ratio, PointsProcessor* next) + : sampling_ratio_(sampling_ratio), + next_(next), + sampler_(new common::FixedRatioSampler(sampling_ratio_)) {} + +void FixedRatioSamplingPointsProcessor::Process( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + if (!sampler_->Pulse()) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult FixedRatioSamplingPointsProcessor::Flush() { + switch (next_->Flush()) { + case PointsProcessor::FlushResult::kFinished: + return PointsProcessor::FlushResult::kFinished; + + case PointsProcessor::FlushResult::kRestartStream: + sampler_ = absl::make_unique(sampling_ratio_); + return PointsProcessor::FlushResult::kRestartStream; + } + LOG(FATAL); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/fixed_ratio_sampling_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/fixed_ratio_sampling_points_processor.h new file mode 100644 index 0000000..0c8763f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/fixed_ratio_sampling_points_processor.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. + */ + +#ifndef CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1. +// makes this filter a no-op. +class FixedRatioSamplingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "fixed_ratio_sampler"; + + FixedRatioSamplingPointsProcessor(double sampling_ratio, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~FixedRatioSamplingPointsProcessor() override{}; + + FixedRatioSamplingPointsProcessor(const FixedRatioSamplingPointsProcessor&) = + delete; + FixedRatioSamplingPointsProcessor& operator=( + const FixedRatioSamplingPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double sampling_ratio_; + PointsProcessor* const next_; + std::unique_ptr sampler_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/frame_id_filtering_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/frame_id_filtering_points_processor.cc new file mode 100644 index 0000000..5fbe4c4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/frame_id_filtering_points_processor.cc @@ -0,0 +1,70 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/frame_id_filtering_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +FrameIdFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next) { + std::vector keep_frames, drop_frames; + if (dictionary->HasKey("keep_frames")) { + keep_frames = + dictionary->GetDictionary("keep_frames")->GetArrayValuesAsStrings(); + } + if (dictionary->HasKey("drop_frames")) { + drop_frames = + dictionary->GetDictionary("drop_frames")->GetArrayValuesAsStrings(); + } + return absl::make_unique( + absl::flat_hash_set(keep_frames.begin(), keep_frames.end()), + absl::flat_hash_set(drop_frames.begin(), drop_frames.end()), + next); +} + +FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor( + const absl::flat_hash_set& keep_frame_ids, + const absl::flat_hash_set& drop_frame_ids, + PointsProcessor* next) + : keep_frame_ids_(keep_frame_ids), + drop_frame_ids_(drop_frame_ids), + next_(next) { + CHECK_NE(keep_frame_ids.empty(), drop_frame_ids.empty()) + << "You have to specify exactly one of the `keep_frames` property or the " + << "`drop_frames` property, but not both at the same time."; +} + +void FrameIdFilteringPointsProcessor::Process( + std::unique_ptr batch) { + if ((!keep_frame_ids_.empty() && keep_frame_ids_.count(batch->frame_id)) || + (!drop_frame_ids_.empty() && !drop_frame_ids_.count(batch->frame_id))) { + next_->Process(std::move(batch)); + } +} + +PointsProcessor::FlushResult FrameIdFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/frame_id_filtering_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/frame_id_filtering_points_processor.h new file mode 100644 index 0000000..a35cb60 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/frame_id_filtering_points_processor.h @@ -0,0 +1,58 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_FRAME_ID_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_FRAME_ID_FILTERING_POINTS_PROCESSOR_H_ + +#include "absl/container/flat_hash_set.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Filters all points with blacklisted frame id or a non-whitelisted frame id. +// Note that you can either specify the whitelist or the blacklist, but not both +// at the same time. +class FrameIdFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "frame_id_filter"; + FrameIdFilteringPointsProcessor( + const absl::flat_hash_set& keep_frame_ids, + const absl::flat_hash_set& drop_frame_ids, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + ~FrameIdFilteringPointsProcessor() override {} + + FrameIdFilteringPointsProcessor(const FrameIdFilteringPointsProcessor&) = + delete; + FrameIdFilteringPointsProcessor& operator=( + const FrameIdFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const absl::flat_hash_set keep_frame_ids_; + const absl::flat_hash_set drop_frame_ids_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FRAME_ID_FILTERING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/hybrid_grid_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/hybrid_grid_points_processor.cc new file mode 100644 index 0000000..c81c1d5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/hybrid_grid_points_processor.cc @@ -0,0 +1,70 @@ +#include "cartographer/io/hybrid_grid_points_processor.h" + +#include +#include + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/3d/range_data_inserter_3d.h" +#include "cartographer/sensor/range_data.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +HybridGridPointsProcessor::HybridGridPointsProcessor( + const double voxel_size, + const mapping::proto::RangeDataInserterOptions3D& + range_data_inserter_options, + std::unique_ptr file_writer, PointsProcessor* const next) + : next_(next), + range_data_inserter_(range_data_inserter_options), + hybrid_grid_(voxel_size), + file_writer_(std::move(file_writer)) {} + +std::unique_ptr +HybridGridPointsProcessor::FromDictionary( + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("voxel_size"), + mapping::CreateRangeDataInserterOptions3D( + dictionary->GetDictionary("range_data_inserter").get()), + file_writer_factory(dictionary->GetString("filename")), next); +} + +void HybridGridPointsProcessor::Process(std::unique_ptr batch) { + range_data_inserter_.Insert( + {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() { + const mapping::proto::HybridGrid hybrid_grid_proto = hybrid_grid_.ToProto(); + std::string serialized; + hybrid_grid_proto.SerializeToString(&serialized); + file_writer_->Write(serialized.data(), serialized.size()); + CHECK(file_writer_->Close()); + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "Hybrid grid generation must be configured to occur after " + "any stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL) << "Failed to receive FlushResult::kFinished"; + // The following unreachable return statement is needed to avoid a GCC bug + // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508 + return FlushResult::kFinished; +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/hybrid_grid_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/hybrid_grid_points_processor.h new file mode 100644 index 0000000..8124e40 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/hybrid_grid_points_processor.h @@ -0,0 +1,55 @@ +#ifndef CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_ + +// Library used for inserting range data points into a hybrid grid. + +#include +#include + +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/3d/range_data_inserter_3d.h" +#include "cartographer/mapping/proto/range_data_inserter_options_3d.pb.h" + +namespace cartographer { +namespace io { + +// Creates a hybrid grid of the points with voxels being 'voxel_size' +// big. 'range_data_inserter' options are used to configure the range +// data ray tracing through the hybrid grid. +class HybridGridPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "write_hybrid_grid"; + HybridGridPointsProcessor(double voxel_size, + const mapping::proto::RangeDataInserterOptions3D& + range_data_inserter_options, + std::unique_ptr file_writer, + PointsProcessor* next); + HybridGridPointsProcessor(const HybridGridPointsProcessor&) = delete; + HybridGridPointsProcessor& operator=(const HybridGridPointsProcessor&) = + delete; + + static std::unique_ptr FromDictionary( + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~HybridGridPointsProcessor() override {} + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + + mapping::RangeDataInserter3D range_data_inserter_; + mapping::HybridGrid hybrid_grid_; + std::unique_ptr file_writer_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/image.cc b/carto_ws/src/cartographer-master/cartographer/io/image.cc new file mode 100644 index 0000000..558ec50 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/image.cc @@ -0,0 +1,106 @@ +#include "cartographer/io/image.h" + +#include + +#include "cartographer/io/file_writer.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { +namespace { + +uint32 Uint8ColorToCairo(const Uint8Color& color) { + return static_cast(255) << 24 | static_cast(color[0]) << 16 | + static_cast(color[1]) << 8 | color[2]; +} + +Uint8Color CairoToUint8Color(uint32 color) { + uint8 r = color >> 16; + uint8 g = color >> 8; + uint8 b = color; + return {{r, g, b}}; +} + +cairo_status_t CairoWriteCallback(void* const closure, + const unsigned char* data, + const unsigned int length) { + if (static_cast(closure)->Write( + reinterpret_cast(data), length)) { + return CAIRO_STATUS_SUCCESS; + } + return CAIRO_STATUS_WRITE_ERROR; +} + +void CheckStrideIsAsExpected(int width) { + const int stride = cairo_format_stride_for_width(kCairoFormat, width); + CHECK_EQ(stride, width * 4); +} + +} // namespace + +UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface) { + return UniqueCairoSurfacePtr(surface, cairo_surface_destroy); +} + +UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface) { + return UniqueCairoPtr(surface, cairo_destroy); +} + +Image::Image(int width, int height) + : width_(width), height_(height), pixels_(width * height, 0) {} + +Image::Image(UniqueCairoSurfacePtr surface) + : width_(cairo_image_surface_get_width(surface.get())), + height_(cairo_image_surface_get_height(surface.get())) { + CHECK_EQ(cairo_image_surface_get_format(surface.get()), kCairoFormat); + CheckStrideIsAsExpected(width_); + + const uint32* pixel_data = + reinterpret_cast(cairo_image_surface_get_data(surface.get())); + const int num_pixels = width_ * height_; + pixels_.reserve(num_pixels); + for (int i = 0; i < num_pixels; ++i) { + pixels_.push_back(pixel_data[i]); + } +} + +void Image::Rotate90DegreesClockwise() { + const auto old_pixels = pixels_; + pixels_.clear(); + for (int x = 0; x < width_; ++x) { + for (int y = height_ - 1; y >= 0; --y) { + pixels_.push_back(old_pixels.at(y * width_ + x)); + } + } + std::swap(width_, height_); +} + +void Image::WritePng(FileWriter* const file_writer) { + // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of + // the data until the surface is finalized. Once it is finalized though, + // cairo_surface_write_to_png fails, complaining that the surface is already + // finalized. This makes it pretty hard to pass back ownership of the image to + // the caller. + UniqueCairoSurfacePtr surface = GetCairoSurface(); + CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS); + CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback, + file_writer), + CAIRO_STATUS_SUCCESS); +} + +const Uint8Color Image::GetPixel(int x, int y) const { + return CairoToUint8Color(pixels_[y * width_ + x]); +} + +void Image::SetPixel(int x, int y, const Uint8Color& color) { + pixels_[y * width_ + x] = Uint8ColorToCairo(color); +} + +UniqueCairoSurfacePtr Image::GetCairoSurface() { + return MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data( + reinterpret_cast(pixels_.data()), kCairoFormat, width_, + height_, width_ * 4 /* stride */)); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/image.h b/carto_ws/src/cartographer-master/cartographer/io/image.h new file mode 100644 index 0000000..54059f1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/image.h @@ -0,0 +1,79 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_IMAGE_H_ +#define CARTOGRAPHER_IO_IMAGE_H_ + +#include +#include + +#include "cairo/cairo.h" +#include "cartographer/common/port.h" +#include "cartographer/io/color.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +// The only cairo image format we use for Cartographer. +constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32; + +// std::unique_ptr for Cairo surfaces. The surface is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniqueCairoSurfacePtr = + std::unique_ptr; + +// Takes ownership. +UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface); + +// std::unique_ptr for Cairo contexts. +using UniqueCairoPtr = std::unique_ptr; + +// Takes ownership. +UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface); + +class Image { + public: + explicit Image(UniqueCairoSurfacePtr surface); + Image(int width, int height); + + const Uint8Color GetPixel(int x, int y) const; + void SetPixel(int x, int y, const Uint8Color& color); + void WritePng(FileWriter* const file_writer); + + // Rotates the image in place. + void Rotate90DegreesClockwise(); + + // Returns a pointer to a cairo surface that contains the current pixel data. + // The 'Image' object must therefore outlive the returned surface object. It + // is undefined behavior to call any of the mutating functions while a pointer + // to this surface is alive. + UniqueCairoSurfacePtr GetCairoSurface(); + + int width() const { return width_; } + int height() const { return height_; } + + private: + int width_; + int height_; + std::vector pixels_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_IMAGE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/intensity_to_color_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/intensity_to_color_points_processor.cc new file mode 100644 index 0000000..c1a2c08 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/intensity_to_color_points_processor.cc @@ -0,0 +1,67 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/intensity_to_color_points_processor.h" + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +IntensityToColorPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const std::string frame_id = + dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : ""; + const float min_intensity = dictionary->GetDouble("min_intensity"); + const float max_intensity = dictionary->GetDouble("max_intensity"); + return absl::make_unique( + min_intensity, max_intensity, frame_id, next); +} + +IntensityToColorPointsProcessor::IntensityToColorPointsProcessor( + const float min_intensity, const float max_intensity, + const std::string& frame_id, PointsProcessor* const next) + : min_intensity_(min_intensity), + max_intensity_(max_intensity), + frame_id_(frame_id), + next_(next) {} + +void IntensityToColorPointsProcessor::Process( + std::unique_ptr batch) { + if (!batch->intensities.empty() && + (frame_id_.empty() || batch->frame_id == frame_id_)) { + batch->colors.clear(); + for (const float intensity : batch->intensities) { + const float gray = cartographer::common::Clamp( + (intensity - min_intensity_) / (max_intensity_ - min_intensity_), 0.f, + 1.f); + batch->colors.push_back({{gray, gray, gray}}); + } + } + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult IntensityToColorPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/intensity_to_color_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/intensity_to_color_points_processor.h new file mode 100644 index 0000000..342a376 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/intensity_to_color_points_processor.h @@ -0,0 +1,64 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +class IntensityToColorPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "intensity_to_color"; + + // Applies ('intensity' - min ) / (max - min) * 255 and color the point grey + // with this value for each point that comes from the sensor with 'frame_id'. + // If 'frame_id' is empty, this applies to all points. + IntensityToColorPointsProcessor(float min_intensity, float max_intensity, + const std::string& frame_id, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~IntensityToColorPointsProcessor() override{}; + + IntensityToColorPointsProcessor(const IntensityToColorPointsProcessor&) = + delete; + IntensityToColorPointsProcessor& operator=( + const IntensityToColorPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const float min_intensity_; + const float max_intensity_; + const std::string frame_id_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream.cc b/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream.cc new file mode 100644 index 0000000..4512b9b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream.cc @@ -0,0 +1,39 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/internal/in_memory_proto_stream.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +void ForwardingProtoStreamWriter::WriteProto( + const google::protobuf::Message& proto) { + CHECK(writer_callback_(&proto)); +} + +bool ForwardingProtoStreamWriter::Close() { return writer_callback_(nullptr); } + +bool InMemoryProtoStreamReader::ReadProto(google::protobuf::Message* proto) { + if (eof()) return false; + proto->CopyFrom(*state_chunks_.front()); + state_chunks_.pop(); + return true; +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream.h b/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream.h new file mode 100644 index 0000000..c628d73 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream.h @@ -0,0 +1,78 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_INTERNAL_IN_MEMORY_PROTO_STREAM_H_ +#define CARTOGRAPHER_IO_INTERNAL_IN_MEMORY_PROTO_STREAM_H_ + +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/port.h" +#include "cartographer/io/proto_stream_interface.h" +#include "google/protobuf/message.h" + +namespace cartographer { +namespace io { + +class ForwardingProtoStreamWriter + : public cartographer::io::ProtoStreamWriterInterface { + public: + // A callback that is invoked anytime 'WriteProto()' is called on the + // 'ForwardingProtoStreamWriter'. When 'Close()' is called on the + // 'ForwardingProtoStreamWriter' the callback is invoked with a 'nullptr'. + using WriterCallback = + std::function; + + explicit ForwardingProtoStreamWriter(WriterCallback writer_callback) + : writer_callback_(writer_callback) {} + ~ForwardingProtoStreamWriter() = default; + + void WriteProto(const google::protobuf::Message& proto) override; + bool Close() override; + + private: + WriterCallback writer_callback_; +}; + +class InMemoryProtoStreamReader + : public cartographer::io::ProtoStreamReaderInterface { + public: + explicit InMemoryProtoStreamReader( + std::queue>&& state_chunks) + : state_chunks_(std::move(state_chunks)) {} + InMemoryProtoStreamReader() = default; + ~InMemoryProtoStreamReader() = default; + + InMemoryProtoStreamReader(const InMemoryProtoStreamReader&) = delete; + InMemoryProtoStreamReader& operator=(const InMemoryProtoStreamReader&) = + delete; + + template + void AddProto(const MessageType& proto) { + state_chunks_.push(absl::make_unique(proto)); + } + + bool ReadProto(google::protobuf::Message* proto) override; + bool eof() const override { return state_chunks_.empty(); } + + private: + std::queue> state_chunks_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_INTERNAL_IN_MEMORY_PROTO_STREAM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream_test.cc b/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream_test.cc new file mode 100644 index 0000000..e274472 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/in_memory_proto_stream_test.cc @@ -0,0 +1,83 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/internal/in_memory_proto_stream.h" + +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +using absl::make_unique; +using google::protobuf::Message; +using mapping::proto::PoseGraph; +using mapping::proto::SerializedData; + +class InMemoryProtoStreamTest : public ::testing::Test { + protected: + void SetUp() override { + pose_graph_.add_trajectory()->set_trajectory_id(1); + serialized_data_.mutable_odometry_data()->set_trajectory_id(2); + } + + PoseGraph pose_graph_; + SerializedData serialized_data_; +}; + +TEST_F(InMemoryProtoStreamTest, ReadStreamInitializedFromQueue) { + std::queue> proto_queue; + proto_queue.push(make_unique(pose_graph_)); + proto_queue.push(make_unique(serialized_data_)); + + InMemoryProtoStreamReader reader(std::move(proto_queue)); + + PoseGraph actual_pose_graph; + EXPECT_FALSE(reader.eof()); + EXPECT_TRUE(reader.ReadProto(&actual_pose_graph)); + EXPECT_EQ(1, actual_pose_graph.trajectory(0).trajectory_id()); + + SerializedData actual_serialized_data; + EXPECT_FALSE(reader.eof()); + EXPECT_TRUE(reader.ReadProto(&actual_serialized_data)); + EXPECT_EQ(2, actual_serialized_data.odometry_data().trajectory_id()); + + EXPECT_TRUE(reader.eof()); +} + +TEST_F(InMemoryProtoStreamTest, ReadStreamInitializedIncrementally) { + InMemoryProtoStreamReader reader; + reader.AddProto(pose_graph_); + reader.AddProto(serialized_data_); + + PoseGraph actual_pose_graph; + EXPECT_FALSE(reader.eof()); + EXPECT_TRUE(reader.ReadProto(&actual_pose_graph)); + EXPECT_EQ(1, actual_pose_graph.trajectory(0).trajectory_id()); + + SerializedData actual_serialized_data; + EXPECT_FALSE(reader.eof()); + EXPECT_TRUE(reader.ReadProto(&actual_serialized_data)); + EXPECT_EQ(2, actual_serialized_data.odometry_data().trajectory_id()); + + EXPECT_TRUE(reader.eof()); +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/mapping_state_serialization.cc b/carto_ws/src/cartographer-master/cartographer/io/internal/mapping_state_serialization.cc new file mode 100644 index 0000000..8305e7a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/mapping_state_serialization.cc @@ -0,0 +1,237 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/internal/mapping_state_serialization.h" + +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace io { +namespace { +using mapping::MapById; +using mapping::NodeId; +using mapping::PoseGraphInterface; +using mapping::SubmapId; +using mapping::TrajectoryNode; +using mapping::proto::SerializedData; + +mapping::proto::AllTrajectoryBuilderOptions +CreateAllTrajectoryBuilderOptionsProto( + const std::vector& + all_options_with_sensor_ids, + const std::vector& trajectory_ids_to_serialize) { + mapping::proto::AllTrajectoryBuilderOptions all_options_proto; + for (auto id : trajectory_ids_to_serialize) { + *all_options_proto.add_options_with_sensor_ids() = + all_options_with_sensor_ids[id]; + } + return all_options_proto; +} + +// Will return all trajectory ids, that have `state != DELETED`. +std::vector GetValidTrajectoryIds( + const std::map& + trajectory_states) { + std::vector valid_trajectories; + for (const auto& t : trajectory_states) { + if (t.second != PoseGraphInterface::TrajectoryState::DELETED) { + valid_trajectories.push_back(t.first); + } + } + return valid_trajectories; +} + +mapping::proto::SerializationHeader CreateHeader() { + mapping::proto::SerializationHeader header; + header.set_format_version(kMappingStateSerializationFormatVersion); + return header; +} + +SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph, + bool include_unfinished_submaps) { + SerializedData proto; + *proto.mutable_pose_graph() = pose_graph.ToProto(include_unfinished_submaps); + return proto; +} + +SerializedData SerializeTrajectoryBuilderOptions( + const std::vector& + trajectory_builder_options, + const std::vector& trajectory_ids_to_serialize) { + SerializedData proto; + *proto.mutable_all_trajectory_builder_options() = + CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_options, + trajectory_ids_to_serialize); + return proto; +} + +void SerializeSubmaps( + const MapById& submap_data, + bool include_unfinished_submaps, ProtoStreamWriterInterface* const writer) { + // Next serialize all submaps. + for (const auto& submap_id_data : submap_data) { + if (!include_unfinished_submaps && + !submap_id_data.data.submap->insertion_finished()) { + continue; + } + SerializedData proto; + auto* const submap_proto = proto.mutable_submap(); + *submap_proto = submap_id_data.data.submap->ToProto( + /*include_probability_grid_data=*/true); + submap_proto->mutable_submap_id()->set_trajectory_id( + submap_id_data.id.trajectory_id); + submap_proto->mutable_submap_id()->set_submap_index( + submap_id_data.id.submap_index); + writer->WriteProto(proto); + } +} + +void SerializeTrajectoryNodes( + const MapById& trajectory_nodes, + ProtoStreamWriterInterface* const writer) { + for (const auto& node_id_data : trajectory_nodes) { + SerializedData proto; + auto* const node_proto = proto.mutable_node(); + node_proto->mutable_node_id()->set_trajectory_id( + node_id_data.id.trajectory_id); + node_proto->mutable_node_id()->set_node_index(node_id_data.id.node_index); + *node_proto->mutable_node_data() = + ToProto(*node_id_data.data.constant_data); + writer->WriteProto(proto); + } +} + +void SerializeTrajectoryData( + const std::map& + all_trajectory_data, + ProtoStreamWriterInterface* const writer) { + for (const auto& trajectory_data : all_trajectory_data) { + SerializedData proto; + auto* const trajectory_data_proto = proto.mutable_trajectory_data(); + trajectory_data_proto->set_trajectory_id(trajectory_data.first); + trajectory_data_proto->set_gravity_constant( + trajectory_data.second.gravity_constant); + *trajectory_data_proto->mutable_imu_calibration() = transform::ToProto( + Eigen::Quaterniond(trajectory_data.second.imu_calibration[0], + trajectory_data.second.imu_calibration[1], + trajectory_data.second.imu_calibration[2], + trajectory_data.second.imu_calibration[3])); + if (trajectory_data.second.fixed_frame_origin_in_map.has_value()) { + *trajectory_data_proto->mutable_fixed_frame_origin_in_map() = + transform::ToProto( + trajectory_data.second.fixed_frame_origin_in_map.value()); + } + writer->WriteProto(proto); + } +} + +void SerializeImuData(const sensor::MapByTime& all_imu_data, + ProtoStreamWriterInterface* const writer) { + for (const int trajectory_id : all_imu_data.trajectory_ids()) { + for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) { + SerializedData proto; + auto* const imu_data_proto = proto.mutable_imu_data(); + imu_data_proto->set_trajectory_id(trajectory_id); + *imu_data_proto->mutable_imu_data() = sensor::ToProto(imu_data); + writer->WriteProto(proto); + } + } +} + +void SerializeOdometryData( + const sensor::MapByTime& all_odometry_data, + ProtoStreamWriterInterface* const writer) { + for (const int trajectory_id : all_odometry_data.trajectory_ids()) { + for (const auto& odometry_data : + all_odometry_data.trajectory(trajectory_id)) { + SerializedData proto; + auto* const odometry_data_proto = proto.mutable_odometry_data(); + odometry_data_proto->set_trajectory_id(trajectory_id); + *odometry_data_proto->mutable_odometry_data() = + sensor::ToProto(odometry_data); + writer->WriteProto(proto); + } + } +} + +void SerializeFixedFramePoseData( + const sensor::MapByTime& + all_fixed_frame_pose_data, + ProtoStreamWriterInterface* const writer) { + for (const int trajectory_id : all_fixed_frame_pose_data.trajectory_ids()) { + for (const auto& fixed_frame_pose_data : + all_fixed_frame_pose_data.trajectory(trajectory_id)) { + SerializedData proto; + auto* const fixed_frame_pose_data_proto = + proto.mutable_fixed_frame_pose_data(); + fixed_frame_pose_data_proto->set_trajectory_id(trajectory_id); + *fixed_frame_pose_data_proto->mutable_fixed_frame_pose_data() = + sensor::ToProto(fixed_frame_pose_data); + writer->WriteProto(proto); + } + } +} + +void SerializeLandmarkNodes( + const std::map& + all_landmark_nodes, + ProtoStreamWriterInterface* const writer) { + for (const auto& node : all_landmark_nodes) { + for (const auto& observation : node.second.landmark_observations) { + SerializedData proto; + auto* landmark_data_proto = proto.mutable_landmark_data(); + landmark_data_proto->set_trajectory_id(observation.trajectory_id); + landmark_data_proto->mutable_landmark_data()->set_timestamp( + common::ToUniversal(observation.time)); + auto* observation_proto = landmark_data_proto->mutable_landmark_data() + ->add_landmark_observations(); + observation_proto->set_id(node.first); + *observation_proto->mutable_landmark_to_tracking_transform() = + transform::ToProto(observation.landmark_to_tracking_transform); + observation_proto->set_translation_weight(observation.translation_weight); + observation_proto->set_rotation_weight(observation.rotation_weight); + writer->WriteProto(proto); + } + } +} + +} // namespace + +void WritePbStream( + const mapping::PoseGraph& pose_graph, + const std::vector& + trajectory_builder_options, + ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps) { + writer->WriteProto(CreateHeader()); + writer->WriteProto( + SerializePoseGraph(pose_graph, include_unfinished_submaps)); + writer->WriteProto(SerializeTrajectoryBuilderOptions( + trajectory_builder_options, + GetValidTrajectoryIds(pose_graph.GetTrajectoryStates()))); + + SerializeSubmaps(pose_graph.GetAllSubmapData(), include_unfinished_submaps, + writer); + SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer); + SerializeTrajectoryData(pose_graph.GetTrajectoryData(), writer); + SerializeImuData(pose_graph.GetImuData(), writer); + SerializeOdometryData(pose_graph.GetOdometryData(), writer); + SerializeFixedFramePoseData(pose_graph.GetFixedFramePoseData(), writer); + SerializeLandmarkNodes(pose_graph.GetLandmarkNodes(), writer); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/mapping_state_serialization.h b/carto_ws/src/cartographer-master/cartographer/io/internal/mapping_state_serialization.h new file mode 100644 index 0000000..ac8bdd9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/mapping_state_serialization.h @@ -0,0 +1,41 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_INTERNAL_MAPPING_STATE_SERIALIZATION_H_ +#define CARTOGRAPHER_IO_INTERNAL_MAPPING_STATE_SERIALIZATION_H_ + +#include "cartographer/io/proto_stream_interface.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" + +namespace cartographer { +namespace io { + +// The current serialization format version. +static constexpr int kMappingStateSerializationFormatVersion = 2; +static constexpr int kFormatVersionWithoutSubmapHistograms = 1; + +// Serialize mapping state to a pbstream. +void WritePbStream( + const mapping::PoseGraph& pose_graph, + const std::vector& + builder_options, + ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_INTERNAL_MAPPING_STATE_SERIALIZATION_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_info.cc b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_info.cc new file mode 100644 index 0000000..194d9d1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_info.cc @@ -0,0 +1,127 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/internal/pbstream_info.h" + +#include +#include +#include + +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_bool(all_debug_strings, false, + "Print debug strings of all serialized data."); + +using cartographer::mapping::proto::SerializedData; + +namespace cartographer { +namespace io { +namespace { + +void Run(const std::string& pbstream_filename, bool all_debug_strings) { + LOG(INFO) << "Reading pbstream file from '" << pbstream_filename << "'..."; + io::ProtoStreamReader reader(pbstream_filename); + io::ProtoStreamDeserializer deserializer(&reader); + const auto header = deserializer.header(); + LOG(INFO) << "Header: " << header.DebugString(); + for (const mapping::proto::TrajectoryBuilderOptionsWithSensorIds& + trajectory_options : deserializer.all_trajectory_builder_options() + .options_with_sensor_ids()) { + LOG(INFO) << "Trajectory options: " << trajectory_options.DebugString(); + } + const mapping::proto::PoseGraph pose_graph = deserializer.pose_graph(); + for (const mapping::proto::Trajectory& trajectory : pose_graph.trajectory()) { + LOG(INFO) << "Trajectory id: " << trajectory.trajectory_id() + << " has #nodes " << trajectory.node_size() << " has #submaps " + << trajectory.submap_size(); + } + if (all_debug_strings) { + LOG(INFO) << "Pose graph: " << pose_graph.DebugString(); + } + + const std::map data_case_to_name = { + {SerializedData::kSubmap, "submap"}, + {SerializedData::kNode, "node"}, + {SerializedData::kTrajectoryData, "trajectory_data"}, + {SerializedData::kImuData, "imu_data"}, + {SerializedData::kOdometryData, "odometry_data"}, + {SerializedData::kFixedFramePoseData, "fixed_frame_pose_data"}, + {SerializedData::kLandmarkData, "landmark_data"}, + }; + // Initialize so zero counts of these are also reported. + std::map data_counts = { + {"submap_2d", 0}, + {"submap_2d_grid", 0}, + {"submap_3d", 0}, + {"submap_3d_high_resolution_hybrid_grid", 0}, + }; + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + if (all_debug_strings) { + LOG(INFO) << "Serialized data: " << proto.DebugString(); + } + auto it = data_case_to_name.find(proto.data_case()); + if (it == data_case_to_name.end()) { + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); + } + const std::string& data_name = it->second; + ++data_counts[data_name]; + if (proto.data_case() == SerializedData::kSubmap) { + if (proto.mutable_submap()->has_submap_2d()) { + ++data_counts["submap_2d"]; + if (proto.mutable_submap()->mutable_submap_2d()->has_grid()) { + ++data_counts["submap_2d_grid"]; + } + } + if (proto.mutable_submap()->has_submap_3d()) { + ++data_counts["submap_3d"]; + if (proto.mutable_submap() + ->mutable_submap_3d() + ->has_high_resolution_hybrid_grid()) { + ++data_counts["submap_3d_high_resolution_hybrid_grid"]; + } + } + } + } + + for (const auto& entry : data_counts) { + LOG(INFO) << "SerializedData package contains #" << entry.first << ": " + << entry.second; + } +} +} // namespace + +int pbstream_info(int argc, char* argv[]) { + std::stringstream ss; + ss << "\n\n" + << "Reads a pbstream file and summarizes its contents.\n\n" + << "Usage: " << argv[0] << " " << argv[1] + << " [flags]\n"; + google::SetUsageMessage(ss.str()); + if (argc < 3) { + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info"); + return EXIT_FAILURE; + } + Run(argv[2], FLAGS_all_debug_strings); + return EXIT_SUCCESS; +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_info.h b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_info.h new file mode 100644 index 0000000..7aec48d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_info.h @@ -0,0 +1,30 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_INTERNAL_PBSTREAM_INFO_H_ +#define CARTOGRAPHER_IO_INTERNAL_PBSTREAM_INFO_H_ + +namespace cartographer { +namespace io { + +// info subtool for pbstream swiss army knife. The command line arguments are +// assumed to be parsed and removed from the remaining arguments already. +int pbstream_info(int argc, char* argv[]); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_INTERNAL_PBSTREAM_INFO_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_migrate.cc b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_migrate.cc new file mode 100644 index 0000000..43d6b8a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_migrate.cc @@ -0,0 +1,57 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/serialization_format_migration.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_bool(include_unfinished_submaps, true, + "Whether to include unfinished submaps in the output."); + +namespace cartographer { +namespace io { + +int pbstream_migrate(int argc, char** argv) { + std::stringstream ss; + ss << "\n\nTool for migrating files that use submaps without histograms " + "to the new submap format, which includes a histogram. You can " + "set --include_unfinished_submaps to false if you want to exclude " + "unfinished submaps in the output." + << "\nUsage: " << argv[0] << " " << argv[1] + << " [flags]"; + google::SetUsageMessage(ss.str()); + + if (argc < 4) { + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_migrate"); + return EXIT_FAILURE; + } + cartographer::io::ProtoStreamReader input(argv[2]); + cartographer::io::ProtoStreamWriter output(argv[3]); + LOG(INFO) << "Migrating serialization format 1 in \"" << argv[2] + << "\" to serialization format 2 in \"" << argv[3] << "\""; + cartographer::io::MigrateStreamVersion1ToVersion2( + &input, &output, FLAGS_include_unfinished_submaps); + CHECK(output.Close()) << "Could not write migrated pbstream file to: " + << argv[3]; + + return EXIT_SUCCESS; +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_migrate.h b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_migrate.h new file mode 100644 index 0000000..2f5a554 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/pbstream_migrate.h @@ -0,0 +1,30 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_INTERNAL_PBSTREAM_MIGRATE_H_ +#define CARTOGRAPHER_IO_INTERNAL_PBSTREAM_MIGRATE_H_ + +namespace cartographer { +namespace io { + +// 'pbstream migrate' entry point. Commandline flags are assumed to be already +// parsed and removed from the remaining arguments. +int pbstream_migrate(int argc, char** argv); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_INTERNAL_PBSTREAM_MIGRATE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/testing/test_helpers.cc b/carto_ws/src/cartographer-master/cartographer/io/internal/testing/test_helpers.cc new file mode 100644 index 0000000..96a56d5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/testing/test_helpers.cc @@ -0,0 +1,44 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/internal/testing/test_helpers.h" + +#include "cartographer/mapping/proto/serialization.pb.h" + +namespace cartographer { +namespace io { +namespace testing { + +std::unique_ptr ProtoReaderFromStrings( + const std::string &header_textpb, + const std::initializer_list &data_textpbs) { + std::queue> proto_queue; + proto_queue.emplace(absl::make_unique< + ::cartographer::mapping::proto::SerializationHeader>( + ProtoFromStringOrDie<::cartographer::mapping::proto::SerializationHeader>( + header_textpb))); + for (const std::string &data_textpb : data_textpbs) { + proto_queue.emplace( + absl::make_unique<::cartographer::mapping::proto::SerializedData>( + ProtoFromStringOrDie< + ::cartographer::mapping::proto::SerializedData>(data_textpb))); + } + return absl::make_unique(std::move(proto_queue)); +} + +} // namespace testing +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/internal/testing/test_helpers.h b/carto_ws/src/cartographer-master/cartographer/io/internal/testing/test_helpers.h new file mode 100644 index 0000000..237c847 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/internal/testing/test_helpers.h @@ -0,0 +1,45 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_TESTING_TEST_HELPERS_H_ +#define CARTOGRAPHER_IO_TESTING_TEST_HELPERS_H_ + +#include + +#include "cartographer/io/internal/in_memory_proto_stream.h" +#include "glog/logging.h" +#include "google/protobuf/text_format.h" + +namespace cartographer { +namespace io { +namespace testing { + +template +T ProtoFromStringOrDie(const std::string &proto_string) { + T msg; + CHECK(google::protobuf::TextFormat::ParseFromString(proto_string, &msg)); + return msg; +} + +std::unique_ptr ProtoReaderFromStrings( + const std::string &header_textpb, + const std::initializer_list &data_textpbs); + +} // namespace testing +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_TESTING_TEST_HELPERS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/min_max_range_filtering_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/min_max_range_filtering_points_processor.cc new file mode 100644 index 0000000..ba5fc79 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/min_max_range_filtering_points_processor.cc @@ -0,0 +1,61 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/min_max_range_filtering_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +MinMaxRangeFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), + next); +} + +MinMaxRangeFilteringPointsProcessor::MinMaxRangeFilteringPointsProcessor( + const double min_range, const double max_range, PointsProcessor* next) + : min_range_squared_(min_range * min_range), + max_range_squared_(max_range * max_range), + next_(next) {} + +void MinMaxRangeFilteringPointsProcessor::Process( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const float range_squared = + (batch->points[i].position - batch->origin).squaredNorm(); + if (!(min_range_squared_ <= range_squared && + range_squared <= max_range_squared_)) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult MinMaxRangeFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/min_max_range_filtering_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/min_max_range_filtering_points_processor.h new file mode 100644 index 0000000..0e90b3e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/min_max_range_filtering_points_processor.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Filters all points that are farther away from their 'origin' as 'max_range' +// or closer than 'min_range'. +class MinMaxRangeFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "min_max_range_filter"; + MinMaxRangeFilteringPointsProcessor(double min_range, double max_range, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~MinMaxRangeFilteringPointsProcessor() override {} + + MinMaxRangeFilteringPointsProcessor( + const MinMaxRangeFilteringPointsProcessor&) = delete; + MinMaxRangeFilteringPointsProcessor& operator=( + const MinMaxRangeFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double min_range_squared_; + const double max_range_squared_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/null_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/null_points_processor.h new file mode 100644 index 0000000..30a0090 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/null_points_processor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ + +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// A points processor that just drops all points. The end of a pipeline usually. +class NullPointsProcessor : public PointsProcessor { + public: + NullPointsProcessor() {} + ~NullPointsProcessor() override {} + + void Process(std::unique_ptr points_batch) override {} + FlushResult Flush() override { return FlushResult::kFinished; } +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/outlier_removing_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/outlier_removing_points_processor.cc new file mode 100644 index 0000000..585c36f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/outlier_removing_points_processor.cc @@ -0,0 +1,132 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/outlier_removing_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +OutlierRemovingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const double miss_per_hit_limit = [&]() { + if (!dictionary->HasKey("miss_per_hit_limit")) { + LOG(INFO) << "Using default value of 3 for miss_per_hit_limit."; + return 3.; + } else { + return dictionary->GetDouble("miss_per_hit_limit"); + } + }(); + return absl::make_unique( + dictionary->GetDouble("voxel_size"), miss_per_hit_limit, next); +} + +OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor( + const double voxel_size, const double miss_per_hit_limit, + PointsProcessor* next) + : voxel_size_(voxel_size), + miss_per_hit_limit_(miss_per_hit_limit), + next_(next), + state_(State::kPhase1), + voxels_(voxel_size_) { + LOG(INFO) << "Marking hits..."; +} + +void OutlierRemovingPointsProcessor::Process( + std::unique_ptr batch) { + switch (state_) { + case State::kPhase1: + ProcessInPhaseOne(*batch); + break; + + case State::kPhase2: + ProcessInPhaseTwo(*batch); + break; + + case State::kPhase3: + ProcessInPhaseThree(std::move(batch)); + break; + } +} + +PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() { + switch (state_) { + case State::kPhase1: + LOG(INFO) << "Counting rays..."; + state_ = State::kPhase2; + return FlushResult::kRestartStream; + + case State::kPhase2: + LOG(INFO) << "Filtering outliers..."; + state_ = State::kPhase3; + return FlushResult::kRestartStream; + + case State::kPhase3: + CHECK(next_->Flush() == FlushResult::kFinished) + << "Voxel filtering and outlier removal must be configured to occur " + "after any stages that require multiple passes."; + return FlushResult::kFinished; + } + LOG(FATAL); +} + +void OutlierRemovingPointsProcessor::ProcessInPhaseOne( + const PointsBatch& batch) { + for (size_t i = 0; i < batch.points.size(); ++i) { + ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i].position)) + ->hits; + } +} + +void OutlierRemovingPointsProcessor::ProcessInPhaseTwo( + const PointsBatch& batch) { + // TODO(whess): This samples every 'voxel_size' distance and could be improved + // by better ray casting, and also by marking the hits of the current range + // data to be excluded. + for (size_t i = 0; i < batch.points.size(); ++i) { + const Eigen::Vector3f delta = batch.points[i].position - batch.origin; + const float length = delta.norm(); + for (float x = 0; x < length; x += voxel_size_) { + const Eigen::Array3i index = + voxels_.GetCellIndex(batch.origin + (x / length) * delta); + if (voxels_.value(index).hits > 0) { + ++voxels_.mutable_value(index)->rays; + } + } + } +} + +void OutlierRemovingPointsProcessor::ProcessInPhaseThree( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const VoxelData voxel = + voxels_.value(voxels_.GetCellIndex(batch->points[i].position)); + if (!(voxel.rays < miss_per_hit_limit_ * voxel.hits)) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/outlier_removing_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/outlier_removing_points_processor.h new file mode 100644 index 0000000..4853c78 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/outlier_removing_points_processor.h @@ -0,0 +1,89 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/3d/hybrid_grid.h" + +namespace cartographer { +namespace io { + +// Voxel filters the data and only passes on points that we believe are on +// non-moving objects. +class OutlierRemovingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "voxel_filter_and_remove_moving_objects"; + + OutlierRemovingPointsProcessor(double voxel_size, double miss_per_hit_limit, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~OutlierRemovingPointsProcessor() override {} + + OutlierRemovingPointsProcessor(const OutlierRemovingPointsProcessor&) = + delete; + OutlierRemovingPointsProcessor& operator=( + const OutlierRemovingPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + // To reduce memory consumption by not having to keep all rays in memory, we + // filter outliers in three phases each going over all data: First we compute + // all voxels containing any hits, then we compute the rays passing through + // each of these voxels, and finally we output all hits in voxels that are + // considered obstructed. + struct VoxelData { + int hits = 0; + int rays = 0; + }; + enum class State { + kPhase1, + kPhase2, + kPhase3, + }; + + // First phase counts the number of hits per voxel. + void ProcessInPhaseOne(const PointsBatch& batch); + + // Second phase counts how many rays pass through each voxel. This is only + // done for voxels that contain hits. This is to reduce memory consumption by + // not adding data to free voxels. + void ProcessInPhaseTwo(const PointsBatch& batch); + + // Third phase produces the output containing all inliers. We consider each + // hit an inlier if it is inside a voxel that has a sufficiently high + // hit-to-ray ratio. + void ProcessInPhaseThree(std::unique_ptr batch); + + const double voxel_size_; + const double miss_per_hit_limit_; + PointsProcessor* const next_; + State state_; + mapping::HybridGridBase voxels_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/pbstream_main.cc b/carto_ws/src/cartographer-master/cartographer/io/pbstream_main.cc new file mode 100644 index 0000000..4fffbd4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/pbstream_main.cc @@ -0,0 +1,50 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "absl/container/flat_hash_set.h" +#include "cartographer/io/internal/pbstream_info.h" +#include "cartographer/io/internal/pbstream_migrate.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + + FLAGS_logtostderr = true; + const std::string usage_message = + "Swiss Army knife for pbstreams.\n\n" + "Currently supported subcommands are:\n" + "\tinfo - Prints summary of pbstream.\n" + "\tmigrate - Migrates pbstream to the new submap format."; + google::ParseCommandLineFlags(&argc, &argv, true); + + if (argc < 2) { + google::SetUsageMessage(usage_message); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); + return EXIT_FAILURE; + } else if (std::string(argv[1]) == "info") { + return ::cartographer::io::pbstream_info(argc, argv); + } else if (std::string(argv[1]) == "migrate") { + return ::cartographer::io::pbstream_migrate(argc, argv); + } else { + LOG(INFO) << "Unknown subtool: \"" << argv[1]; + google::SetUsageMessage(usage_message); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); + return EXIT_FAILURE; + } +} diff --git a/carto_ws/src/cartographer-master/cartographer/io/pcd_writing_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/pcd_writing_points_processor.cc new file mode 100644 index 0000000..abd2c98 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/pcd_writing_points_processor.cc @@ -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. + */ + +#include "cartographer/io/pcd_writing_points_processor.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +namespace { + +// Writes the PCD header claiming 'num_points' will follow it into +// 'output_file'. +void WriteBinaryPcdHeader(const bool has_color, const int64 num_points, + FileWriter* const file_writer) { + std::string color_header_field = !has_color ? "" : " rgb"; + std::string color_header_type = !has_color ? "" : " U"; + std::string color_header_size = !has_color ? "" : " 4"; + std::string color_header_count = !has_color ? "" : " 1"; + + std::ostringstream stream; + stream << "# generated by Cartographer\n" + << "VERSION .7\n" + << "FIELDS x y z" << color_header_field << "\n" + << "SIZE 4 4 4" << color_header_size << "\n" + << "TYPE F F F" << color_header_type << "\n" + << "COUNT 1 1 1" << color_header_count << "\n" + << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n" + << "HEIGHT 1\n" + << "VIEWPOINT 0 0 0 1 0 0 0\n" + << "POINTS " << std::setw(15) << std::setfill('0') << num_points + << "\n" + << "DATA binary\n"; + const std::string out = stream.str(); + file_writer->WriteHeader(out.data(), out.size()); +} + +void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point, + FileWriter* const file_writer) { + char buffer[12]; + memcpy(buffer, &point[0], sizeof(float)); + memcpy(buffer + 4, &point[1], sizeof(float)); + memcpy(buffer + 8, &point[2], sizeof(float)); + CHECK(file_writer->Write(buffer, 12)); +} + +void WriteBinaryPcdPointColor(const Uint8Color& color, + FileWriter* const file_writer) { + char buffer[4]; + buffer[0] = color[2]; + buffer[1] = color[1]; + buffer[2] = color[0]; + buffer[3] = 0; + CHECK(file_writer->Write(buffer, 4)); +} + +} // namespace + +std::unique_ptr +PcdWritingPointsProcessor::FromDictionary( + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + file_writer_factory(dictionary->GetString("filename")), next); +} + +PcdWritingPointsProcessor::PcdWritingPointsProcessor( + std::unique_ptr file_writer, PointsProcessor* const next) + : next_(next), + num_points_(0), + has_colors_(false), + file_writer_(std::move(file_writer)) {} + +PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() { + WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get()); + CHECK(file_writer_->Close()); + + switch (next_->Flush()) { + case FlushResult::kFinished: + return FlushResult::kFinished; + + case FlushResult::kRestartStream: + LOG(FATAL) << "PCD generation must be configured to occur after any " + "stages that require multiple passes."; + } + LOG(FATAL); +} + +void PcdWritingPointsProcessor::Process(std::unique_ptr batch) { + if (batch->points.empty()) { + next_->Process(std::move(batch)); + return; + } + + if (num_points_ == 0) { + has_colors_ = !batch->colors.empty(); + WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get()); + } + for (size_t i = 0; i < batch->points.size(); ++i) { + WriteBinaryPcdPointCoordinate(batch->points[i].position, + file_writer_.get()); + if (!batch->colors.empty()) { + WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]), + file_writer_.get()); + } + ++num_points_; + } + next_->Process(std::move(batch)); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/pcd_writing_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/pcd_writing_points_processor.h new file mode 100644 index 0000000..4e209a7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/pcd_writing_points_processor.h @@ -0,0 +1,55 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Streams a PCD file to disk. The header is written in 'Flush'. +class PcdWritingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "write_pcd"; + PcdWritingPointsProcessor(std::unique_ptr file_writer, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~PcdWritingPointsProcessor() override {} + + PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete; + PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) = + delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + + int64 num_points_; + bool has_colors_; + std::unique_ptr file_writer_; +}; + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/ply_writing_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/ply_writing_points_processor.cc new file mode 100644 index 0000000..8b0ecdb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/ply_writing_points_processor.cc @@ -0,0 +1,161 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/ply_writing_points_processor.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +namespace { + +// Writes the PLY header claiming 'num_points' will follow it into +// 'output_file'. +void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities, + const std::vector& comments, + const int64 num_points, + FileWriter* const file_writer) { + const std::string color_header = !has_color ? "" + : "property uchar red\n" + "property uchar green\n" + "property uchar blue\n"; + const std::string intensity_header = + !has_intensities ? "" : "property float intensity\n"; + std::ostringstream stream; + stream << "ply\n" + << "format binary_little_endian 1.0\n" + << "comment generated by Cartographer\n"; + for (const std::string& comment : comments) { + stream << "comment " << comment << "\n"; + } + stream << "element vertex " << std::setw(15) << std::setfill('0') + << num_points << "\n" + << "property float x\n" + << "property float y\n" + << "property float z\n" + << color_header << intensity_header << "end_header\n"; + const std::string out = stream.str(); + CHECK(file_writer->WriteHeader(out.data(), out.size())); +} + +void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point, + FileWriter* const file_writer) { + // TODO(sirver): This ignores endianness. + char buffer[12]; + memcpy(buffer, &point[0], sizeof(float)); + memcpy(buffer + 4, &point[1], sizeof(float)); + memcpy(buffer + 8, &point[2], sizeof(float)); + CHECK(file_writer->Write(buffer, 12)); +} + +void WriteBinaryIntensity(const float intensity, + FileWriter* const file_writer) { + // TODO(sirver): This ignores endianness. + CHECK(file_writer->Write(reinterpret_cast(&intensity), + sizeof(float))); +} + +void WriteBinaryPlyPointColor(const Uint8Color& color, + FileWriter* const file_writer) { + CHECK(file_writer->Write(reinterpret_cast(color.data()), + color.size())); +} + +} // namespace + +std::unique_ptr +PlyWritingPointsProcessor::FromDictionary( + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + file_writer_factory(dictionary->GetString("filename")), + std::vector(), next); +} + +PlyWritingPointsProcessor::PlyWritingPointsProcessor( + std::unique_ptr file_writer, + const std::vector& comments, PointsProcessor* const next) + : next_(next), + comments_(comments), + num_points_(0), + has_colors_(false), + file_(std::move(file_writer)) {} + +PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() { + WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, num_points_, + file_.get()); + CHECK(file_->Close()) << "Closing PLY file_writer failed."; + + switch (next_->Flush()) { + case FlushResult::kFinished: + return FlushResult::kFinished; + + case FlushResult::kRestartStream: + LOG(FATAL) << "PLY generation must be configured to occur after any " + "stages that require multiple passes."; + } + LOG(FATAL); +} + +void PlyWritingPointsProcessor::Process(std::unique_ptr batch) { + if (batch->points.empty()) { + next_->Process(std::move(batch)); + return; + } + + if (num_points_ == 0) { + has_colors_ = !batch->colors.empty(); + has_intensities_ = !batch->intensities.empty(); + WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, 0, + file_.get()); + } + if (has_colors_) { + CHECK_EQ(batch->points.size(), batch->colors.size()) + << "First PointsBatch had colors, but encountered one without. " + "frame_id: " + << batch->frame_id; + } + if (has_intensities_) { + CHECK_EQ(batch->points.size(), batch->intensities.size()) + << "First PointsBatch had intensities, but encountered one without. " + "frame_id: " + << batch->frame_id; + } + + for (size_t i = 0; i < batch->points.size(); ++i) { + WriteBinaryPlyPointCoordinate(batch->points[i].position, file_.get()); + if (has_colors_) { + WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get()); + } + if (has_intensities_) { + WriteBinaryIntensity(batch->intensities[i], file_.get()); + } + ++num_points_; + } + next_->Process(std::move(batch)); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/ply_writing_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/ply_writing_points_processor.h new file mode 100644 index 0000000..7629587 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/ply_writing_points_processor.h @@ -0,0 +1,56 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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 "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Streams a PLY file to disk. The header is written in 'Flush'. +class PlyWritingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "write_ply"; + PlyWritingPointsProcessor(std::unique_ptr file_writer, + const std::vector& comments, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~PlyWritingPointsProcessor() override {} + + PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete; + PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) = + delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + + std::vector comments_; + int64 num_points_; + bool has_colors_; + bool has_intensities_; + std::unique_ptr file_; +}; + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/points_batch.cc b/carto_ws/src/cartographer-master/cartographer/io/points_batch.cc new file mode 100644 index 0000000..cb4848c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/points_batch.cc @@ -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. + */ + +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +void RemovePoints(absl::flat_hash_set to_remove, PointsBatch* batch) { + const int new_num_points = batch->points.size() - to_remove.size(); + std::vector points; + points.reserve(new_num_points); + std::vector intensities; + if (!batch->intensities.empty()) { + intensities.reserve(new_num_points); + } + std::vector colors; + if (!batch->colors.empty()) { + colors.reserve(new_num_points); + } + + for (size_t i = 0; i < batch->points.size(); ++i) { + if (to_remove.count(i) == 1) { + continue; + } + points.push_back(batch->points[i]); + if (!batch->colors.empty()) { + colors.push_back(batch->colors[i]); + } + if (!batch->intensities.empty()) { + intensities.push_back(batch->intensities[i]); + } + } + batch->points = std::move(points); + batch->intensities = std::move(intensities); + batch->colors = std::move(colors); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/points_batch.h b/carto_ws/src/cartographer-master/cartographer/io/points_batch.h new file mode 100644 index 0000000..cc1d6bc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/points_batch.h @@ -0,0 +1,75 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_POINTS_BATCH_H_ +#define CARTOGRAPHER_IO_POINTS_BATCH_H_ + +#include +#include +#include + +#include "Eigen/Core" +#include "absl/container/flat_hash_set.h" +#include "cartographer/common/time.h" +#include "cartographer/io/color.h" +#include "cartographer/sensor/rangefinder_point.h" + +namespace cartographer { +namespace io { + +// A number of points, captured around the same 'time' and by a +// sensor at the same 'origin'. +struct PointsBatch { + PointsBatch() { + origin = Eigen::Vector3f::Zero(); + trajectory_id = 0; + } + + // Time at which the first point of this batch has been acquired. + common::Time start_time; + + // Origin of the data, i.e. the location of the sensor in the world at + // 'time'. + Eigen::Vector3f origin; + + // Sensor that generated this data's 'frame_id' or empty if this information + // is unknown. + std::string frame_id; + + // Trajectory ID that produced this point. + int trajectory_id; + + // Geometry of the points in the map frame. + std::vector points; + + // Intensities are optional and may be unspecified. The meaning of these + // intensity values varies by device. For example, the VLP16 provides values + // in the range [0, 100] for non-specular return values and values up to 255 + // for specular returns. On the other hand, Hokuyo lasers provide a 16-bit + // value that rarely peaks above 4096. + std::vector intensities; + + // Colors are optional. If set, they are RGB values. + std::vector colors; +}; + +// Removes the indices in 'to_remove' from 'batch'. +void RemovePoints(absl::flat_hash_set to_remove, PointsBatch* batch); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_POINTS_BATCH_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/points_processor.h new file mode 100644 index 0000000..06b7a78 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/points_processor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +// A processor in a pipeline. It processes a 'points_batch' and hands it to the +// next processor in the pipeline. +class PointsProcessor { + public: + enum class FlushResult { + kRestartStream, + kFinished, + }; + + PointsProcessor() {} + virtual ~PointsProcessor() {} + + PointsProcessor(const PointsProcessor&) = delete; + PointsProcessor& operator=(const PointsProcessor&) = delete; + + // Receive a 'points_batch', process it and pass it on. + virtual void Process(std::unique_ptr points_batch) = 0; + + // Some implementations will perform expensive computations and others that do + // multiple passes over the data might ask for restarting the stream. + virtual FlushResult Flush() = 0; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder.cc b/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder.cc new file mode 100644 index 0000000..6592245 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder.cc @@ -0,0 +1,141 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/points_processor_pipeline_builder.h" + +#include "absl/memory/memory.h" +#include "cartographer/io/coloring_points_processor.h" +#include "cartographer/io/counting_points_processor.h" +#include "cartographer/io/fixed_ratio_sampling_points_processor.h" +#include "cartographer/io/frame_id_filtering_points_processor.h" +#include "cartographer/io/hybrid_grid_points_processor.h" +#include "cartographer/io/intensity_to_color_points_processor.h" +#include "cartographer/io/min_max_range_filtering_points_processor.h" +#include "cartographer/io/null_points_processor.h" +#include "cartographer/io/outlier_removing_points_processor.h" +#include "cartographer/io/pcd_writing_points_processor.h" +#include "cartographer/io/ply_writing_points_processor.h" +#include "cartographer/io/probability_grid_points_processor.h" +#include "cartographer/io/vertical_range_filtering_points_processor.h" +#include "cartographer/io/xray_points_processor.h" +#include "cartographer/io/xyz_writing_points_processor.h" +#include "cartographer/mapping/proto/trajectory.pb.h" + +namespace cartographer { +namespace io { + +template +void RegisterPlainPointsProcessor( + PointsProcessorPipelineBuilder* const builder) { + builder->Register( + PointsProcessorType::kConfigurationFileActionName, + [](common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return PointsProcessorType::FromDictionary(dictionary, next); + }); +} + +template +void RegisterFileWritingPointsProcessor( + const FileWriterFactory& file_writer_factory, + PointsProcessorPipelineBuilder* const builder) { + builder->Register( + PointsProcessorType::kConfigurationFileActionName, + [file_writer_factory]( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return PointsProcessorType::FromDictionary(file_writer_factory, + dictionary, next); + }); +} + +template +void RegisterFileWritingPointsProcessorWithTrajectories( + const std::vector& trajectories, + const FileWriterFactory& file_writer_factory, + PointsProcessorPipelineBuilder* const builder) { + builder->Register( + PointsProcessorType::kConfigurationFileActionName, + [&trajectories, file_writer_factory]( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return PointsProcessorType::FromDictionary( + trajectories, file_writer_factory, dictionary, next); + }); +} + +void RegisterBuiltInPointsProcessors( + const std::vector& trajectories, + const FileWriterFactory& file_writer_factory, + PointsProcessorPipelineBuilder* builder) { + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); + RegisterFileWritingPointsProcessorWithTrajectories( + trajectories, file_writer_factory, builder); + RegisterFileWritingPointsProcessorWithTrajectories< + ProbabilityGridPointsProcessor>(trajectories, file_writer_factory, + builder); +} + +void PointsProcessorPipelineBuilder::Register(const std::string& name, + FactoryFunction factory) { + CHECK(factories_.count(name) == 0) << "A points processor named '" << name + << "' has already been registered."; + factories_[name] = std::move(factory); +} + +PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {} + +std::vector> +PointsProcessorPipelineBuilder::CreatePipeline( + common::LuaParameterDictionary* const dictionary) const { + std::vector> pipeline; + // The last consumer in the pipeline must exist, so that the one created after + // it (and being before it in the pipeline) has a valid 'next' to point to. + // The last consumer will just drop all points. + pipeline.emplace_back(absl::make_unique()); + + std::vector> configurations = + dictionary->GetArrayValuesAsDictionaries(); + + // We construct the pipeline starting at the back. + for (auto it = configurations.rbegin(); it != configurations.rend(); it++) { + const std::string action = (*it)->GetString("action"); + auto factory_it = factories_.find(action); + CHECK(factory_it != factories_.end()) + << "Unknown action '" << action + << "'. Did you register the correspoinding PointsProcessor?"; + pipeline.push_back(factory_it->second(it->get(), pipeline.back().get())); + } + return pipeline; +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder.h b/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder.h new file mode 100644 index 0000000..2e576d4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder.h @@ -0,0 +1,71 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_ +#define CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_ + +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/proto/trajectory.pb.h" + +namespace cartographer { +namespace io { + +// Builder to create a points processor pipeline out of a Lua configuration. +// You can register all built-in PointsProcessors using +// 'RegisterBuiltInPointsProcessors'. Non-built-in PointsProcessors must define +// a name and a factory method for building itself from a +// LuaParameterDictionary. See the various built-in PointsProcessors for +// examples. +class PointsProcessorPipelineBuilder { + public: + using FactoryFunction = std::function( + common::LuaParameterDictionary*, PointsProcessor* next)>; + + PointsProcessorPipelineBuilder(); + + PointsProcessorPipelineBuilder(const PointsProcessorPipelineBuilder&) = + delete; + PointsProcessorPipelineBuilder& operator=( + const PointsProcessorPipelineBuilder&) = delete; + + // Register a new PointsProcessor type uniquly identified by 'name' which will + // be created using 'factory'. + void Register(const std::string& name, FactoryFunction factory); + + std::vector> CreatePipeline( + common::LuaParameterDictionary* dictionary) const; + + private: + absl::flat_hash_map factories_; +}; + +// Register all 'PointsProcessor' that ship with Cartographer with this +// 'builder'. +void RegisterBuiltInPointsProcessors( + const std::vector& trajectories, + const FileWriterFactory& file_writer_factory, + PointsProcessorPipelineBuilder* builder); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder_test.cc b/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder_test.cc new file mode 100644 index 0000000..da55241 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/points_processor_pipeline_builder_test.cc @@ -0,0 +1,40 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/points_processor_pipeline_builder.h" + +#include + +#include "cartographer/common/port.h" +#include "cartographer/io/file_writer.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +TEST(PointsProcessorPipelineBuilderTest, RegisterBuiltInPointsProcessors) { + PointsProcessorPipelineBuilder builder; + FileWriterFactory dummy_factory = + [](const std::string& filename) -> std::unique_ptr { + return nullptr; + }; + RegisterBuiltInPointsProcessors({}, dummy_factory, &builder); +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor.cc new file mode 100644 index 0000000..907b995 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor.cc @@ -0,0 +1,212 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/probability_grid_points_processor.h" + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/io/draw_trajectories.h" +#include "cartographer/io/image.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { +namespace { + +void DrawTrajectoriesIntoImage( + const mapping::ProbabilityGrid& probability_grid, + const Eigen::Array2i& offset, + const std::vector& trajectories, + cairo_surface_t* cairo_surface) { + for (size_t i = 0; i < trajectories.size(); ++i) { + DrawTrajectory( + trajectories[i], GetColor(i), + [&probability_grid, + &offset](const transform::Rigid3d& pose) -> Eigen::Array2i { + return probability_grid.limits().GetCellIndex( + pose.cast().translation().head<2>()) - + offset; + }, + cairo_surface); + } +} + +uint8 ProbabilityToColor(float probability_from_grid) { + const float probability = 1.f - probability_from_grid; + return ::cartographer::common::RoundToInt( + 255 * ((probability - mapping::kMinProbability) / + (mapping::kMaxProbability - mapping::kMinProbability))); +} + +std::string FileExtensionFromOutputType( + const ProbabilityGridPointsProcessor::OutputType& output_type) { + if (output_type == ProbabilityGridPointsProcessor::OutputType::kPng) { + return ".png"; + } else if (output_type == ProbabilityGridPointsProcessor::OutputType::kPb) { + return ".pb"; + } + LOG(FATAL) << "OutputType does not exist!"; +} + +ProbabilityGridPointsProcessor::OutputType OutputTypeFromString( + const std::string& output_type) { + if (output_type == "png") { + return ProbabilityGridPointsProcessor::OutputType::kPng; + } else if (output_type == "pb") { + return ProbabilityGridPointsProcessor::OutputType::kPb; + } else { + LOG(FATAL) << "OutputType " << output_type << " does not exist!"; + } +} + +} // namespace + +ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor( + const double resolution, + const mapping::proto::ProbabilityGridRangeDataInserterOptions2D& + probability_grid_range_data_inserter_options, + const DrawTrajectories& draw_trajectories, const OutputType& output_type, + std::unique_ptr file_writer, + const std::vector& trajectories, + PointsProcessor* const next) + : draw_trajectories_(draw_trajectories), + output_type_(output_type), + trajectories_(trajectories), + file_writer_(std::move(file_writer)), + next_(next), + range_data_inserter_(probability_grid_range_data_inserter_options), + probability_grid_( + CreateProbabilityGrid(resolution, &conversion_tables_)) { + LOG_IF(WARNING, output_type == OutputType::kPb && + draw_trajectories_ == DrawTrajectories::kYes) + << "Drawing the trajectories is not supported when writing the " + "probability grid as protobuf."; +} + +std::unique_ptr +ProbabilityGridPointsProcessor::FromDictionary( + const std::vector& trajectories, + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") || + dictionary->GetBool("draw_trajectories")) + ? DrawTrajectories::kYes + : DrawTrajectories::kNo; + const auto output_type = + dictionary->HasKey("output_type") + ? OutputTypeFromString(dictionary->GetString("output_type")) + : OutputType::kPng; + return absl::make_unique( + dictionary->GetDouble("resolution"), + mapping::CreateProbabilityGridRangeDataInserterOptions2D( + dictionary->GetDictionary("range_data_inserter").get()), + draw_trajectories, output_type, + file_writer_factory(dictionary->GetString("filename") + + FileExtensionFromOutputType(output_type)), + trajectories, next); +} + +void ProbabilityGridPointsProcessor::Process( + std::unique_ptr batch) { + range_data_inserter_.Insert( + {batch->origin, sensor::PointCloud(batch->points), {}}, + &probability_grid_); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { + if (output_type_ == OutputType::kPng) { + Eigen::Array2i offset; + std::unique_ptr image = + DrawProbabilityGrid(probability_grid_, &offset); + if (image != nullptr) { + if (draw_trajectories_ == + ProbabilityGridPointsProcessor::DrawTrajectories::kYes) { + DrawTrajectoriesIntoImage(probability_grid_, offset, trajectories_, + image->GetCairoSurface().get()); + } + image->WritePng(file_writer_.get()); + CHECK(file_writer_->Close()); + } + } else if (output_type_ == OutputType::kPb) { + const auto probability_grid_proto = probability_grid_.ToProto(); + std::string probability_grid_serialized; + probability_grid_proto.SerializeToString(&probability_grid_serialized); + file_writer_->Write(probability_grid_serialized.data(), + probability_grid_serialized.size()); + CHECK(file_writer_->Close()); + } else { + LOG(FATAL) << "Output Type " << FileExtensionFromOutputType(output_type_) + << " is not supported."; + } + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "ProbabilityGrid generation must be configured to occur " + "after any stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL); + // The following unreachable return statement is needed to avoid a GCC bug + // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508 + return FlushResult::kFinished; +} + +std::unique_ptr DrawProbabilityGrid( + const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset) { + mapping::CellLimits cell_limits; + probability_grid.ComputeCroppedLimits(offset, &cell_limits); + if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { + LOG(WARNING) << "Not writing output: empty probability grid"; + return nullptr; + } + auto image = absl::make_unique(cell_limits.num_x_cells, + cell_limits.num_y_cells); + for (const Eigen::Array2i& xy_index : + mapping::XYIndexRangeIterator(cell_limits)) { + const Eigen::Array2i index = xy_index + *offset; + constexpr uint8 kUnknownValue = 128; + const uint8 value = + probability_grid.IsKnown(index) + ? ProbabilityToColor(probability_grid.GetProbability(index)) + : kUnknownValue; + image->SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}}); + } + return image; +} + +mapping::ProbabilityGrid CreateProbabilityGrid( + const double resolution, + mapping::ValueConversionTables* conversion_tables) { + constexpr int kInitialProbabilityGridSize = 100; + Eigen::Vector2d max = + 0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones(); + return mapping::ProbabilityGrid( + mapping::MapLimits(resolution, max, + mapping::CellLimits(kInitialProbabilityGridSize, + kInitialProbabilityGridSize)), + conversion_tables); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor.h new file mode 100644 index 0000000..2114bf8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor.h @@ -0,0 +1,92 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/io/file_writer.h" +#include "cartographer/io/image.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" + +namespace cartographer { +namespace io { + +// Creates a probability grid with the specified 'resolution'. As all points are +// projected into the x-y plane the z component of the data is ignored. +// 'range_data_inserter' options are used to configure the range data ray +// tracing through the probability grid. +class ProbabilityGridPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "write_probability_grid"; + enum class DrawTrajectories { kNo, kYes }; + enum class OutputType { kPng, kPb }; + ProbabilityGridPointsProcessor( + double resolution, + const mapping::proto::ProbabilityGridRangeDataInserterOptions2D& + probability_grid_range_data_inserter_options, + const DrawTrajectories& draw_trajectories, const OutputType& output_type, + std::unique_ptr file_writer, + const std::vector& trajectories, + PointsProcessor* next); + ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) = + delete; + ProbabilityGridPointsProcessor& operator=( + const ProbabilityGridPointsProcessor&) = delete; + + static std::unique_ptr FromDictionary( + const std::vector& trajectories, + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~ProbabilityGridPointsProcessor() override {} + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const DrawTrajectories draw_trajectories_; + const OutputType output_type_; + const std::vector trajectories_; + std::unique_ptr file_writer_; + PointsProcessor* const next_; + mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_; + mapping::ValueConversionTables conversion_tables_; + mapping::ProbabilityGrid probability_grid_; +}; + +// Draws 'probability_grid' into an image and fills in 'offset' with the cropped +// map limits. Returns 'nullptr' if probability_grid was empty. +std::unique_ptr DrawProbabilityGrid( + const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset); + +// Create an initially empty probability grid with 'resolution' and a small +// size, suitable for a PointsBatchProcessor. +mapping::ProbabilityGrid CreateProbabilityGrid( + const double resolution, mapping::ValueConversionTables* conversion_tables); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor_test.cc b/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor_test.cc new file mode 100644 index 0000000..1af0a25 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/probability_grid_points_processor_test.cc @@ -0,0 +1,159 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/probability_grid_points_processor.h" + +#include + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/io/fake_file_writer.h" +#include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/value_conversion_tables.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +std::unique_ptr CreatePointsBatch() { + auto points_batch = ::absl::make_unique(); + points_batch->origin = Eigen::Vector3f(0, 0, 0); + points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}}); + points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}}); + points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}}); + points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}}); + points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}}); + return points_batch; +} + +::cartographer::io::FileWriterFactory CreateFakeFileWriterFactory( + const std::string& expected_filename, + std::shared_ptr> fake_file_writer_output) { + return [&fake_file_writer_output, + &expected_filename](const std::string& full_filename) { + EXPECT_EQ(expected_filename, full_filename); + return ::absl::make_unique<::cartographer::io::FakeFileWriter>( + full_filename, fake_file_writer_output); + }; +} + +std::vector> +CreatePipelineFromDictionary( + common::LuaParameterDictionary* const pipeline_dictionary, + const std::vector& trajectories, + ::cartographer::io::FileWriterFactory file_writer_factory) { + auto builder = + ::absl::make_unique<::cartographer::io::PointsProcessorPipelineBuilder>(); + builder->Register( + ProbabilityGridPointsProcessor::kConfigurationFileActionName, + [&trajectories, file_writer_factory]( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return ProbabilityGridPointsProcessor::FromDictionary( + trajectories, file_writer_factory, dictionary, next); + }); + + return builder->CreatePipeline(pipeline_dictionary); +} + +std::vector CreateExpectedProbabilityGrid( + std::unique_ptr points_batch, + common::LuaParameterDictionary* const probability_grid_options) { + ::cartographer::mapping::ProbabilityGridRangeDataInserter2D + range_data_inserter( + cartographer::mapping:: + CreateProbabilityGridRangeDataInserterOptions2D( + probability_grid_options->GetDictionary("range_data_inserter") + .get())); + mapping::ValueConversionTables conversion_tables; + auto probability_grid = CreateProbabilityGrid( + probability_grid_options->GetDouble("resolution"), &conversion_tables); + range_data_inserter.Insert( + {points_batch->origin, sensor::PointCloud(points_batch->points), {}}, + &probability_grid); + + std::vector probability_grid_proto( + probability_grid.ToProto().ByteSize()); + probability_grid.ToProto().SerializePartialToArray( + probability_grid_proto.data(), probability_grid_proto.size()); + return probability_grid_proto; +} + +std::unique_ptr CreateParameterDictionary() { + auto parameter_dictionary = + cartographer::common::LuaParameterDictionary::NonReferenceCounted( + R"text( + pipeline = { + { + action = "write_probability_grid", + resolution = 0.05, + range_data_inserter = { + insert_free_space = true, + hit_probability = 0.55, + miss_probability = 0.49, + }, + draw_trajectories = false, + output_type = "pb", + filename = "map" + } + } + return pipeline + )text", + absl::make_unique()); + return parameter_dictionary; +} + +class ProbabilityGridPointsProcessorTest : public ::testing::Test { + protected: + ProbabilityGridPointsProcessorTest() + : pipeline_dictionary_(CreateParameterDictionary()) {} + + void Run(const std::string& expected_filename) { + const auto pipeline = CreatePipelineFromDictionary( + pipeline_dictionary_.get(), dummy_trajectories_, + CreateFakeFileWriterFactory(expected_filename, + fake_file_writer_output_)); + EXPECT_TRUE(pipeline.size() > 0); + + do { + pipeline.back()->Process(CreatePointsBatch()); + } while (pipeline.back()->Flush() == + cartographer::io::PointsProcessor::FlushResult::kRestartStream); + } + + std::shared_ptr> fake_file_writer_output_ = + std::make_shared>(); + std::unique_ptr + pipeline_dictionary_; + const std::vector dummy_trajectories_; +}; + +TEST_F(ProbabilityGridPointsProcessorTest, WriteProto) { + const auto expected_prob_grid_proto = CreateExpectedProbabilityGrid( + CreatePointsBatch(), + pipeline_dictionary_->GetArrayValuesAsDictionaries().front().get()); + Run("map.pb"); + EXPECT_THAT(*fake_file_writer_output_, + ::testing::ContainerEq(expected_prob_grid_proto)); +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream.cc b/carto_ws/src/cartographer-master/cartographer/io/proto_stream.cc new file mode 100644 index 0000000..60ea021 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream.cc @@ -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. + */ + +#include "cartographer/io/proto_stream.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +namespace { + +// First eight bytes to identify our proto stream format. +const uint64 kMagic = 0x7b1d1f7b5bf501db; + +void WriteSizeAsLittleEndian(uint64 size, std::ostream* out) { + for (int i = 0; i != 8; ++i) { + out->put(size & 0xff); + size >>= 8; + } +} + +bool ReadSizeAsLittleEndian(std::istream* in, uint64* size) { + *size = 0; + for (int i = 0; i != 8; ++i) { + *size >>= 8; + *size += static_cast(in->get()) << 56; + } + return !in->fail(); +} + +} // namespace + +ProtoStreamWriter::ProtoStreamWriter(const std::string& filename) + : out_(filename, std::ios::out | std::ios::binary) { + WriteSizeAsLittleEndian(kMagic, &out_); +} + +void ProtoStreamWriter::Write(const std::string& uncompressed_data) { + std::string compressed_data; + common::FastGzipString(uncompressed_data, &compressed_data); + WriteSizeAsLittleEndian(compressed_data.size(), &out_); + out_.write(compressed_data.data(), compressed_data.size()); +} + +void ProtoStreamWriter::WriteProto(const google::protobuf::Message& proto) { + std::string uncompressed_data; + proto.SerializeToString(&uncompressed_data); + Write(uncompressed_data); +} + +bool ProtoStreamWriter::Close() { + out_.close(); + return !out_.fail(); +} + +ProtoStreamReader::ProtoStreamReader(const std::string& filename) + : in_(filename, std::ios::in | std::ios::binary) { + uint64 magic; + if (!ReadSizeAsLittleEndian(&in_, &magic) || magic != kMagic) { + in_.setstate(std::ios::failbit); + } + CHECK(in_.good()) << "Failed to open proto stream '" << filename << "'."; +} + +bool ProtoStreamReader::Read(std::string* decompressed_data) { + uint64 compressed_size; + if (!ReadSizeAsLittleEndian(&in_, &compressed_size)) { + return false; + } + std::string compressed_data(compressed_size, '\0'); + if (!in_.read(&compressed_data.front(), compressed_size)) { + return false; + } + common::FastGunzipString(compressed_data, decompressed_data); + return true; +} + +bool ProtoStreamReader::ReadProto(google::protobuf::Message* proto) { + std::string decompressed_data; + return Read(&decompressed_data) && proto->ParseFromString(decompressed_data); +} + +bool ProtoStreamReader::eof() const { return in_.eof(); } + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream.h b/carto_ws/src/cartographer-master/cartographer/io/proto_stream.h new file mode 100644 index 0000000..fa27056 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream.h @@ -0,0 +1,73 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_PROTO_STREAM_H_ +#define CARTOGRAPHER_IO_PROTO_STREAM_H_ + +#include + +#include "cartographer/common/port.h" +#include "cartographer/io/proto_stream_interface.h" +#include "google/protobuf/message.h" + +namespace cartographer { +namespace io { + +// A simple writer of a compressed sequence of protocol buffer messages to a +// file. The format is not intended to be compatible with any other format used +// outside of Cartographer. +// +// TODO(whess): Compress the file instead of individual messages for better +// compression performance? Should we use LZ4? +class ProtoStreamWriter : public ProtoStreamWriterInterface { + public: + ProtoStreamWriter(const std::string& filename); + ~ProtoStreamWriter() = default; + + ProtoStreamWriter(const ProtoStreamWriter&) = delete; + ProtoStreamWriter& operator=(const ProtoStreamWriter&) = delete; + + void WriteProto(const google::protobuf::Message& proto) override; + bool Close() override; + + private: + void Write(const std::string& uncompressed_data); + + std::ofstream out_; +}; + +// A reader of the format produced by ProtoStreamWriter. +class ProtoStreamReader : public ProtoStreamReaderInterface { + public: + explicit ProtoStreamReader(const std::string& filename); + ~ProtoStreamReader() = default; + + ProtoStreamReader(const ProtoStreamReader&) = delete; + ProtoStreamReader& operator=(const ProtoStreamReader&) = delete; + + bool ReadProto(google::protobuf::Message* proto) override; + bool eof() const override; + + private: + bool Read(std::string* decompressed_data); + + std::ifstream in_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_PROTO_STREAM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer.cc b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer.cc new file mode 100644 index 0000000..fef0448 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer.cc @@ -0,0 +1,79 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/proto_stream_deserializer.h" + +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { +namespace { +mapping::proto::SerializationHeader ReadHeaderOrDie( + ProtoStreamReaderInterface* const reader) { + mapping::proto::SerializationHeader header; + CHECK(reader->ReadProto(&header)) << "Failed to read SerializationHeader."; + return header; +} + +bool IsVersionSupported(const mapping::proto::SerializationHeader& header) { + return header.format_version() == kMappingStateSerializationFormatVersion || + header.format_version() == kFormatVersionWithoutSubmapHistograms; +} + +} // namespace + +mapping::proto::PoseGraph DeserializePoseGraphFromFile( + const std::string& file_name) { + ProtoStreamReader reader(file_name); + ProtoStreamDeserializer deserializer(&reader); + return deserializer.pose_graph(); +} + +ProtoStreamDeserializer::ProtoStreamDeserializer( + ProtoStreamReaderInterface* const reader) + : reader_(reader), header_(ReadHeaderOrDie(reader)) { + CHECK(IsVersionSupported(header_)) << "Unsupported serialization format \"" + << header_.format_version() << "\""; + + CHECK(ReadNextSerializedData(&pose_graph_)) + << "Serialized stream misses PoseGraph."; + CHECK(pose_graph_.has_pose_graph()) + << "Serialized stream order corrupt. Expecting `PoseGraph` after " + "`SerializationHeader`, but got field tag " + << pose_graph_.data_case(); + + CHECK(ReadNextSerializedData(&all_trajectory_builder_options_)) + << "Serialized stream misses `AllTrajectoryBuilderOptions`."; + CHECK(all_trajectory_builder_options_.has_all_trajectory_builder_options()) + << "Serialized stream order corrupt. Expecting " + "`AllTrajectoryBuilderOptions` after " + "PoseGraph, got field tag " + << all_trajectory_builder_options_.data_case(); + + CHECK_EQ(pose_graph_.pose_graph().trajectory_size(), + all_trajectory_builder_options_.all_trajectory_builder_options() + .options_with_sensor_ids_size()); +} + +bool ProtoStreamDeserializer::ReadNextSerializedData( + mapping::proto::SerializedData* data) { + return reader_->ReadProto(data); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer.h b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer.h new file mode 100644 index 0000000..ee0dfd9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer.h @@ -0,0 +1,72 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_ +#define CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_ + +#include "cartographer/io/proto_stream_interface.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" + +namespace cartographer { +namespace io { + +// Helper function for deserializing the PoseGraph from a proto stream file. +mapping::proto::PoseGraph DeserializePoseGraphFromFile( + const std::string& file_name); + +// Helper for deserializing a previously serialized mapping state from a +// proto stream, abstracting away the format parsing logic. +class ProtoStreamDeserializer { + public: + explicit ProtoStreamDeserializer(ProtoStreamReaderInterface* const reader); + + ProtoStreamDeserializer(const ProtoStreamDeserializer&) = delete; + ProtoStreamDeserializer& operator=(const ProtoStreamDeserializer&) = delete; + ProtoStreamDeserializer(ProtoStreamDeserializer&&) = delete; + + mapping::proto::SerializationHeader& header() { return header_; } + + mapping::proto::PoseGraph& pose_graph() { + return *pose_graph_.mutable_pose_graph(); + } + const mapping::proto::PoseGraph& pose_graph() const { + return pose_graph_.pose_graph(); + } + + const mapping::proto::AllTrajectoryBuilderOptions& + all_trajectory_builder_options() { + return all_trajectory_builder_options_.all_trajectory_builder_options(); + } + + // Reads the next `SerializedData` message of the ProtoStream into `data`. + // Returns `true` if the message was successfully read or `false` in case + // there are no-more messages or an error occurred. + bool ReadNextSerializedData(mapping::proto::SerializedData* data); + + private: + ProtoStreamReaderInterface* reader_; + + mapping::proto::SerializationHeader header_; + mapping::proto::SerializedData pose_graph_; + mapping::proto::SerializedData all_trajectory_builder_options_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer_test.cc b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer_test.cc new file mode 100644 index 0000000..9079799 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_deserializer_test.cc @@ -0,0 +1,143 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/proto_stream_deserializer.h" + +#include + +#include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/io/internal/testing/test_helpers.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "google/protobuf/text_format.h" +#include "google/protobuf/util/message_differencer.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +using ::cartographer::io::testing::ProtoFromStringOrDie; +using ::cartographer::io::testing::ProtoReaderFromStrings; +using ::cartographer::mapping::proto::SerializationHeader; +using ::cartographer::mapping::proto::SerializedData; +using ::google::protobuf::Message; +using ::google::protobuf::util::MessageDifferencer; +using ::testing::Eq; +using ::testing::Not; + +constexpr char kSerializationHeaderProtoString[] = "format_version: 1"; +constexpr char kUnsupportedSerializationHeaderProtoString[] = + "format_version: 123"; +constexpr char kPoseGraphProtoString[] = "pose_graph {}"; +constexpr char kAllTrajectoryBuilderOptionsProtoString[] = + "all_trajectory_builder_options {}"; +constexpr char kSubmapProtoString[] = "submap {}"; +constexpr char kNodeProtoString[] = "node {}"; +constexpr char kTrajectoryDataProtoString[] = "trajectory_data {}"; +constexpr char kImuDataProtoString[] = "imu_data {}"; +constexpr char kOdometryDataProtoString[] = "odometry_data {}"; +constexpr char kFixedFramePoseDataProtoString[] = "fixed_frame_pose_data {}"; +constexpr char kLandmarkDataProtoString[] = "landmark_data {}"; + +class ProtoStreamDeserializerTest : public ::testing::Test { + protected: + std::unique_ptr reader_; +}; + +// This test checks if the serialization works. +TEST_F(ProtoStreamDeserializerTest, WorksOnGoldenTextStream) { + // Load text proto into in_memory_reader. + reader_ = ProtoReaderFromStrings(kSerializationHeaderProtoString, + { + kPoseGraphProtoString, + kAllTrajectoryBuilderOptionsProtoString, + kSubmapProtoString, + kNodeProtoString, + kTrajectoryDataProtoString, + kImuDataProtoString, + kOdometryDataProtoString, + kFixedFramePoseDataProtoString, + kLandmarkDataProtoString, + }); + + io::ProtoStreamDeserializer deserializer(reader_.get()); + + EXPECT_TRUE(MessageDifferencer::Equals( + deserializer.header(), ProtoFromStringOrDie( + kSerializationHeaderProtoString))); + + EXPECT_TRUE(MessageDifferencer::Equals( + deserializer.pose_graph(), + ProtoFromStringOrDie(kPoseGraphProtoString) + .pose_graph())); + + EXPECT_TRUE( + MessageDifferencer::Equals(deserializer.all_trajectory_builder_options(), + ProtoFromStringOrDie( + kAllTrajectoryBuilderOptionsProtoString) + .all_trajectory_builder_options())); + + SerializedData serialized_data; + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + // TODO(sebastianklose): Add matcher for protos in common place and use here. + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kSubmapProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, ProtoFromStringOrDie(kNodeProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kTrajectoryDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kImuDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kOdometryDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kFixedFramePoseDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kLandmarkDataProtoString))); + + EXPECT_FALSE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(reader_->eof()); +} + +TEST_F(ProtoStreamDeserializerTest, FailsIfVersionNotSupported) { + reader_ = + ProtoReaderFromStrings(kUnsupportedSerializationHeaderProtoString, {}); + EXPECT_DEATH(absl::make_unique(reader_.get()), + "Unsupported serialization format"); +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream_interface.h b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_interface.h new file mode 100644 index 0000000..18ca2e4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_interface.h @@ -0,0 +1,54 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_PROTO_STREAM_INTERFACE_H_ +#define CARTOGRAPHER_IO_PROTO_STREAM_INTERFACE_H_ + +#include "cartographer/common/port.h" +#include "google/protobuf/message.h" + +namespace cartographer { +namespace io { + +// A writer for writing proto messages to a pbstream. +class ProtoStreamWriterInterface { + public: + virtual ~ProtoStreamWriterInterface() {} + + // Serializes, compressed and writes the 'proto' to the file. + virtual void WriteProto(const google::protobuf::Message& proto) = 0; + + // This should be called to check whether writing was successful. + virtual bool Close() = 0; +}; + +// A reader of the format produced by ProtoStreamWriter. +class ProtoStreamReaderInterface { + public: + ProtoStreamReaderInterface() = default; + virtual ~ProtoStreamReaderInterface() {} + + // Deserialize compressed proto from the pb stream. + virtual bool ReadProto(google::protobuf::Message* proto) = 0; + + // 'End-of-file' marker for the pb stream. + virtual bool eof() const = 0; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_PROTO_STREAM_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/proto_stream_test.cc b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_test.cc new file mode 100644 index 0000000..f2514a2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/proto_stream_test.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/proto_stream.h" + +#include +#include +#include +#include + +#include "cartographer/common/port.h" +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +class ProtoStreamTest : public ::testing::Test { + protected: + void SetUp() override { test_directory_ = "."; } + + std::string test_directory_; +}; + +TEST_F(ProtoStreamTest, WriteAndReadBack) { + const std::string test_file = test_directory_ + "/test_trajectory.pbstream"; + { + ProtoStreamWriter writer(test_file); + for (int i = 0; i != 10; ++i) { + mapping::proto::Trajectory trajectory; + trajectory.add_node()->set_timestamp(i); + writer.WriteProto(trajectory); + } + ASSERT_TRUE(writer.Close()); + } + { + ProtoStreamReader reader(test_file); + for (int i = 0; i != 10; ++i) { + mapping::proto::Trajectory trajectory; + ASSERT_TRUE(reader.ReadProto(&trajectory)); + ASSERT_EQ(1, trajectory.node_size()); + EXPECT_EQ(i, trajectory.node(0).timestamp()); + } + mapping::proto::Trajectory trajectory; + EXPECT_FALSE(reader.ReadProto(&trajectory)); + } + remove(test_file.c_str()); +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration.cc b/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration.cc new file mode 100644 index 0000000..7ed692e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration.cc @@ -0,0 +1,278 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/serialization_format_migration.h" + +#include + +#include "cartographer/common/config.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +using mapping::proto::SerializedData; + +void MigrateStreamVersion1ToVersion2( + cartographer::io::ProtoStreamReaderInterface* const input, + cartographer::io::ProtoStreamWriterInterface* const output, + bool include_unfinished_submaps) { + auto file_resolver = ::absl::make_unique( + std::vector{std::string(common::kSourceDirectory) + + "/configuration_files"}); + const std::string kCode = R"text( + include "map_builder.lua" + MAP_BUILDER.use_trajectory_builder_3d = true + return MAP_BUILDER)text"; + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + kCode, std::move(file_resolver)); + const auto options = + mapping::CreateMapBuilderOptions(&lua_parameter_dictionary); + + common::ThreadPool thread_pool(1); + CHECK(!options.use_trajectory_builder_2d()); + // We always use 3D here. 2D submaps do not have histograms. + mapping::PoseGraph3D pose_graph( + options.pose_graph_options(), + absl::make_unique( + options.pose_graph_options().optimization_problem_options()), + &thread_pool); + + ProtoStreamDeserializer deserializer(input); + + // Create a copy of the pose_graph_proto, such that we can re-write the + // trajectory ids. + mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph(); + const auto& all_builder_options_proto = + deserializer.all_trajectory_builder_options(); + + std::vector + trajectory_builder_options; + for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) { + auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i); + const auto& options_with_sensor_ids_proto = + all_builder_options_proto.options_with_sensor_ids(i); + trajectory_builder_options.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_proto.trajectory_id(), i); + } + + // Apply the calculated remapping to constraints in the pose graph proto. + for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) { + constraint_proto.mutable_submap_id()->set_trajectory_id( + constraint_proto.submap_id().trajectory_id()); + constraint_proto.mutable_node_id()->set_trajectory_id( + constraint_proto.node_id().trajectory_id()); + } + + mapping::MapById submap_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Submap& submap_proto : + trajectory_proto.submap()) { + submap_poses.Insert(mapping::SubmapId{trajectory_proto.trajectory_id(), + submap_proto.submap_index()}, + transform::ToRigid3(submap_proto.pose())); + } + } + + mapping::MapById node_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Node& node_proto : + trajectory_proto.node()) { + node_poses.Insert(mapping::NodeId{trajectory_proto.trajectory_id(), + node_proto.node_index()}, + transform::ToRigid3(node_proto.pose())); + } + } + + // Set global poses of landmarks. + for (const auto& landmark : pose_graph_proto.landmark_poses()) { + pose_graph.SetLandmarkPose(landmark.landmark_id(), + transform::ToRigid3(landmark.global_pose()), + true); + } + + mapping::MapById + submap_id_to_submap; + mapping::MapById node_id_to_node; + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + switch (proto.data_case()) { + case SerializedData::kPoseGraph: + LOG(FATAL) << "Found multiple serialized `PoseGraph`. Serialized " + "stream likely corrupt!."; + case SerializedData::kAllTrajectoryBuilderOptions: + LOG(FATAL) << "Found multiple serialized " + "`AllTrajectoryBuilderOptions`. Serialized stream likely " + "corrupt!."; + case SerializedData::kSubmap: { + CHECK(proto.submap().has_submap_3d()) + << "Converting to the new submap format only makes sense for 3D."; + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + proto.submap().submap_id().trajectory_id()); + submap_id_to_submap.Insert( + mapping::SubmapId{proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()}, + proto.submap()); + break; + } + case SerializedData::kNode: { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + proto.node().node_id().trajectory_id()); + const mapping::NodeId node_id(proto.node().node_id().trajectory_id(), + proto.node().node_id().node_index()); + const transform::Rigid3d& node_pose = node_poses.at(node_id); + pose_graph.AddNodeFromProto(node_pose, proto.node()); + node_id_to_node.Insert(node_id, proto.node()); + break; + } + case SerializedData::kTrajectoryData: { + proto.mutable_trajectory_data()->set_trajectory_id( + proto.trajectory_data().trajectory_id()); + pose_graph.SetTrajectoryDataFromProto(proto.trajectory_data()); + break; + } + case SerializedData::kImuData: { + pose_graph.AddImuData(proto.imu_data().trajectory_id(), + sensor::FromProto(proto.imu_data().imu_data())); + break; + } + case SerializedData::kOdometryData: { + pose_graph.AddOdometryData( + proto.odometry_data().trajectory_id(), + sensor::FromProto(proto.odometry_data().odometry_data())); + break; + } + case SerializedData::kFixedFramePoseData: { + pose_graph.AddFixedFramePoseData( + proto.fixed_frame_pose_data().trajectory_id(), + sensor::FromProto( + proto.fixed_frame_pose_data().fixed_frame_pose_data())); + break; + } + case SerializedData::kLandmarkData: { + pose_graph.AddLandmarkData( + proto.landmark_data().trajectory_id(), + sensor::FromProto(proto.landmark_data().landmark_data())); + break; + } + default: + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); + } + } + + // TODO(schwoere): Remove backwards compatibility once the pbstream format + // version 2 is established. + if (deserializer.header().format_version() == + kFormatVersionWithoutSubmapHistograms) { + submap_id_to_submap = MigrateSubmapFormatVersion1ToVersion2( + submap_id_to_submap, node_id_to_node, pose_graph_proto); + } + + for (const auto& submap_id_submap : submap_id_to_submap) { + pose_graph.AddSubmapFromProto(submap_poses.at(submap_id_submap.id), + submap_id_submap.data); + } + + pose_graph.AddSerializedConstraints( + mapping::FromProto(pose_graph_proto.constraint())); + CHECK(input->eof()); + + WritePbStream(pose_graph, trajectory_builder_options, output, + include_unfinished_submaps); +} + +mapping::MapById +MigrateSubmapFormatVersion1ToVersion2( + const mapping::MapById& + submap_id_to_submap, + mapping::MapById& node_id_to_node, + const mapping::proto::PoseGraph& pose_graph_proto) { + using namespace mapping; + if (submap_id_to_submap.empty() || + submap_id_to_submap.begin()->data.has_submap_2d()) { + return submap_id_to_submap; + } + + MapById migrated_submaps = submap_id_to_submap; + for (const proto::PoseGraph::Constraint& constraint_proto : + pose_graph_proto.constraint()) { + if (constraint_proto.tag() == proto::PoseGraph::Constraint::INTRA_SUBMAP) { + NodeId node_id{constraint_proto.node_id().trajectory_id(), + constraint_proto.node_id().node_index()}; + CHECK(node_id_to_node.Contains(node_id)); + const TrajectoryNode::Data node_data = + FromProto(node_id_to_node.at(node_id).node_data()); + const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity = + node_data.rotational_scan_matcher_histogram; + + SubmapId submap_id{constraint_proto.submap_id().trajectory_id(), + constraint_proto.submap_id().submap_index()}; + CHECK(migrated_submaps.Contains(submap_id)); + proto::Submap& migrated_submap_proto = migrated_submaps.at(submap_id); + CHECK(migrated_submap_proto.has_submap_3d()); + + proto::Submap3D* submap_3d_proto = + migrated_submap_proto.mutable_submap_3d(); + const double submap_yaw_from_gravity = + transform::GetYaw(transform::ToRigid3(submap_3d_proto->local_pose()) + .inverse() + .rotation() * + node_data.local_pose.rotation() * + node_data.gravity_alignment.inverse()); + const Eigen::VectorXf rotational_scan_matcher_histogram_in_submap = + scan_matching::RotationalScanMatcher::RotateHistogram( + rotational_scan_matcher_histogram_in_gravity, + submap_yaw_from_gravity); + + if (submap_3d_proto->rotational_scan_matcher_histogram_size() == 0) { + for (Eigen::VectorXf::Index i = 0; + i != rotational_scan_matcher_histogram_in_submap.size(); ++i) { + submap_3d_proto->add_rotational_scan_matcher_histogram( + rotational_scan_matcher_histogram_in_submap(i)); + } + } else { + auto submap_histogram = + submap_3d_proto->mutable_rotational_scan_matcher_histogram(); + for (Eigen::VectorXf::Index i = 0; + i != rotational_scan_matcher_histogram_in_submap.size(); ++i) { + *submap_histogram->Mutable(i) += + rotational_scan_matcher_histogram_in_submap(i); + } + } + } + } + return migrated_submaps; +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration.h b/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration.h new file mode 100644 index 0000000..32e101d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration.h @@ -0,0 +1,45 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_SERIALIZATION_FORMAT_MIGRATION_H_ +#define CARTOGRAPHER_IO_SERIALIZATION_FORMAT_MIGRATION_H_ + +#include "cartographer/io/proto_stream_interface.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/proto/serialization.pb.h" + +namespace cartographer { +namespace io { + +// This helper function migrates the input stream, which is supposed +// to contain submaps without histograms (stream format version 1) to +// an output stream containing submaps with histograms (version 2). +void MigrateStreamVersion1ToVersion2( + cartographer::io::ProtoStreamReaderInterface* const input, + cartographer::io::ProtoStreamWriterInterface* const output, + bool include_unfinished_submaps); + +mapping::MapById +MigrateSubmapFormatVersion1ToVersion2( + const mapping::MapById& + submap_id_to_submaps, + mapping::MapById& node_id_to_nodes, + const mapping::proto::PoseGraph& pose_graph_proto); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_SERIALIZATION_FORMAT_MIGRATION_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration_test.cc b/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration_test.cc new file mode 100644 index 0000000..19e3535 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/serialization_format_migration_test.cc @@ -0,0 +1,111 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/serialization_format_migration.h" + +#include +#include + +#include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +class SubmapHistogramMigrationTest : public ::testing::Test { + protected: + void SetUp() override { + CreateSubmap(); + CreateNode(); + CreatePoseGraphWithNodeToSubmapConstraint(); + } + + private: + void CreateSubmap() { + submap_ = mapping::testing::CreateFakeSubmap3D(); + submap_.mutable_submap_3d()->clear_rotational_scan_matcher_histogram(); + } + + void CreateNode() { + node_ = mapping::testing::CreateFakeNode(); + constexpr int histogram_size = 10; + for (int i = 0; i < histogram_size; ++i) { + node_.mutable_node_data()->add_rotational_scan_matcher_histogram(1.f); + } + } + + void CreatePoseGraphWithNodeToSubmapConstraint() { + mapping::proto::PoseGraph::Constraint* constraint = + pose_graph_.add_constraint(); + constraint->set_tag(mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP); + *constraint->mutable_node_id() = node_.node_id(); + *constraint->mutable_submap_id() = submap_.submap_id(); + *constraint->mutable_relative_pose() = + transform::ToProto(transform::Rigid3d::Identity()); + } + + protected: + mapping::proto::PoseGraph pose_graph_; + mapping::proto::Submap submap_; + mapping::proto::Node node_; +}; + +TEST_F(SubmapHistogramMigrationTest, + SubmapHistogramGenerationFromTrajectoryNodes) { + mapping::MapById + submap_id_to_submap; + mapping::SubmapId submap_id(submap_.submap_id().trajectory_id(), + submap_.submap_id().submap_index()); + submap_id_to_submap.Insert(submap_id, submap_); + + mapping::MapById node_id_to_node; + mapping::NodeId node_id(node_.node_id().trajectory_id(), + node_.node_id().node_index()); + node_id_to_node.Insert(node_id, node_); + + const auto submap_id_to_submap_migrated = + MigrateSubmapFormatVersion1ToVersion2(submap_id_to_submap, + node_id_to_node, pose_graph_); + + EXPECT_EQ(submap_id_to_submap_migrated.size(), submap_id_to_submap.size()); + const mapping::proto::Submap& migrated_submap = + submap_id_to_submap_migrated.at(submap_id); + + EXPECT_FALSE(migrated_submap.has_submap_2d()); + EXPECT_TRUE(migrated_submap.has_submap_3d()); + const mapping::proto::Submap3D& migrated_submap_3d = + migrated_submap.submap_3d(); + const mapping::proto::TrajectoryNodeData& node_data = node_.node_data(); + EXPECT_EQ(migrated_submap_3d.rotational_scan_matcher_histogram_size(), + node_data.rotational_scan_matcher_histogram_size()); + for (int i = 0; i < node_data.rotational_scan_matcher_histogram_size(); ++i) { + EXPECT_EQ(migrated_submap_3d.rotational_scan_matcher_histogram(i), + node_data.rotational_scan_matcher_histogram(i)); + } +} + +} // namespace +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/submap_painter.cc b/carto_ws/src/cartographer-master/cartographer/io/submap_painter.cc new file mode 100644 index 0000000..fe250a3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/submap_painter.cc @@ -0,0 +1,549 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/submap_painter.h" +#include +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cairo/cairo.h" +#include +#include +int c[4000000]={0}; +int a[4000000]={0}; +namespace cartographer { +namespace io { +namespace { + +Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) { + return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation(); +} + +void CairoPaintSubmapSlices( + const double scale, + const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + cairo_t* cr, std::function draw_callback) { + cairo_scale(cr, scale, scale); + + for (auto& pair : submaps) { + const auto& submap_slice = pair.second; + const auto& submap_id = pair.first; + if (submap_slice.surface == nullptr) { + return; + } + const Eigen::Matrix4d homo = + ToEigen(submap_slice.pose * submap_slice.slice_pose).matrix(); + + cairo_save(cr); + cairo_matrix_t matrix; + cairo_matrix_init(&matrix, homo(1, 0), homo(0, 0), -homo(1, 1), -homo(0, 1), + homo(0, 3), -homo(1, 3)); + cairo_transform(cr, &matrix); + + const double submap_resolution = submap_slice.resolution; + cairo_scale(cr, submap_resolution, submap_resolution); + + // Invokes caller's callback to utilize slice data in global cooridnate + // frame. e.g. finds bounding box, paints slices. + draw_callback(submap_id,submap_slice); + cairo_restore(cr); + } +} + +bool Has2DGrid(const mapping::proto::Submap& submap) { + return submap.has_submap_2d() && submap.submap_2d().has_grid(); +} + +bool Has3DGrids(const mapping::proto::Submap& submap) { + return submap.has_submap_3d() && + submap.submap_3d().has_low_resolution_hybrid_grid() && + submap.submap_3d().has_high_resolution_hybrid_grid(); +} + +} // namespace + +//按value值比较 +bool cmp_value(const std::pair left,const std::pair right) +{ + return left.second < right.second; +} + + +//如何初始化（0，0）？ +std::vector < std::map > semmap(4000000); +//存放计算过的子图 +std::map semmap_id; +int num=0; + +int* Slices( + const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + const double resolution) { + Eigen::AlignedBox2f bounding_box; + { + auto surface = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, 1, 1)); + auto cr = MakeUniqueCairoPtr(cairo_create(surface.get())); + const auto update_bounding_box = [&bounding_box, &cr](double x, double y) { + cairo_user_to_device(cr.get(), &x, &y); + bounding_box.extend(Eigen::Vector2f(x, y)); + }; + + + CairoPaintSubmapSlices( + 1. / resolution, submaps, cr.get(), + [&update_bounding_box](const ::cartographer::mapping::SubmapId& submap_id,const SubmapSlice& submap_slice) { + update_bounding_box(0, 0); + update_bounding_box(submap_slice.width, 0); + update_bounding_box(0, submap_slice.height); + update_bounding_box(submap_slice.width, submap_slice.height); + }); + } + num=num+1; + //std::cout<(cairo_image_surface_get_data(submap_slice.surface.get())); + //std::cout<(id,1)); + for (int y = height - 1; y >= 0; --y) { + for (int x = 0; x < width; ++x) { + const uint32_t packed = pixel_data[y * width + x]; + const unsigned char sem =(uint8_t)(packed & 0xFFu); + int xm=homo(1,0)*x-homo(1,1)*y+20*homo(0,3)+o_x-1; + int ym=homo(0,0)*x-homo(0,1)*y-20*homo(1,3)+1+o_y; + std::map ::iterator iter; + //记得给semmap初始化 + //vector.at()会抛出越界访问,vector[]不会，注意vector的越界调用，编译可能不会报错，读取到的数据是随机的 + iter=semmap.at(ym * 2000 + xm).find(sem); + if(iter != semmap.at(ym * 2000 + xm).end()){ + if(iter->second<=5){ + semmap.at(ym * 2000 + xm)[sem]=semmap.at(ym * 2000 + xm)[sem]+1; + //在摄像头视域时，for循环给其他语义减一 + if (sem!=0){ + iter=semmap.at(ym * 2000 + xm).begin(); + for(;iter != semmap.at(ym * 2000 + xm).end();iter++){ + if(iter->first!=sem && iter->second>0 ){ + iter->second=iter->second-1; + } + } + } + } + }else{ + semmap.at(ym * 2000 + xm).insert(std::pair(sem,1)); + } + //策略2 保存0的键值对 + int value_0; + auto iter0=semmap.at(ym * 2000 + xm).find(0); + //如果存在0的键值对 因为不知道怎么给全局变量赋值0：0 + if (iter0 != semmap.at(ym*2000+xm).end() ){ + //如果0的value值大于等于1 + value_0=iter0->second; + //删除0的键值对 + semmap.at(ym*2000+xm).erase(iter0); + //返回除0外value值最大的迭代器 + auto max_probability= max_element(semmap.at(ym * 2000 + xm).begin(),semmap.at(ym * 2000 + xm).end(),cmp_value); + if ((max_probability->second)>=3 && max_probability->first!=1 && xm<2000 && ym<2000 && xm>=0 && ym>=0){ + //std::cout<second<<"sec "; + a[ym * 2000 + xm]=max_probability->first; + }else{ + a[ym * 2000 + xm]=0; + } + //恢复0键值对 + semmap.at(ym * 2000 + xm).insert(std::pair(0,value_0)); + } + //如果不存在0的键值对 + // else{ + // auto max_probability= max_element(semmap.at(ym * 2000 + xm).begin(),semmap.at(ym * 2000 + xm).end(),cmp_value); + // if (xm<2000 && ym<2000 && xm>=0 && ym>=0){ + // a[ym * 2000 + xm]=max_probability->first; + // } + // } + + + + // int total=0; + // iter=semmap.at(ym * 2000 + xm).begin(); + // for (;iter != semmap.at(ym * 2000 + xm).end();iter++){ + // //考虑int类型表示的数字范围上限 + // total=total+iter->second; + // } + // //策略0 + // for (;iter != semmap.at(ym * 2000 + xm).end();iter++){ + // //考虑int类型表示的数字范围上限 + // float probability_color=iter->second/total; + // if (iter->first==0 && probability_color>0.9 && xm<2000 && ym<2000 && xm>=0 && ym>=0){ + // a[ym * 2000 + xm]=iter->first; + // continue; + // }else if((probability_color>0.6 && xm<2000 && ym<2000 && xm>=0 && ym>=0)||(iter->second>3 && xm<2000 && ym<2000 && xm>=0 && ym>=0)){ + // a[ym * 2000 + xm]=iter->first; + // continue; + // } + // } + + // if (sem!=0 && xm<2000 && ym<2000 && xm>=0 && ym>=0){ + // a[ym * 2000 + xm]=sem; + // // //std::cout<(cairo_image_surface_get_data(surface.get())); + for (int y = heightmap - 1; y >= 0; --y) { + for (int x = 0; x < widthmap; ++x) { + const uint32_t packed = pixel_data[y * widthmap + x]; + const unsigned char probality = packed >> 16; + const unsigned char observed = packed >> 8; + const unsigned char value=::cartographer::common::RoundToInt((1. - probality / 255.) * 100.); + if (value!=0) + std::cout<=0 && ym>=0) + a[ym * 2000 + xm]=0; + if (value>0 && value<=100 && a[ym * 2000 + xm]<100 && xm<2000 && ym<2000 && xm>=0 && ym>=0) + a[ym * 2000 + xm]=100; + } + } + }*/ + const uint32_t* q=reinterpret_cast(cairo_image_surface_get_data(surface.get())); + for(int i=0;i& submaps, + const double resolution) { + Eigen::AlignedBox2f bounding_box; + { + auto surface = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, 1, 1)); + auto cr = MakeUniqueCairoPtr(cairo_create(surface.get())); + const auto update_bounding_box = [&bounding_box, &cr](double x, double y) { + cairo_user_to_device(cr.get(), &x, &y); + bounding_box.extend(Eigen::Vector2f(x, y)); + }; + + CairoPaintSubmapSlices( + 1. / resolution, submaps, cr.get(), + [&update_bounding_box](const ::cartographer::mapping::SubmapId& submap_id,const SubmapSlice& submap_slice) { + update_bounding_box(0, 0); + update_bounding_box(submap_slice.width, 0); + update_bounding_box(0, submap_slice.height); + update_bounding_box(submap_slice.width, submap_slice.height); + }); + } + + const int kPaddingPixel = 5; + const Eigen::Array2i size( + std::ceil(bounding_box.sizes().x()) + 2 * kPaddingPixel, + std::ceil(bounding_box.sizes().y()) + 2 * kPaddingPixel); + const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel, + -bounding_box.min().y() + kPaddingPixel); + + auto surface = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, size.x(), size.y())); + int widthmap=cairo_image_surface_get_width(surface.get()); + int heightmap=cairo_image_surface_get_height(surface.get()); + { + auto cr = MakeUniqueCairoPtr(cairo_create(surface.get())); + cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.0); + cairo_paint(cr.get()); + cairo_translate(cr.get(), origin.x(), origin.y()); + CairoPaintSubmapSlices(1. / resolution, submaps, cr.get(), + [&cr,&surface](const ::cartographer::mapping::SubmapId& submap_id,const SubmapSlice& submap_slice) { + cairo_set_source_surface( + cr.get(), submap_slice.surface.get(), 0., 0.); + cairo_paint(cr.get()); + }); + // for (int i=0; i<4000000;i++) { + // a[i]=0; + // //c[i]=0; + // } + + const uint32_t* pixel_data = reinterpret_cast(cairo_image_surface_get_data(surface.get())); + for (int y = heightmap - 1; y >= 0; --y) { + for (int x = 0; x < widthmap; ++x) { + const uint32_t packed = pixel_data[y * widthmap + x]; + const unsigned char probality = packed >> 16; + const unsigned char observed = packed >> 8; + const unsigned char value=::cartographer::common::RoundToInt((1. - probality / 255.) * 100.); + //if (value!=0) + //std::cout<=0 && ym>=0){ + //a[ym * 2000 + xm]=value; + c[ym * 2000 + xm]=value; + } + if (value>65 && value<=100 && xm<2000 && ym<2000 && xm>=0 && ym>=0 ){ + //a[ym * 2000 + xm]=value; + c[ym * 2000 + xm]=value; + } + if ( value>65 && value<=100 && a[ym * 2000 + xm]>100 && xm<2000 && ym<2000 && xm>=0 && ym>=0 ){ + //a[ym * 2000 + xm]=value; + c[ym * 2000 + xm]=a[ym * 2000 + xm]; + } + + }else{ + c[ym * 2000 + xm]=-1; + } + } + } + + cairo_surface_flush(surface.get()); + } + return PaintSubmapSlicesResult(std::move(surface), origin); +} + +void FillSubmapSlice( + const ::cartographer::transform::Rigid3d& global_submap_pose, + const ::cartographer::mapping::proto::Submap& proto, + SubmapSlice* const submap_slice, + mapping::ValueConversionTables* conversion_tables) { + ::cartographer::mapping::proto::SubmapQuery::Response response; + ::cartographer::transform::Rigid3d local_pose; + if (proto.has_submap_3d()) { + mapping::Submap3D submap(proto.submap_3d()); + local_pose = submap.local_pose(); + submap.ToResponseProto(global_submap_pose, &response); + } else { + ::cartographer::mapping::Submap2D submap(proto.submap_2d(), + conversion_tables); + local_pose = submap.local_pose(); + submap.ToResponseProto(global_submap_pose, &response); + } + submap_slice->pose = global_submap_pose; + + auto& texture_proto = response.textures(0); + const SubmapTexture::Pixels pixels = UnpackTextureData( + texture_proto.cells(), texture_proto.width(), texture_proto.height()); + submap_slice->width = texture_proto.width(); + submap_slice->height = texture_proto.height(); + submap_slice->resolution = texture_proto.resolution(); + submap_slice->slice_pose = + ::cartographer::transform::ToRigid3(texture_proto.slice_pose()); + submap_slice->surface = + DrawTexture(pixels.intensity, pixels.alpha, pixels.color,texture_proto.width(), + texture_proto.height(), &submap_slice->cairo_data); +} + +void DeserializeAndFillSubmapSlices( + ProtoStreamDeserializer* deserializer, + std::map* submap_slices, + mapping::ValueConversionTables* conversion_tables) { + std::map submap_poses; + for (const auto& trajectory : deserializer->pose_graph().trajectory()) { + for (const auto& submap : trajectory.submap()) { + submap_poses[mapping::SubmapId(trajectory.trajectory_id(), + submap.submap_index())] = + transform::ToRigid3(submap.pose()); + } + } + mapping::proto::SerializedData proto; + while (deserializer->ReadNextSerializedData(&proto)) { + if (proto.has_submap() && + (Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) { + const auto& submap = proto.submap(); + const mapping::SubmapId id{submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; + FillSubmapSlice(submap_poses.at(id), submap, &(*submap_slices)[id], + conversion_tables); + } + } +} + +SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells, + const int width, const int height) { + SubmapTexture::Pixels pixels; + std::string cells; + // 将压缩后的地图栅格数据 解压成 字符串 + ::cartographer::common::FastGunzipString(compressed_cells, &cells); + + const int num_pixels = width * height; + CHECK_EQ(cells.size(), 2 * num_pixels); + pixels.intensity.reserve(num_pixels); + pixels.alpha.reserve(num_pixels); + //。。。增加color + pixels.color.reserve(num_pixels); + + // 填充数据 + //std::cout << "delta"< 0 ? delta : 0; + //const uint8 value = delta ; + //if (value>200) + //std::cout << int (value) << " "; + const uint8 alpha = delta > 0 ? 0 : -delta; + //!!!const uint8 alpha = 255; + pixels.intensity.push_back(value); + pixels.alpha.push_back(alpha); + pixels.color.push_back(cells[(i * width + j) * 2 + 1]); + } + } + return pixels; +} + +/** + * @brief 指向新创建的图像的指针 + * + * @param[in] intensity 地图栅格数据 + * @param[in] alpha 地图栅格的透明度 + * @param[in] width 地图的宽 + * @param[in] height 地图的高 + * @param[out] cairo_data 4字节的值, 左边3个字节分别存储了alpha_value intensity_value 与 observed + * @return UniqueCairoSurfacePtr 指向新创建的图像的指针 + */ +//.... +UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, + const std::vector& alpha, + //。。。增加color传参 + const std::vector& color, + const int width, const int height, + std::vector* const cairo_data) { + CHECK(cairo_data->empty()); + + // Properly dealing with a non-common stride would make this code much more + // complicated. Let's check that it is not needed. + const int expected_stride = 4 * width; + CHECK_EQ(expected_stride, cairo_format_stride_for_width(kCairoFormat, width)); + + + + // 对cairo_data进行填充 + for (size_t i = 0; i < intensity.size(); ++i) { + //std::cout<<"enter for !!!!!"<push_back((alpha_value << 24) | // 第一字节 存储透明度 + (intensity_value << 16) | // 第二字节 存储栅格值 + (observed << 8) | // 第三字节 存储是否被更新过 + //。。。color + color_value); // 第四字节 始终为0 + //std::cout<back()<<"s "; + //std::cout<(cairo_data->data()), kCairoFormat, width, + height, expected_stride) ); + + CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS) + << cairo_status_to_string(cairo_surface_status(surface.get())); + return surface; +} + +UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, + const std::vector& alpha, + const int width, const int height, + std::vector* const cairo_data) { + CHECK(cairo_data->empty()); + + // Properly dealing with a non-common stride would make this code much more + // complicated. Let's check that it is not needed. + const int expected_stride = 4 * width; + CHECK_EQ(expected_stride, cairo_format_stride_for_width(kCairoFormat, width)); + for (size_t i = 0; i < intensity.size(); ++i) { + // We use the red channel to track intensity information. The green + // channel we use to track if a cell was ever observed. + const uint8_t intensity_value = intensity.at(i); + const uint8_t alpha_value = alpha.at(i); + const uint8_t observed = + (intensity_value == 0 && alpha_value == 0) ? 0 : 255; + cairo_data->push_back((alpha_value << 24) | (intensity_value << 16) | + (observed << 8) | 0); + + } + + auto surface = MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data( + reinterpret_cast(cairo_data->data()), kCairoFormat, width, + height, expected_stride)); + CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS) + << cairo_status_to_string(cairo_surface_status(surface.get())); + return surface; +} + +} // namespace io +} // namespace cartographer + + diff --git a/carto_ws/src/cartographer-master/cartographer/io/submap_painter.h b/carto_ws/src/cartographer-master/cartographer/io/submap_painter.h new file mode 100644 index 0000000..6a01472 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/submap_painter.h @@ -0,0 +1,125 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_IO_SUBMAP_PAINTER_H_ +#define CARTOGRAPHER_IO_SUBMAP_PAINTER_H_ + +#include "Eigen/Geometry" +#include "cairo/cairo.h" +#include "cartographer/io/image.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" +#include "cartographer/transform/rigid_transform.h" +extern int c[4000000]; +extern int a[4000000]; +namespace cartographer { +namespace io { + +struct PaintSubmapSlicesResult { + PaintSubmapSlicesResult(::cartographer::io::UniqueCairoSurfacePtr surface, + Eigen::Array2f origin) + : surface(std::move(surface)), origin(origin) {} + ::cartographer::io::UniqueCairoSurfacePtr surface; + + // Top left pixel of 'surface' in map frame. + Eigen::Array2f origin; +}; + +// 地图的完整信息, 栅格数据是Cairo格式的 +struct SubmapSlice { + // 构造时使surface为指向nullptr + SubmapSlice() + : surface(::cartographer::io::MakeUniqueCairoSurfacePtr(nullptr)) {} + + // Texture data. + int width; + int height; + int version; + double resolution; + ::cartographer::transform::Rigid3d slice_pose; + ::cartographer::io::UniqueCairoSurfacePtr surface; // surface是图形库cairo的image画布 + // Pixel data used by 'surface'. Must outlive 'surface'. + std::vector cairo_data; + + // Metadata. + ::cartographer::transform::Rigid3d pose; + int metadata_version = -1; +}; + +//。。。解压要改 增加color 解压后的地图栅格数据 +struct SubmapTexture { + struct Pixels { + std::vector intensity; // 地图栅格值 + std::vector alpha; // 栅格的透明度 + std::vector color; //。。。增加颜色值 + }; + Pixels pixels; + int width; + int height; + double resolution; + ::cartographer::transform::Rigid3d slice_pose; +}; + +// 压缩后的地图栅格数据 +struct SubmapTextures { + int version; + std::vector textures; +}; + +PaintSubmapSlicesResult PaintSubmapSlices( + const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + double resolution); +int* Slices( + const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + const double resolution); +void FillSubmapSlice( + const ::cartographer::transform::Rigid3d& global_submap_pose, + const ::cartographer::mapping::proto::Submap& proto, + SubmapSlice* const submap_slice, + mapping::ValueConversionTables* conversion_tables); + +void DeserializeAndFillSubmapSlices( + ProtoStreamDeserializer* deserializer, + std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submap_slices, + mapping::ValueConversionTables* conversion_tables); + +// Unpacks cell data as provided by the backend into 'intensity' and 'alpha'. +SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells, + int width, int height); + +// Draw a texture into a cairo surface. 'cairo_data' will store the pixel data +// for the surface and must therefore outlive the use of the surface. +UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, + const std::vector& alpha, + //。。。增加color传参 + const std::vector& color, + int width, + int height, + std::vector* cairo_data); +UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, + const std::vector& alpha, + + int width, + int height, + std::vector* cairo_data); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_SUBMAP_PAINTER_H_ + diff --git a/carto_ws/src/cartographer-master/cartographer/io/vertical_range_filtering_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/vertical_range_filtering_points_processor.cc new file mode 100644 index 0000000..325d359 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/vertical_range_filtering_points_processor.cc @@ -0,0 +1,59 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/vertical_range_filtering_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +VerticalRangeFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("min_z"), dictionary->GetDouble("max_z"), + next); +} + +VerticalRangeFilteringPointsProcessor::VerticalRangeFilteringPointsProcessor( + const double min_z, const double max_z, + PointsProcessor* next) + : min_z_(min_z), max_z_(max_z), + next_(next) {} + +void VerticalRangeFilteringPointsProcessor::Process( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const float distance_z = batch->points[i].position.z() - batch->origin.z(); + if (!(min_z_ <= distance_z && distance_z <= max_z_) ) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult VerticalRangeFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/vertical_range_filtering_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/vertical_range_filtering_points_processor.h new file mode 100644 index 0000000..d23eb20 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/vertical_range_filtering_points_processor.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Filters all points which distance in the Z direction from their 'origin' +// exceeds 'max_z' or 'min_z'. +class VerticalRangeFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "vertical_range_filter"; + VerticalRangeFilteringPointsProcessor(double min_z, double max_z, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~VerticalRangeFilteringPointsProcessor() override {} + + VerticalRangeFilteringPointsProcessor( + const VerticalRangeFilteringPointsProcessor&) = delete; + VerticalRangeFilteringPointsProcessor& operator=( + const VerticalRangeFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double min_z_; + const double max_z_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/xray_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/xray_points_processor.cc new file mode 100644 index 0000000..282bdda --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/xray_points_processor.cc @@ -0,0 +1,282 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/io/xray_points_processor.h" + +#include +#include +#include + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "absl/strings/str_cat.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/io/draw_trajectories.h" +#include "cartographer/io/image.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/detect_floors.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace io { +namespace { + +struct PixelData { + size_t num_occupied_cells_in_column = 0; + float mean_r = 0.; + float mean_g = 0.; + float mean_b = 0.; +}; + +class PixelDataMatrix { + public: + PixelDataMatrix(const int width, const int height) + : width_(width), data_(width * height) {} + + int width() const { return width_; } + int height() const { return data_.size() / width_; } + const PixelData& operator()(const int x, const int y) const { + return data_.at(x + y * width_); + } + + PixelData& operator()(const int x, const int y) { + return data_.at(x + y * width_); + } + + private: + int width_; + std::vector data_; +}; + +float Mix(const float a, const float b, const float t) { + return a * (1. - t) + t * b; +} + +// Convert 'matrix' into a pleasing-to-look-at image. +Image IntoImage(const PixelDataMatrix& matrix, double saturation_factor) { + Image image(matrix.width(), matrix.height()); + float max = std::numeric_limits::min(); + for (int y = 0; y < matrix.height(); ++y) { + for (int x = 0; x < matrix.width(); ++x) { + const PixelData& cell = matrix(x, y); + if (cell.num_occupied_cells_in_column == 0.) { + continue; + } + max = std::max(max, std::log(cell.num_occupied_cells_in_column)); + } + } + + for (int y = 0; y < matrix.height(); ++y) { + for (int x = 0; x < matrix.width(); ++x) { + const PixelData& cell = matrix(x, y); + if (cell.num_occupied_cells_in_column == 0.) { + image.SetPixel(x, y, {{255, 255, 255}}); + continue; + } + + // We use a logarithmic weighting for how saturated a pixel will be. The + // basic idea here was that walls (full height) are fully saturated, but + // details like chairs and tables are still well visible. + const float saturation = + std::min(1.0, std::log(cell.num_occupied_cells_in_column) / + max * saturation_factor); + const FloatColor color = {{Mix(1.f, cell.mean_r, saturation), + Mix(1.f, cell.mean_g, saturation), + Mix(1.f, cell.mean_b, saturation)}}; + image.SetPixel(x, y, ToUint8Color(color)); + } + } + return image; +} + +bool ContainedIn(const common::Time& time, + const std::vector& timespans) { + for (const mapping::Timespan& timespan : timespans) { + if (timespan.start <= time && time <= timespan.end) { + return true; + } + } + return false; +} + +} // namespace + +XRayPointsProcessor::XRayPointsProcessor( + const double voxel_size, const double saturation_factor, + const transform::Rigid3f& transform, + const std::vector& floors, + const DrawTrajectories& draw_trajectories, + const std::string& output_filename, + const std::vector& trajectories, + FileWriterFactory file_writer_factory, PointsProcessor* const next) + : draw_trajectories_(draw_trajectories), + trajectories_(trajectories), + file_writer_factory_(file_writer_factory), + next_(next), + floors_(floors), + output_filename_(output_filename), + transform_(transform), + saturation_factor_(saturation_factor) { + for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { + aggregations_.emplace_back( + Aggregation{mapping::HybridGridBase(voxel_size), {}}); + } +} + +std::unique_ptr XRayPointsProcessor::FromDictionary( + const std::vector& trajectories, + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + std::vector floors; + const bool separate_floor = dictionary->HasKey("separate_floors") && + dictionary->GetBool("separate_floors"); + const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") || + dictionary->GetBool("draw_trajectories")) + ? DrawTrajectories::kYes + : DrawTrajectories::kNo; + const double saturation_factor = + dictionary->HasKey("saturation_factor") + ? dictionary->GetDouble("saturation_factor") + : 1.; + if (separate_floor) { + CHECK_EQ(trajectories.size(), 1) + << "Can only detect floors with a single trajectory."; + floors = mapping::DetectFloors(trajectories.at(0)); + } + + return absl::make_unique( + dictionary->GetDouble("voxel_size"), saturation_factor, + transform::FromDictionary(dictionary->GetDictionary("transform").get()) + .cast(), + floors, draw_trajectories, dictionary->GetString("filename"), + trajectories, file_writer_factory, next); +} + +void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, + FileWriter* const file_writer) { + if (bounding_box_.isEmpty()) { + LOG(WARNING) << "Not writing output: bounding box is empty."; + return; + } + + // Returns the (x, y) pixel of the given 'index'. + const auto voxel_index_to_pixel = + [this](const Eigen::Array3i& index) -> Eigen::Array2i { + // We flip the y axis, since matrices rows are counted from the top. + return Eigen::Array2i(bounding_box_.max()[1] - index[1], + bounding_box_.max()[2] - index[2]); + }; + + // Hybrid grid uses X: forward, Y: left, Z: up. + // For the screen we are using. X: right, Y: up + const int xsize = bounding_box_.sizes()[1] + 1; + const int ysize = bounding_box_.sizes()[2] + 1; + PixelDataMatrix pixel_data_matrix(xsize, ysize); + for (mapping::HybridGridBase::Iterator it(aggregation.voxels); + !it.Done(); it.Next()) { + const Eigen::Array3i cell_index = it.GetCellIndex(); + const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index); + PixelData& pixel_data = pixel_data_matrix(pixel.x(), pixel.y()); + const auto& column_data = aggregation.column_data.at( + std::make_pair(cell_index[1], cell_index[2])); + pixel_data.mean_r = column_data.sum_r / column_data.count; + pixel_data.mean_g = column_data.sum_g / column_data.count; + pixel_data.mean_b = column_data.sum_b / column_data.count; + ++pixel_data.num_occupied_cells_in_column; + } + + Image image = IntoImage(pixel_data_matrix, saturation_factor_); + if (draw_trajectories_ == DrawTrajectories::kYes) { + for (size_t i = 0; i < trajectories_.size(); ++i) { + DrawTrajectory( + trajectories_[i], GetColor(i), + [&voxel_index_to_pixel, &aggregation, + this](const transform::Rigid3d& pose) -> Eigen::Array2i { + return voxel_index_to_pixel(aggregation.voxels.GetCellIndex( + (transform_ * pose.cast()).translation())); + }, + image.GetCairoSurface().get()); + } + } + + image.WritePng(file_writer); + CHECK(file_writer->Close()); +} + +void XRayPointsProcessor::Insert(const PointsBatch& batch, + Aggregation* const aggregation) { + constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}}; + for (size_t i = 0; i < batch.points.size(); ++i) { + const sensor::RangefinderPoint camera_point = transform_ * batch.points[i]; + const Eigen::Array3i cell_index = + aggregation->voxels.GetCellIndex(camera_point.position); + *aggregation->voxels.mutable_value(cell_index) = true; + bounding_box_.extend(cell_index.matrix()); + ColumnData& column_data = + aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])]; + const auto& color = + batch.colors.empty() ? kDefaultColor : batch.colors.at(i); + column_data.sum_r += color[0]; + column_data.sum_g += color[1]; + column_data.sum_b += color[2]; + ++column_data.count; + } +} + +void XRayPointsProcessor::Process(std::unique_ptr batch) { + if (floors_.empty()) { + CHECK_EQ(aggregations_.size(), 1); + Insert(*batch, &aggregations_[0]); + } else { + for (size_t i = 0; i < floors_.size(); ++i) { + if (!ContainedIn(batch->start_time, floors_[i].timespans)) { + continue; + } + Insert(*batch, &aggregations_[i]); + } + } + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult XRayPointsProcessor::Flush() { + if (floors_.empty()) { + CHECK_EQ(aggregations_.size(), 1); + WriteVoxels(aggregations_[0], + file_writer_factory_(output_filename_ + ".png").get()); + } else { + for (size_t i = 0; i < floors_.size(); ++i) { + WriteVoxels( + aggregations_[i], + file_writer_factory_(absl::StrCat(output_filename_, i, ".png")) + .get()); + } + } + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "X-Ray generation must be configured to occur after any " + "stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/xray_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/xray_points_processor.h new file mode 100644 index 0000000..8d0f758 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/xray_points_processor.h @@ -0,0 +1,103 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/detect_floors.h" +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace io { + +// Creates X-ray cuts through the points with pixels being 'voxel_size' big. +class XRayPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "write_xray_image"; + enum class DrawTrajectories { kNo, kYes }; + XRayPointsProcessor( + double voxel_size, double saturation_factor, + const transform::Rigid3f& transform, + const std::vector& floors, + const DrawTrajectories& draw_trajectories, + const std::string& output_filename, + const std::vector& trajectories, + FileWriterFactory file_writer_factory, PointsProcessor* next); + + static std::unique_ptr FromDictionary( + const std::vector& trajectories, + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~XRayPointsProcessor() override {} + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + Eigen::AlignedBox3i bounding_box() const { return bounding_box_; } + + private: + struct ColumnData { + float sum_r = 0.; + float sum_g = 0.; + float sum_b = 0.; + uint32_t count = 0; + }; + + struct Aggregation { + mapping::HybridGridBase voxels; + std::map, ColumnData> column_data; + }; + + void WriteVoxels(const Aggregation& aggregation, + FileWriter* const file_writer); + void Insert(const PointsBatch& batch, Aggregation* aggregation); + + const DrawTrajectories draw_trajectories_; + const std::vector trajectories_; + FileWriterFactory file_writer_factory_; + PointsProcessor* const next_; + + // If empty, we do not separate into floors. + std::vector floors_; + + const std::string output_filename_; + const transform::Rigid3f transform_; + + // Only has one entry if we do not separate into floors. + std::vector aggregations_; + + // Bounding box containing all cells with data in all 'aggregations_'. + Eigen::AlignedBox3i bounding_box_; + + // Scale the saturation of the point color. If saturation_factor_ > 1, the + // point has darker color, otherwise it has lighter color. + const double saturation_factor_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/io/xyz_writing_points_processor.cc b/carto_ws/src/cartographer-master/cartographer/io/xyz_writing_points_processor.cc new file mode 100644 index 0000000..befea10 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/xyz_writing_points_processor.cc @@ -0,0 +1,58 @@ +#include "cartographer/io/xyz_writing_points_processor.h" + +#include + +#include "absl/memory/memory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +namespace { + +void WriteXyzPoint(const Eigen::Vector3f& point, + FileWriter* const file_writer) { + std::ostringstream stream; + stream << std::setprecision(6); + stream << point.x() << " " << point.y() << " " << point.z() << "\n"; + const std::string out = stream.str(); + CHECK(file_writer->Write(out.data(), out.size())); +} + +} // namespace + +XyzWriterPointsProcessor::XyzWriterPointsProcessor( + std::unique_ptr file_writer, PointsProcessor* const next) + : next_(next), file_writer_(std::move(file_writer)) {} + +std::unique_ptr +XyzWriterPointsProcessor::FromDictionary( + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + file_writer_factory(dictionary->GetString("filename")), next); +} + +PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() { + CHECK(file_writer_->Close()) << "Closing XYZ file failed."; + switch (next_->Flush()) { + case FlushResult::kFinished: + return FlushResult::kFinished; + + case FlushResult::kRestartStream: + LOG(FATAL) << "XYZ generation must be configured to occur after any " + "stages that require multiple passes."; + } + LOG(FATAL); +} + +void XyzWriterPointsProcessor::Process(std::unique_ptr batch) { + for (const sensor::RangefinderPoint& point : batch->points) { + WriteXyzPoint(point.position, file_writer_.get()); + } + next_->Process(std::move(batch)); +} + +} // namespace io +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/io/xyz_writing_points_processor.h b/carto_ws/src/cartographer-master/cartographer/io/xyz_writing_points_processor.h new file mode 100644 index 0000000..c0e0c64 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/io/xyz_writing_points_processor.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ + +#include +#include +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Writes ASCII xyz points. +class XyzWriterPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "write_xyz"; + + XyzWriterPointsProcessor(std::unique_ptr, PointsProcessor* next); + + static std::unique_ptr FromDictionary( + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~XyzWriterPointsProcessor() override {} + + XyzWriterPointsProcessor(const XyzWriterPointsProcessor&) = delete; + XyzWriterPointsProcessor& operator=(const XyzWriterPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + std::unique_ptr file_writer_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/grid_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/grid_2d.cc new file mode 100644 index 0000000..eea27b6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/grid_2d.cc @@ -0,0 +1,192 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "cartographer/mapping/2d/grid_2d.h" + +namespace cartographer { +namespace mapping { +namespace { + +float MinCorrespondenceCostFromProto(const proto::Grid2D& proto) { + if (proto.min_correspondence_cost() == 0.f && + proto.max_correspondence_cost() == 0.f) { + LOG(WARNING) << "proto::Grid2D: min_correspondence_cost " + "is initialized with 0 indicating an older version of the " + "protobuf format. Loading default values."; + return kMinCorrespondenceCost; + } else { + return proto.min_correspondence_cost(); + } +} + +float MaxCorrespondenceCostFromProto(const proto::Grid2D& proto) { + if (proto.min_correspondence_cost() == 0.f && + proto.max_correspondence_cost() == 0.f) { + LOG(WARNING) << "proto::Grid2D: max_correspondence_cost " + "is initialized with 0 indicating an older version of the " + "protobuf format. Loading default values."; + return kMaxCorrespondenceCost; + } else { + return proto.max_correspondence_cost(); + } +} +} // namespace + +proto::GridOptions2D CreateGridOptions2D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::GridOptions2D options; + const std::string grid_type_string = + parameter_dictionary->GetString("grid_type"); + proto::GridOptions2D_GridType grid_type; + CHECK(proto::GridOptions2D_GridType_Parse(grid_type_string, &grid_type)) + << "Unknown GridOptions2D_GridType kind: " << grid_type_string; + options.set_grid_type(grid_type); + options.set_resolution(parameter_dictionary->GetDouble("resolution")); + return options; +} + +Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, + float max_correspondence_cost, + ValueConversionTables* conversion_tables) + : limits_(limits), + correspondence_cost_cells_( + limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, + kUnknownCorrespondenceValue), + //intensities(10000000), + intensities(limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells), + min_correspondence_cost_(min_correspondence_cost), + max_correspondence_cost_(max_correspondence_cost), + value_to_correspondence_cost_table_(conversion_tables->GetConversionTable( + max_correspondence_cost, min_correspondence_cost, + max_correspondence_cost)) { + //intensities.resize(limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells); + CHECK_LT(min_correspondence_cost_, max_correspondence_cost_); +} + +Grid2D::Grid2D(const proto::Grid2D& proto, + ValueConversionTables* conversion_tables) + : limits_(proto.limits()), + correspondence_cost_cells_(), + //intensities(10000000), + intensities(limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells), + min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)), + max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)), + value_to_correspondence_cost_table_(conversion_tables->GetConversionTable( + max_correspondence_cost_, min_correspondence_cost_, + max_correspondence_cost_)) { + //intensities.resize(limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells); + CHECK_LT(min_correspondence_cost_, max_correspondence_cost_); + if (proto.has_known_cells_box()) { + const auto& box = proto.known_cells_box(); + known_cells_box_ = + Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()), + Eigen::Vector2i(box.max_x(), box.max_y())); + } + correspondence_cost_cells_.reserve(proto.cells_size()); + for (const auto& cell : proto.cells()) { + CHECK_LE(cell, std::numeric_limits::max()); + correspondence_cost_cells_.push_back(cell); + } +} + +// Finishes the update sequence. +void Grid2D::FinishUpdate() { + while (!update_indices_.empty()) { + DCHECK_GE(correspondence_cost_cells_[update_indices_.back()], + kUpdateMarker); + correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker; + update_indices_.pop_back(); + } +} + +// Fills in 'offset' and 'limits' to define a subregion of that contains all +// known cells. +void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, + CellLimits* const limits) const { + if (known_cells_box_.isEmpty()) { + *offset = Eigen::Array2i::Zero(); + *limits = CellLimits(1, 1); + return; + } + *offset = known_cells_box_.min().array(); + *limits = CellLimits(known_cells_box_.sizes().x() + 1, + known_cells_box_.sizes().y() + 1); +} + +// Grows the map as necessary to include 'point'. This changes the meaning of +// these coordinates going forward. This method must be called immediately +// after 'FinishUpdate', before any calls to 'ApplyLookupTable'. +void Grid2D::GrowLimits(const Eigen::Vector2f& point) { + GrowLimits(point, {mutable_correspondence_cost_cells()}, + {kUnknownCorrespondenceValue}); +} + +void Grid2D::GrowLimits(const Eigen::Vector2f& point, + const std::vector*>& grids, + const std::vector& grids_unknown_cell_values) { + CHECK(update_indices_.empty()); + while (!limits_.Contains(limits_.GetCellIndex(point))) { + 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_.max() + + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), + 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; + + for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) { + std::vector new_cells(new_size, + grids_unknown_cell_values[grid_index]); + 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] = + (*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells]; + } + } + *grids[grid_index] = new_cells; + } + limits_ = new_limits; + if (!known_cells_box_.isEmpty()) { + known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); + } + } +} + +proto::Grid2D Grid2D::ToProto() const { + proto::Grid2D result; + *result.mutable_limits() = mapping::ToProto(limits_); + *result.mutable_cells() = {correspondence_cost_cells_.begin(), + correspondence_cost_cells_.end()}; + CHECK(update_indices().empty()) << "Serializing a grid during an update is " + "not supported. Finish the update first."; + if (!known_cells_box().isEmpty()) { + auto* const box = result.mutable_known_cells_box(); + box->set_max_x(known_cells_box().max().x()); + box->set_max_y(known_cells_box().max().y()); + box->set_min_x(known_cells_box().min().x()); + box->set_min_y(known_cells_box().min().y()); + } + result.set_min_correspondence_cost(min_correspondence_cost_); + result.set_max_correspondence_cost(max_correspondence_cost_); + return result; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/grid_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/2d/grid_2d.h new file mode 100644 index 0000000..f84d471 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/grid_2d.h @@ -0,0 +1,160 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_GRID_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_GRID_2D_H_ + +#include + +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/grid_interface.h" +#include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/proto/grid_2d.pb.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" + +namespace cartographer { +namespace mapping { + +proto::GridOptions2D CreateGridOptions2D( + common::LuaParameterDictionary* const parameter_dictionary); + +enum class GridType { PROBABILITY_GRID, TSDF }; + +class Grid2D : public GridInterface { + public: + Grid2D(const MapLimits& limits, float min_correspondence_cost, + float max_correspondence_cost, + ValueConversionTables* conversion_tables); + explicit Grid2D(const proto::Grid2D& proto, + ValueConversionTables* conversion_tables); + + // Returns the limits of this Grid2D. + const MapLimits& limits() const { return limits_; } + + // Finishes the update sequence. + void FinishUpdate(); + + // Returns the correspondence cost of the cell with 'cell_index'. + float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { + if (!limits().Contains(cell_index)) return max_correspondence_cost_; + return (*value_to_correspondence_cost_table_) + [correspondence_cost_cells()[ToFlatIndex(cell_index)]]; + } + + virtual GridType GetGridType() const = 0; + + // Returns the minimum possible correspondence cost. + float GetMinCorrespondenceCost() const { return min_correspondence_cost_; } + + // Returns the maximum possible correspondence cost. + float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; } + + // Returns true if the probability at the specified index is known. + // 指定的栅格是否被更新过 + bool IsKnown(const Eigen::Array2i& cell_index) const { + return limits_.Contains(cell_index) && + correspondence_cost_cells_[ToFlatIndex(cell_index)] != + kUnknownCorrespondenceValue; + } + + // 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; + + // Grows the map as necessary to include 'point'. This changes the meaning of + // these coordinates going forward. This method must be called immediately + // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. + virtual void GrowLimits(const Eigen::Vector2f& point); + + virtual std::unique_ptr ComputeCroppedGrid() const = 0; + + virtual proto::Grid2D ToProto() const; + + virtual bool DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const = 0; + + protected: + void GrowLimits(const Eigen::Vector2f& point, + const std::vector*>& grids, + const std::vector& grids_unknown_cell_values); + + // 返回不可以修改的栅格地图数组的引用 + const std::vector& correspondence_cost_cells() const { + //。。。return correspondence_cost_cells_; + return correspondence_cost_cells_; + } +// 。。。返回不可以修改的栅格地图强度数组的引用 + const std::vector>& correspondence_cost_cells2() const { + //。。。return correspondence_cost_cells_; + return intensities; + } + const std::vector& update_indices() const { return update_indices_;} + const Eigen::AlignedBox2i& known_cells_box() const { + return known_cells_box_; + } + + // 返回可以修改的栅格地图数组的指针 + //。。。结构更改后返回改结构第一维的地址 + std::vector* mutable_correspondence_cost_cells() { + //return &correspondence_cost_cells_; + return &(correspondence_cost_cells_); + } + //。。。结构更改后返回改结构第二维的地址 + std::vector>* mutable_correspondence_cost_cells2() { + //return &correspondence_cost_cells_; + return &(intensities); + } + + std::vector* mutable_update_indices() { return &update_indices_; } + Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } + + // Converts a 'cell_index' into an index into 'cells_'. + // 二维像素坐标转为一维索引坐标 + int ToFlatIndex(const Eigen::Array2i& cell_index) const { + CHECK(limits_.Contains(cell_index)) << cell_index; + return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x(); + } + + private: + MapLimits limits_; // 地图大小边界, 包括x和y最大值, 分辨率, x和y方向栅格数 + + // 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知 + //。。。新写一个类代替uint16 + //。。。std::vector correspondence_cost_cells_; + + std::vector correspondence_cost_cells_; + std::vector> intensities; + + float min_correspondence_cost_; + float max_correspondence_cost_; + std::vector update_indices_; // 记录已经更新过的索引 + + // Bounding box of known cells to efficiently compute cropping limits. + Eigen::AlignedBox2i known_cells_box_; // 栅格的bounding box, 存的是像素坐标 + // 将[0, 1~32767] 映射到 [0.9, 0.1~0.9] 的转换表 + const std::vector* value_to_correspondence_cost_table_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_GRID_2D_H_ + + diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/map_limits.h b/carto_ws/src/cartographer-master/cartographer/mapping/2d/map_limits.h new file mode 100644 index 0000000..0ad1f3c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/map_limits.h @@ -0,0 +1,109 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ +#define CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ + +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/proto/map_limits.pb.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +// Defines the limits of a grid map. This class must remain inlined for +// performance reasons. +class MapLimits { + public: + MapLimits(const double resolution, const Eigen::Vector2d& max, + const CellLimits& cell_limits) + : resolution_(resolution), max_(max), cell_limits_(cell_limits) { + CHECK_GT(resolution_, 0.); + CHECK_GT(cell_limits.num_x_cells, 0.); + CHECK_GT(cell_limits.num_y_cells, 0.); + } + + explicit MapLimits(const proto::MapLimits& map_limits) + : resolution_(map_limits.resolution()), + max_(transform::ToEigen(map_limits.max())), + cell_limits_(map_limits.cell_limits()) {} + + // 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 corner of the limits, i.e., all pixels have positions with + // smaller coordinates. + const Eigen::Vector2d& max() const { return max_; } + + // 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' which may be outside + // the map, i.e., negative or too large indices that will return false for + // Contains(). + Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const { + // Index values are row major and the top left has Eigen::Array2i::Zero() + // and contains (centered_max_x, centered_max_y). We need to flip and + // rotate. + return Eigen::Array2i( + common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5), + common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5)); + } + + // Returns the center of the cell at 'cell_index'. + Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const { + return {max_.x() - resolution() * (cell_index[1] + 0.5), + max_.y() - resolution() * (cell_index[0] + 0.5)}; + } + + // Returns true if the ProbabilityGrid contains 'cell_index'. + bool Contains(const Eigen::Array2i& cell_index) const { + return (Eigen::Array2i(0, 0) <= cell_index).all() && + (cell_index < + Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells)) + .all(); + } + + private: + double resolution_; + Eigen::Vector2d max_; + CellLimits cell_limits_; +}; + +inline proto::MapLimits ToProto(const MapLimits& map_limits) { + proto::MapLimits result; + result.set_resolution(map_limits.resolution()); + *result.mutable_max() = transform::ToProto(map_limits.max()); + *result.mutable_cell_limits() = ToProto(map_limits.cell_limits()); + return result; +} + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/map_limits_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/map_limits_test.cc new file mode 100644 index 0000000..ee54164 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/map_limits_test.cc @@ -0,0 +1,67 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/2d/map_limits.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(MapLimitsTest, ToProto) { + const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); + const auto proto = ToProto(limits); + EXPECT_EQ(limits.resolution(), proto.resolution()); + EXPECT_EQ(limits.max().x(), proto.max().x()); + EXPECT_EQ(limits.max().y(), proto.max().y()); + EXPECT_EQ(ToProto(limits.cell_limits()).DebugString(), + proto.cell_limits().DebugString()); +} + +TEST(MapLimitsTest, ProtoConstructor) { + proto::MapLimits limits; + limits.set_resolution(1.); + limits.mutable_max()->set_x(2.); + limits.mutable_max()->set_y(3.); + limits.mutable_cell_limits()->set_num_x_cells(4); + limits.mutable_cell_limits()->set_num_y_cells(5); + + const MapLimits native(limits); + EXPECT_EQ(limits.resolution(), native.resolution()); + EXPECT_EQ(limits.max().x(), native.max().x()); + EXPECT_EQ(limits.max().y(), native.max().y()); + EXPECT_EQ(limits.cell_limits().DebugString(), + ToProto(native.cell_limits()).DebugString()); +} + +TEST(MapLimitsTest, ConstructAndGet) { + const MapLimits limits(42., Eigen::Vector2d(3., 0.), 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::Vector2d& max = limits.max(); + EXPECT_EQ(3., max.x()); + EXPECT_EQ(0., max.y()); + + EXPECT_EQ(42., limits.resolution()); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid.cc new file mode 100644 index 0000000..2bb42cb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid.cc @@ -0,0 +1,266 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "cartographer/mapping/2d/probability_grid.h" + +#include +#include +#include "absl/memory/memory.h" +#include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/submaps.h" + +namespace cartographer { +namespace mapping { + +/** + * @brief ProbabilityGrid的构造函数 + * + * @param[in] limits 地图坐标信息 + * @param[in] conversion_tables 转换表 + */ +ProbabilityGrid::ProbabilityGrid(const MapLimits& limits, + ValueConversionTables* conversion_tables) + : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost, + conversion_tables), + conversion_tables_(conversion_tables) { +} + +ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, + ValueConversionTables* conversion_tables) + : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { + CHECK(proto.has_probability_grid_2d()); + +} + +// Sets the probability of the cell at 'cell_index' to the given +// 'probability'. Only allowed if the cell was unknown before. +// 将 索引 处单元格的概率设置为给定的概率, 仅当单元格之前处于未知状态时才允许 +void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, + const float probability) { + // 获取对应栅格的引用 + uint16& cell = + (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)]; + CHECK_EQ(cell, kUnknownProbabilityValue); + // 为栅格赋值 value + cell = + CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability)); + // 更新bounding_box + mutable_known_cells_box()->extend(cell_index.matrix()); +} + +// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() +// to the probability of the cell at 'cell_index' if the cell has not already +// been updated. Multiple updates of the same cell will be ignored until +// FinishUpdate() is called. Returns true if the cell was updated. +// 如果单元格尚未更新,则将调用 ComputeLookupTableToApplyOdds() 时指定的 'odds' 应用于单元格在 'cell_index' 处的概率 +// 在调用 FinishUpdate() 之前，将忽略同一单元格的多次更新。如果单元格已更新，则返回 true +// +// If this is the first call to ApplyOdds() for the specified cell, its value +// will be set to probability corresponding to 'odds'. +// 如果这是对指定单元格第一次调用 ApplyOdds(),则其值将设置为与 'odds' 对应的概率 + +// 使用查找表对指定栅格进行栅格值的更新 + +//。。新增函数 +bool ProbabilityGrid::ApplyLookupTableHit(const Eigen::Array2i& cell_index, + const std::vector& table, + const uint8 intensities) { + DCHECK_EQ(table.size(), kUpdateMarker); + const int flat_index = ToFlatIndex(cell_index); + // 获取对应栅格的指针 + //。。。更改&(*mutable_correspondence_cost_cells())的结构（第一维指向概率 第二维指向颜色） cell指向该结构的第一维 + uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index]; + //。。。增加指针cell2指向该结构的第二维 一个color的数组 + //float* cell2 = &(*mutable_correspondence_cost_cells2())[flat_index]; + // 对处于更新状态的栅格, 不再进行更新了 + if (*cell >= kUpdateMarker) { + return false; + } + // 标记这个索引的栅格已经被更新过 + mutable_update_indices()->push_back(flat_index); + // 更新栅格值 + //。。。cell2_>append(range.hit.color) + + if(((*mutable_correspondence_cost_cells2()).size())extend(cell_index.matrix()); + return true; +} + +bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, + const std::vector& table) { + DCHECK_EQ(table.size(), kUpdateMarker); + const int flat_index = ToFlatIndex(cell_index); + // 获取对应栅格的指针 + uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index]; + // 对处于更新状态的栅格, 不再进行更新了 + if (*cell >= kUpdateMarker) { + return false; + } + // 标记这个索引的栅格已经被更新过 + mutable_update_indices()->push_back(flat_index); + // 更新栅格值 + //。。。cell2_>append(range.hit.color) + //*cell2 = table[*cell]; + *cell = table[*cell]; + DCHECK_GE(*cell, kUpdateMarker); + // 更新bounding_box + mutable_known_cells_box()->extend(cell_index.matrix()); + return true; +} + +GridType ProbabilityGrid::GetGridType() const { + return GridType::PROBABILITY_GRID; +} + +// Returns the probability of the cell with 'cell_index'. +// 获取 索引 处单元格的占用概率 +float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { + if (!limits().Contains(cell_index)) return kMinProbability; + return CorrespondenceCostToProbability(ValueToCorrespondenceCost( + correspondence_cost_cells()[ToFlatIndex(cell_index)])); +} +//int ColorGrid::GetColor(const Eigen::Array2i& cell_index) const 取出改结构第二维的众数 记得声明 +//。。。新增函数getcolor +float ProbabilityGrid::GetColor(const Eigen::Array2i& cell_index) const { + if (!limits().Contains(cell_index)) return 0; //?？？ + if ((correspondence_cost_cells2()[ToFlatIndex(cell_index)]).empty()) + return 0; + else + return majorityElement((correspondence_cost_cells2()[ToFlatIndex(cell_index)])); +} +//。。。新增函数 + float ProbabilityGrid::majorityElement(std::vector nums) const{ + + int size = nums.size(); + std::map counter{}; + for (auto x : nums) + { + if (counter.find(x) == counter.end()) + { + counter[x] = 1; + } + else + { + counter[x] += 1; + } + if (counter[x] > size/2) + return x; + } + return 0; +} + +proto::Grid2D ProbabilityGrid::ToProto() const { + proto::Grid2D result; + result = Grid2D::ToProto(); + result.mutable_probability_grid_2d(); + return result; +} + +// 根据bounding_box对栅格地图进行裁剪到正好包含点云 +std::unique_ptr ProbabilityGrid::ComputeCroppedGrid() const { + Eigen::Array2i offset; + CellLimits cell_limits; + // 根据bounding_box对栅格地图进行裁剪 + ComputeCroppedLimits(&offset, &cell_limits); + const double resolution = limits().resolution(); + // 重新计算最大值坐标 + const Eigen::Vector2d max = + limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); + // 重新定义概率栅格地图的大小 + std::unique_ptr cropped_grid = + absl::make_unique( + MapLimits(resolution, max, cell_limits), conversion_tables_); + // 给新栅格地图赋值 + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) continue; + cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset)); + } + + // 返回新地图的指针 + return std::unique_ptr(cropped_grid.release()); +} + +// 获取压缩后的地图栅格数据 +bool ProbabilityGrid::DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const { + Eigen::Array2i offset; + CellLimits cell_limits; + // 根据bounding_box对栅格地图进行裁剪 + ComputeCroppedLimits(&offset, &cell_limits); + + std::string cells; + //std::cout << "delta"< 0 ? 0 : -delta; + const uint8 value = delta > 0 ? delta : 0; + // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度 + //。。。将颜色值存入cell + cells.push_back(delta); + + //std::cout << int (delta) << ""; + //cells.push_back((value || alpha) ? alpha : 1); + //。。。cells.pushback(color) + cells.push_back(color); + } + + // 保存地图栅格数据时进行压缩 + common::FastGzipString(cells, texture->mutable_cells()); + + // 填充地图描述信息 + texture->set_width(cell_limits.num_x_cells); + texture->set_height(cell_limits.num_y_cells); + const double resolution = limits().resolution(); + texture->set_resolution(resolution); + const double max_x = limits().max().x() - resolution * offset.y(); + const double max_y = limits().max().y() - resolution * offset.x(); + *texture->mutable_slice_pose() = transform::ToProto( + local_pose.inverse() * + transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); + + return true; +} + +} // namespace mapping +} // namespace cartographer + diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid.h b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid.h new file mode 100644 index 0000000..c4a07c4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid.h @@ -0,0 +1,80 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ +#define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ + +#include +#include +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/2d/xy_index.h" + +namespace cartographer { +namespace mapping { + +// Represents a 2D grid of probabilities. +class ProbabilityGrid : public Grid2D { + public: + explicit ProbabilityGrid(const MapLimits& limits, + ValueConversionTables* conversion_tables); + explicit ProbabilityGrid(const proto::Grid2D& proto, + ValueConversionTables* conversion_tables); + + // Sets the probability of the cell at 'cell_index' to the given + // 'probability'. Only allowed if the cell was unknown before. + void SetProbability(const Eigen::Array2i& cell_index, + const float probability); + + // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() + // to the probability of the cell at 'cell_index' if the cell has not already + // been updated. Multiple updates of the same cell will be ignored until + // FinishUpdate() 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& cell_index, + const std::vector& table); + //.......xinzeng + bool ApplyLookupTableHit(const Eigen::Array2i& cell_index, + const std::vector& table, + const uint8 intensities); + + GridType GetGridType() const override; + + // Returns the probability of the cell with 'cell_index'. + float GetProbability(const Eigen::Array2i& cell_index) const; + //。。。get color + float GetColor(const Eigen::Array2i& cell_index)const; + //求众数 + float majorityElement(std::vector nums) const; + proto::Grid2D ToProto() const override; + std::unique_ptr ComputeCroppedGrid() const override; + bool DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const override; + + private: + ValueConversionTables* conversion_tables_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ + diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc new file mode 100644 index 0000000..a640702 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc @@ -0,0 +1,324 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_range_data_inserter_2d.h" +#include +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" +extern int cnew[4000000]; +// static int x_last=0; +// static int y_last=0; +// static int xm_last=0; +// static int ym_last=0; + +namespace cartographer { +namespace mapping { +namespace { + +// Factor for subpixel accuracy of start and end point for ray casts. +constexpr int kSubpixelScale = 1000; + +// 根据点云的bounding box, 看是否需要对地图进行扩张 +void GrowAsNeeded(const sensor::RangeData& range_data, + ProbabilityGrid* const probability_grid) { + // 找到点云的bounding_box + Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); + // Padding around bounding box to avoid numerical issues at cell boundaries. + constexpr float kPadding = 1e-6f; + for (const sensor::RangefinderPoint& hit : range_data.returns) { + bounding_box.extend(hit.position.head<2>()); + } + for (const sensor::RangefinderPoint& miss : range_data.misses) { + bounding_box.extend(miss.position.head<2>()); + } + // 是否对地图进行扩张 + probability_grid->GrowLimits(bounding_box.min() - + kPadding * Eigen::Vector2f::Ones()); + probability_grid->GrowLimits(bounding_box.max() + + kPadding * Eigen::Vector2f::Ones()); +} + +/** + * @brief 根据雷达点对栅格地图进行更新 + * + * @param[in] range_data + * @param[in] hit_table 更新占用栅格时的查找表 + * @param[in] miss_table 更新空闲栅格时的查找表 + * @param[in] insert_free_space + * @param[in] probability_grid 栅格地图 + */ + +//... + +Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) { + return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation(); +} +void CastRays(const sensor::RangeData& range_data, + const std::vector& hit_table, + const std::vector& miss_table, + const bool insert_free_space, + ProbabilityGrid* probability_grid, + const transform::Rigid3d local_pose_) { + // 根据雷达数据调整地图范围 + GrowAsNeeded(range_data, probability_grid); + + const MapLimits& limits = probability_grid->limits(); + const double superscaled_resolution = limits.resolution() / kSubpixelScale; + const MapLimits superscaled_limits( + superscaled_resolution, limits.max(), + CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, + limits.cell_limits().num_y_cells * kSubpixelScale)); + // 雷达原点在地图中的像素坐标, 作为画线的起始坐标 + const Eigen::Array2i begin = + superscaled_limits.GetCellIndex(range_data.origin.head<2>()); + // Compute and add the end points. + std::vector ends; + ends.reserve(range_data.returns.size()); + //。。。for (const sensor::RangefinderPoint& hit : range_data.returns) { + for (int i=0;i())); + ends.push_back(superscaled_limits.GetCellIndex((range_data.returns.points())[i].position.head<2>())); + // 更新hit点的栅格值 + //新写函数HitpplyLookupTable，增加入口参数range_data.returns + //。。。probability_grid->ApplyLookupTable(ends.back()/kSubpixelScale, hit_table); + probability_grid->ApplyLookupTableHit(ends.back()/kSubpixelScale, hit_table,uint8(range_data.returns.intensities()[i])); + } + + // 如果不插入free空间就可以结束了 + if (!insert_free_space) { + return; + } + + const Eigen::Matrix4d homo =ToEigen(local_pose_).matrix(); + int o_x=1000; + int o_y=1000; + int xm; + int ym; + if (begin.y()/1000>150){ + xm=-begin.y()/1000+20*homo(0,3)+(o_x+200); + ym=begin.x()/1000-20*homo(1,3)+(o_y-200); + + // LOG(INFO)<<"wyk--guiji jububiao:"< end; + end.reserve(range_data.returns.size()); + // Now add the misses. + for (int i=0;i())); + //如果需要使非摄像头视域的小物体保留下来，则需要当前时刻的整张地图信息以及当前时刻该子图的位姿 + // LOG(INFO)<<"wyk--guiji begin:"< ray = + RayToPixelMaskVisualNew(begin, end.back(), kSubpixelScale,uint8(range_data.returns.intensities()[i]),local_pose_,0,hit_table,probability_grid); + // RayToPixelMaskVisual(begin, end.back(), kSubpixelScale,uint8(range_data.returns.intensities()[i])); + if (ray.size()==0) + continue; + //if(ray.size()==0) + //LOG(INFO)<<"test!!!!!!!!!!!!!!!!!is:"<ApplyLookupTable(cell_index, miss_table); + } + } + + // // Now add the misses. + // for (const Eigen::Array2i& end : ends) { + // //返回一个vector miss点坐标的集合 如果在视线范围内看的到就进行光线跟踪，否则返回空vector 不做光线跟踪 + // std::vector ray = + // RayToPixelMask(begin, end, kSubpixelScale); + // for (const Eigen::Array2i& cell_index : ray) { + // // 从起点到end点之前, 更新miss点的栅格值 + // probability_grid->ApplyLookupTable(cell_index, miss_table); + // } + // } + + //Finally, compute and add empty rays based on misses in the range data. + for (const sensor::RangefinderPoint& missing_echo : range_data.misses) { + std::vector ray = RayToPixelMask( + begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()), + kSubpixelScale); + for (const Eigen::Array2i& cell_index : ray) { + // 从起点到misses点之前, 更新miss点的栅格值 + probability_grid->ApplyLookupTable(cell_index, miss_table); + } + } +} + + + +void CastRays(const sensor::RangeData& range_data, + const std::vector& hit_table, + const std::vector& miss_table, + const bool insert_free_space, ProbabilityGrid* probability_grid) { + // 根据雷达数据调整地图范围 + GrowAsNeeded(range_data, probability_grid); + + const MapLimits& limits = probability_grid->limits(); + const double superscaled_resolution = limits.resolution() / kSubpixelScale; + const MapLimits superscaled_limits( + superscaled_resolution, limits.max(), + CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, + limits.cell_limits().num_y_cells * kSubpixelScale)); + // 雷达原点在地图中的像素坐标, 作为画线的起始坐标 + const Eigen::Array2i begin = + superscaled_limits.GetCellIndex(range_data.origin.head<2>()); + // Compute and add the end points. + std::vector ends; + ends.reserve(range_data.returns.size()); + //。。。for (const sensor::RangefinderPoint& hit : range_data.returns) { + for (int i=0;i())); + ends.push_back(superscaled_limits.GetCellIndex((range_data.returns.points())[i].position.head<2>())); + // 更新hit点的栅格值 + //新写函数HitpplyLookupTable，增加入口参数range_data.returns + //。。。probability_grid->ApplyLookupTable(ends.back()/kSubpixelScale, hit_table); + probability_grid->ApplyLookupTableHit(ends.back()/kSubpixelScale, hit_table,uint8(range_data.returns.intensities()[i])); + } + + // 如果不插入free空间就可以结束了 + if (!insert_free_space) { + return; + } + std::vector end; + end.reserve(range_data.returns.size()); + for (int i=0;i())); + std::vector ray = + RayToPixelMaskVisual(begin, end.back(), kSubpixelScale,uint8(range_data.returns.intensities()[i])); + if (ray.size()==0) + continue; + for (const Eigen::Array2i& cell_index : ray) { + // 从起点到end点之前, 更新miss点的栅格值 + probability_grid->ApplyLookupTable(cell_index, miss_table); + } + } + + // Finally, compute and add empty rays based on misses in the range data. + for (const sensor::RangefinderPoint& missing_echo : range_data.misses) { + std::vector ray = RayToPixelMask( + begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()), + kSubpixelScale); + for (const Eigen::Array2i& cell_index : ray) { + // 从起点到misses点之前, 更新miss点的栅格值 + probability_grid->ApplyLookupTable(cell_index, miss_table); + } + } +} +} // namespace + +proto::ProbabilityGridRangeDataInserterOptions2D +CreateProbabilityGridRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::ProbabilityGridRangeDataInserterOptions2D 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; +} + +// 写入器的构造, 新建了2个查找表 +ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D( + const proto::ProbabilityGridRangeDataInserterOptions2D& options) + : options_(options), + // 生成更新占用栅格时的查找表 // param: hit_probability + hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( + Odds(options.hit_probability()))), // 0.55 + // 生成更新空闲栅格时的查找表 // param: miss_probability + miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( + Odds(options.miss_probability()))) {} // 0.49 + +/** + * @brief 将点云写入栅格地图 + * + * @param[in] range_data 要写入地图的点云 + * @param[in] grid 栅格地图 + */ +void ProbabilityGridRangeDataInserter2D::Insert( + const sensor::RangeData& range_data, GridInterface* const grid) const { + //将Grid2D类型强制转化成其子类 + ProbabilityGrid* const probability_grid = static_cast(grid); + CHECK(probability_grid != nullptr); + // By not finishing the update after hits are inserted, we give hits priority + // (i.e. no hits will be ignored because of a miss in the same cell). + // param: insert_free_space + CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), + probability_grid); + probability_grid->FinishUpdate(); +} + +void ProbabilityGridRangeDataInserter2D::Insert_new( + const sensor::RangeData& range_data, GridInterface* const grid,const transform::Rigid3d local_pose_) const { + //将Grid2D类型强制转化成其子类 Grid2D没有子图位姿 该函数需要改变接口，得到submap的local_pose + ProbabilityGrid* const probability_grid = static_cast(grid); + CHECK(probability_grid != nullptr); + // By not finishing the update after hits are inserted, we give hits priority + // (i.e. no hits will be ignored because of a miss in the same cell). + // param: insert_free_space + CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), + probability_grid,local_pose_); + probability_grid->FinishUpdate(); + +} + + +} // namespace mapping +} // namespace cartographer + diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h new file mode 100644 index 0000000..a324639 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_ +#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_ + +#include +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/range_data_inserter_interface.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +proto::ProbabilityGridRangeDataInserterOptions2D +CreateProbabilityGridRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface { + public: + explicit ProbabilityGridRangeDataInserter2D( + const proto::ProbabilityGridRangeDataInserterOptions2D& options); + + ProbabilityGridRangeDataInserter2D( + const ProbabilityGridRangeDataInserter2D&) = delete; + ProbabilityGridRangeDataInserter2D& operator=( + const ProbabilityGridRangeDataInserter2D&) = delete; + + // Inserts 'range_data' into 'probability_grid'. + virtual void Insert(const sensor::RangeData& range_data, + GridInterface* grid) const override; + virtual void Insert_new(const sensor::RangeData& range_data, + GridInterface* grid, + const transform::Rigid3d local_pose_) const override; + + private: + const proto::ProbabilityGridRangeDataInserterOptions2D options_; + const std::vector hit_table_; + const std::vector miss_table_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_test.cc new file mode 100644 index 0000000..334a0b4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/probability_grid_test.cc @@ -0,0 +1,206 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/2d/probability_grid.h" + +#include + +#include "cartographer/mapping/probability_values.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using Eigen::Array2i; +using Eigen::Vector2f; + +TEST(ProbabilityGridTest, ProtoConstructor) { + proto::Grid2D proto; + const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); + *proto.mutable_limits() = ToProto(limits); + for (int i = 6; i < 12; ++i) { + proto.mutable_cells()->Add(static_cast(i)); + } + proto.mutable_known_cells_box()->set_max_x(19); + proto.mutable_known_cells_box()->set_max_y(20); + proto.mutable_known_cells_box()->set_min_x(21); + proto.mutable_known_cells_box()->set_min_y(22); + proto.mutable_probability_grid_2d(); + + ValueConversionTables conversion_tables; + ProbabilityGrid grid(proto, &conversion_tables); + EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); + EXPECT_EQ(grid.GetGridType(), GridType::PROBABILITY_GRID); + + // TODO(macmason): Figure out how to test the contents of cells_ and + // {min, max}_{x, y}_ gracefully. +} + +TEST(ProbabilityGridTest, ConstructorGridType) { + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), + &conversion_tables); + EXPECT_EQ(probability_grid.GetGridType(), GridType::PROBABILITY_GRID); +} + +TEST(ProbabilityGridTest, ToProto) { + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), + &conversion_tables); + + const auto proto = probability_grid.ToProto(); + EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(), + proto.limits().DebugString()); + + // TODO(macmason): Figure out how to test the contents of cells_ and + // {min, max}_{x, y}_ gracefully. +} + +TEST(ProbabilityGridTest, ApplyOdds) { + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), + &conversion_tables); + const MapLimits& limits = probability_grid.limits(); + + EXPECT_TRUE(limits.Contains(Array2i(0, 0))); + EXPECT_TRUE(limits.Contains(Array2i(0, 1))); + EXPECT_TRUE(limits.Contains(Array2i(1, 0))); + EXPECT_TRUE(limits.Contains(Array2i(1, 1))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 0))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 1))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 0))); + EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 1))); + + probability_grid.SetProbability(Array2i(1, 0), 0.5); + + probability_grid.ApplyLookupTable( + Array2i(1, 0), + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9))); + probability_grid.FinishUpdate(); + EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5); + + probability_grid.SetProbability(Array2i(0, 1), 0.5); + probability_grid.ApplyLookupTable( + Array2i(0, 1), + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.1))); + probability_grid.FinishUpdate(); + EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5); + + // Tests adding odds to an unknown cell. + probability_grid.ApplyLookupTable( + Array2i(1, 1), + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.42))); + EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4); + + // Tests that further updates are ignored if FinishUpdate() isn't called. + probability_grid.ApplyLookupTable( + Array2i(1, 1), + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9))); + EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4); + probability_grid.FinishUpdate(); + probability_grid.ApplyLookupTable( + Array2i(1, 1), + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9))); + EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42); +} + +TEST(ProbabilityGridTest, GetProbability) { + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)), + &conversion_tables); + + const MapLimits& limits = probability_grid.limits(); + EXPECT_EQ(1., limits.max().x()); + EXPECT_EQ(2., 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.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), + kMaxProbability); + EXPECT_NEAR(probability_grid.GetProbability( + limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), + kMaxProbability, 1e-6); + for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)), + limits.GetCellIndex(Vector2f(0.5f, 0.5f)), + limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) { + EXPECT_TRUE(limits.Contains(xy_index)); + EXPECT_FALSE(probability_grid.IsKnown(xy_index)); + } +} + +TEST(ProbabilityGridTest, GetCellIndex) { + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)), + &conversion_tables); + + 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( + (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all()); + EXPECT_TRUE( + (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all()); + + // Check around the origin. + EXPECT_TRUE( + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all()); +} + +TEST(ProbabilityGridTest, CorrectCropping) { + // Create a probability grid with random values. + std::mt19937 rng(42); + std::uniform_real_distribution value_distribution(kMinProbability, + kMaxProbability); + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)), + &conversion_tables); + for (const Array2i& xy_index : + XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) { + probability_grid.SetProbability(xy_index, value_distribution(rng)); + } + Array2i offset; + CellLimits limits; + probability_grid.ComputeCroppedLimits(&offset, &limits); + EXPECT_TRUE((offset == Array2i(100, 100)).all()); + EXPECT_EQ(limits.num_x_cells, 200); + EXPECT_EQ(limits.num_y_cells, 200); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/range_data_inserter_2d_test.cc new file mode 100644 index 0000000..6b182aa --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -0,0 +1,137 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/probability_values.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +class RangeDataInserterTest2D : public ::testing::Test { + protected: + RangeDataInserterTest2D() + : probability_grid_( + MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5)), + &conversion_tables_) { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "insert_free_space = true, " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "}"); + options_ = CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary.get()); + range_data_inserter_ = + absl::make_unique(options_); + } + + void InsertPointCloud() { + sensor::RangeData range_data; + range_data.returns.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); + range_data.origin.x() = -0.5f; + range_data.origin.y() = 0.5f; + range_data_inserter_->Insert(range_data, &probability_grid_); + probability_grid_.FinishUpdate(); + } + + ValueConversionTables conversion_tables_; + ProbabilityGrid probability_grid_; + std::unique_ptr range_data_inserter_; + proto::ProbabilityGridRangeDataInserterOptions2D options_; +}; + +TEST_F(RangeDataInserterTest2D, InsertPointCloud) { + InsertPointCloud(); + + EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9); + EXPECT_NEAR(5., probability_grid_.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 cell_index(row, column); + EXPECT_TRUE(probability_grid_.limits().Contains(cell_index)); + switch (expected_states[column][row]) { + case State::UNKNOWN: + EXPECT_FALSE(probability_grid_.IsKnown(cell_index)); + break; + case State::MISS: + EXPECT_NEAR(options_.miss_probability(), + probability_grid_.GetProbability(cell_index), 1e-4); + break; + case State::HIT: + EXPECT_NEAR(options_.hit_probability(), + probability_grid_.GetProbability(cell_index), 1e-4); + break; + } + } + } +} + +TEST_F(RangeDataInserterTest2D, ProbabilityProgression) { + InsertPointCloud(); + EXPECT_NEAR( + options_.hit_probability(), + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-3.5f, 0.5f))), + 1e-4); + EXPECT_NEAR( + options_.miss_probability(), + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-2.5f, 0.5f))), + 1e-4); + + for (int i = 0; i < 1000; ++i) { + InsertPointCloud(); + } + EXPECT_NEAR( + kMaxProbability, + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-3.5f, 0.5f))), + 1e-3); + EXPECT_NEAR( + kMinProbability, + probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( + Eigen::Vector2f(-2.5f, 0.5f))), + 1e-3); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d.cc new file mode 100644 index 0000000..878f38c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d.cc @@ -0,0 +1,251 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/submap_2d.h" + +#include +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "absl/memory/memory.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/range_data_inserter_interface.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +proto::SubmapsOptions2D CreateSubmapsOptions2D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::SubmapsOptions2D options; + options.set_num_range_data( + parameter_dictionary->GetNonNegativeInt("num_range_data")); + *options.mutable_grid_options_2d() = CreateGridOptions2D( + parameter_dictionary->GetDictionary("grid_options_2d").get()); + *options.mutable_range_data_inserter_options() = + CreateRangeDataInserterOptions( + parameter_dictionary->GetDictionary("range_data_inserter").get()); + + bool valid_range_data_inserter_grid_combination = false; + const proto::GridOptions2D_GridType& grid_type = + options.grid_options_2d().grid_type(); + const proto::RangeDataInserterOptions_RangeDataInserterType& + range_data_inserter_type = + options.range_data_inserter_options().range_data_inserter_type(); + if (grid_type == proto::GridOptions2D::PROBABILITY_GRID && + range_data_inserter_type == + proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) { + valid_range_data_inserter_grid_combination = true; + } + if (grid_type == proto::GridOptions2D::TSDF && + range_data_inserter_type == + proto::RangeDataInserterOptions::TSDF_INSERTER_2D) { + valid_range_data_inserter_grid_combination = true; + } + CHECK(valid_range_data_inserter_grid_combination) + << "Invalid combination grid_type " << grid_type + << " with range_data_inserter_type " << range_data_inserter_type; + CHECK_GT(options.num_range_data(), 0); + return options; +} + +Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid, + ValueConversionTables* conversion_tables) + //由激光雷达原点生成子图原点？ + : Submap(transform::Rigid3d::Translation( + Eigen::Vector3d(origin.x(), origin.y(), 0.))), + conversion_tables_(conversion_tables) { + grid_ = std::move(grid); +} + +Submap2D::Submap2D(const proto::Submap2D& proto, + ValueConversionTables* conversion_tables) + : Submap(transform::ToRigid3(proto.local_pose())), + conversion_tables_(conversion_tables) { + if (proto.has_grid()) { + if (proto.grid().has_probability_grid_2d()) { + grid_ = + absl::make_unique(proto.grid(), conversion_tables_); + } else if (proto.grid().has_tsdf_2d()) { + grid_ = absl::make_unique(proto.grid(), conversion_tables_); + } else { + LOG(FATAL) << "proto::Submap2D has grid with unknown type."; + } + } + set_num_range_data(proto.num_range_data()); + set_insertion_finished(proto.finished()); +} + +proto::Submap Submap2D::ToProto(const bool include_grid_data) const { + proto::Submap proto; + auto* const submap_2d = proto.mutable_submap_2d(); + *submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); + submap_2d->set_num_range_data(num_range_data()); + submap_2d->set_finished(insertion_finished()); + if (include_grid_data) { + CHECK(grid_); + *submap_2d->mutable_grid() = grid_->ToProto(); + } + return proto; +} + +void Submap2D::UpdateFromProto(const proto::Submap& proto) { + CHECK(proto.has_submap_2d()); + const auto& submap_2d = proto.submap_2d(); + set_num_range_data(submap_2d.num_range_data()); + set_insertion_finished(submap_2d.finished()); + if (proto.submap_2d().has_grid()) { + if (proto.submap_2d().grid().has_probability_grid_2d()) { + grid_ = absl::make_unique(proto.submap_2d().grid(), + conversion_tables_); + } else if (proto.submap_2d().grid().has_tsdf_2d()) { + grid_ = absl::make_unique(proto.submap_2d().grid(), + conversion_tables_); + } else { + LOG(FATAL) << "proto::Submap2D has grid with unknown type."; + } + } +} + +void Submap2D::ToResponseProto( + const transform::Rigid3d&, + proto::SubmapQuery::Response* const response) const { + if (!grid_) return; + response->set_submap_version(num_range_data()); + proto::SubmapQuery::Response::SubmapTexture* const texture = + response->add_textures(); + grid()->DrawToSubmapTexture(texture, local_pose()); +} + +void Submap2D::InsertRangeData( + const sensor::RangeData& range_data, + const RangeDataInserterInterface* range_data_inserter) { + CHECK(grid_); + CHECK(!insertion_finished()); + range_data_inserter->Insert(range_data, grid_.get()); + set_num_range_data(num_range_data() + 1); +} +void Submap2D::InsertRangeDataNew( + const sensor::RangeData& range_data, + const RangeDataInserterInterface* range_data_inserter, + const transform::Rigid3d local_pose_) { + CHECK(grid_); + CHECK(!insertion_finished()); + range_data_inserter->Insert_new(range_data, grid_.get(),local_pose_); + set_num_range_data(num_range_data() + 1); +} +void Submap2D::Finish() { + CHECK(grid_); + CHECK(!insertion_finished()); + grid_ = grid_->ComputeCroppedGrid(); + set_insertion_finished(true); +} + +ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options) + : options_(options), range_data_inserter_(CreateRangeDataInserter()) {} + +std::vector> ActiveSubmaps2D::submaps() const { + return std::vector>(submaps_.begin(), + submaps_.end()); +} + +std::vector> ActiveSubmaps2D::InsertRangeData( + const sensor::RangeData& range_data) { + if (submaps_.empty() || + submaps_.back()->num_range_data() == options_.num_range_data()) { + AddSubmap(range_data.origin.head<2>()); + } + for (auto& submap : submaps_) { + //submap->InsertRangeData(range_data, range_data_inserter_.get()); + submap->InsertRangeDataNew(range_data, range_data_inserter_.get(),submap->local_pose()); + } + if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { + submaps_.front()->Finish(); + } + return submaps(); +} + + + +std::unique_ptr +ActiveSubmaps2D::CreateRangeDataInserter() { + switch (options_.range_data_inserter_options().range_data_inserter_type()) { + case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D: + return absl::make_unique( + options_.range_data_inserter_options() + .probability_grid_range_data_inserter_options_2d()); + case proto::RangeDataInserterOptions::TSDF_INSERTER_2D: + return absl::make_unique( + options_.range_data_inserter_options() + .tsdf_range_data_inserter_options_2d()); + default: + LOG(FATAL) << "Unknown RangeDataInserterType."; + } +} + +std::unique_ptr ActiveSubmaps2D::CreateGrid( + const Eigen::Vector2f& origin) { + constexpr int kInitialSubmapSize = 100; + float resolution = options_.grid_options_2d().resolution(); + switch (options_.grid_options_2d().grid_type()) { + case proto::GridOptions2D::PROBABILITY_GRID: + return absl::make_unique( + MapLimits(resolution, + origin.cast() + 0.5 * kInitialSubmapSize * + resolution * + Eigen::Vector2d::Ones(), + CellLimits(kInitialSubmapSize, kInitialSubmapSize)), + &conversion_tables_); + case proto::GridOptions2D::TSDF: + return absl::make_unique( + MapLimits(resolution, + origin.cast() + 0.5 * kInitialSubmapSize * + resolution * + Eigen::Vector2d::Ones(), + CellLimits(kInitialSubmapSize, kInitialSubmapSize)), + options_.range_data_inserter_options() + .tsdf_range_data_inserter_options_2d() + .truncation_distance(), + options_.range_data_inserter_options() + .tsdf_range_data_inserter_options_2d() + .maximum_weight(), + &conversion_tables_); + default: + LOG(FATAL) << "Unknown GridType."; + } +} + +void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { + if (submaps_.size() >= 2) { + // This will crop the finished Submap before inserting a new Submap to + // reduce peak memory usage a bit. + CHECK(submaps_.front()->insertion_finished()); + submaps_.erase(submaps_.begin()); + } + submaps_.push_back(absl::make_unique( + origin, + std::unique_ptr( + static_cast(CreateGrid(origin).release())), + &conversion_tables_)); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d.h new file mode 100644 index 0000000..4cc06b3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d.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_MAPPING_2D_SUBMAP_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" +#include "cartographer/mapping/range_data_inserter_interface.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/mapping/value_conversion_tables.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { + +proto::SubmapsOptions2D CreateSubmapsOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +class Submap2D : public Submap { + public: + Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid, + ValueConversionTables* conversion_tables); + explicit Submap2D(const proto::Submap2D& proto, + ValueConversionTables* conversion_tables); + + proto::Submap ToProto(bool include_grid_data) const override; + void UpdateFromProto(const proto::Submap& proto) override; + + void ToResponseProto(const transform::Rigid3d& global_submap_pose, + proto::SubmapQuery::Response* response) const override; + + const Grid2D* grid() const { return grid_.get(); } + + // Insert 'range_data' into this submap using 'range_data_inserter'. The + // submap must not be finished yet. + void InsertRangeData(const sensor::RangeData& range_data, + const RangeDataInserterInterface* range_data_inserter); + + //... + void InsertRangeDataNew(const sensor::RangeData& range_data, + const RangeDataInserterInterface* range_data_inserter, + const transform::Rigid3d local_pose_); + void Finish(); + + private: + std::unique_ptr grid_; + ValueConversionTables* conversion_tables_; +}; + +// The first active submap will be created on the insertion of the first range +// data. Except during this initialization when no or only one single submap +// exists, there are always two submaps into which range data is 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 range data 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 created. The "old" submap is forgotten by this object. +class ActiveSubmaps2D { + public: + explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options); + + ActiveSubmaps2D(const ActiveSubmaps2D&) = delete; + ActiveSubmaps2D& operator=(const ActiveSubmaps2D&) = delete; + + // Inserts 'range_data' into the Submap collection. + std::vector> InsertRangeData( + const sensor::RangeData& range_data); + + std::vector> submaps() const; + + private: + std::unique_ptr CreateRangeDataInserter(); + std::unique_ptr CreateGrid(const Eigen::Vector2f& origin); + void FinishSubmap(); + void AddSubmap(const Eigen::Vector2f& origin); + + const proto::SubmapsOptions2D options_; + std::vector> submaps_; + std::unique_ptr range_data_inserter_; + ValueConversionTables conversion_tables_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d_test.cc new file mode 100644 index 0000000..d9b6e21 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/submap_2d_test.cc @@ -0,0 +1,127 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/2d/submap_2d.h" + +#include +#include +#include +#include + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { + constexpr int kNumRangeData = 10; + auto parameter_dictionary = common::MakeDictionary( + "return {" + "num_range_data = " + + std::to_string(kNumRangeData) + + ", " + "grid_options_2d = {" + "grid_type = \"PROBABILITY_GRID\"," + "resolution = 0.05, " + "}," + "range_data_inserter = {" + "range_data_inserter_type = \"PROBABILITY_GRID_INSERTER_2D\"," + "probability_grid_range_data_inserter = {" + "insert_free_space = true, " + "hit_probability = 0.53, " + "miss_probability = 0.495, " + "}," + "tsdf_range_data_inserter = { " + "truncation_distance = 2.0," + "maximum_weight = 10.," + "update_free_space = false," + "normal_estimation_options = {" + "num_normal_samples = 2," + "sample_radius = 10.," + "}," + "project_sdf_distance_to_scan_normal = false," + "update_weight_range_exponent = 0," + "update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0," + "update_weight_distance_cell_to_hit_kernel_bandwidth = 0," + "}," + "}," + "}"); + ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())}; + std::set> all_submaps; + for (int i = 0; i != 1000; ++i) { + auto insertion_submaps = + submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); + // Except for the first, maps should only be returned after enough range + // data. + for (const auto& submap : insertion_submaps) { + all_submaps.insert(submap); + } + if (submaps.submaps().size() > 1) { + EXPECT_LE(kNumRangeData, submaps.submaps().front()->num_range_data()); + } + } + EXPECT_EQ(2, submaps.submaps().size()); + int correct_num_finished_submaps = 0; + int num_unfinished_submaps = 0; + for (const auto& submap : all_submaps) { + if (submap->num_range_data() == kNumRangeData * 2) { + ++correct_num_finished_submaps; + } else { + EXPECT_EQ(kNumRangeData, submap->num_range_data()); + ++num_unfinished_submaps; + } + } + // Submaps should not be left without the right number of range data in them. + EXPECT_EQ(correct_num_finished_submaps, all_submaps.size() - 1); + EXPECT_EQ(1, num_unfinished_submaps); +} + +TEST(Submap2DTest, ToFromProto) { + MapLimits expected_map_limits(1., Eigen::Vector2d(2., 3.), + CellLimits(100, 110)); + ValueConversionTables conversion_tables; + Submap2D expected(Eigen::Vector2f(4.f, 5.f), + absl::make_unique(expected_map_limits, + &conversion_tables), + &conversion_tables); + const proto::Submap proto = + expected.ToProto(true /* include_probability_grid_data */); + EXPECT_TRUE(proto.has_submap_2d()); + EXPECT_FALSE(proto.has_submap_3d()); + const auto actual = Submap2D(proto.submap_2d(), &conversion_tables); + EXPECT_TRUE(expected.local_pose().translation().isApprox( + actual.local_pose().translation(), 1e-6)); + EXPECT_TRUE(expected.local_pose().rotation().isApprox( + actual.local_pose().rotation(), 1e-6)); + EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); + EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished()); + EXPECT_NEAR(expected.grid()->limits().resolution(), + actual.grid()->limits().resolution(), 1e-6); + EXPECT_TRUE(expected.grid()->limits().max().isApprox( + actual.grid()->limits().max(), 1e-6)); + EXPECT_EQ(expected.grid()->limits().cell_limits().num_x_cells, + actual.grid()->limits().cell_limits().num_x_cells); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/xy_index.h b/carto_ws/src/cartographer-master/cartographer/mapping/2d/xy_index.h new file mode 100644 index 0000000..2899e2d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/xy_index.h @@ -0,0 +1,114 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ +#define CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/proto/cell_limits_2d.pb.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +struct CellLimits { + CellLimits() = default; + CellLimits(int init_num_x_cells, int init_num_y_cells) + : num_x_cells(init_num_x_cells), num_y_cells(init_num_y_cells) {} + + explicit CellLimits(const proto::CellLimits& cell_limits) + : num_x_cells(cell_limits.num_x_cells()), + num_y_cells(cell_limits.num_y_cells()) {} + + int num_x_cells = 0; + int num_y_cells = 0; +}; + +inline proto::CellLimits ToProto(const CellLimits& cell_limits) { + proto::CellLimits result; + result.set_num_x_cells(cell_limits.num_x_cells); + result.set_num_y_cells(cell_limits.num_y_cells); + return result; +} + +// Iterates in row-major order through a range of xy-indices. +class XYIndexRangeIterator + : public std::iterator { + public: + // Constructs a new iterator for the specified range. + XYIndexRangeIterator(const Eigen::Array2i& min_xy_index, + const Eigen::Array2i& max_xy_index) + : min_xy_index_(min_xy_index), + max_xy_index_(max_xy_index), + xy_index_(min_xy_index) {} + + // Constructs a new iterator for everything contained in 'cell_limits'. + explicit XYIndexRangeIterator(const CellLimits& cell_limits) + : XYIndexRangeIterator(Eigen::Array2i::Zero(), + Eigen::Array2i(cell_limits.num_x_cells - 1, + cell_limits.num_y_cells - 1)) {} + + XYIndexRangeIterator& operator++() { + // This is a necessary evil. Bounds checking is very expensive and needs to + // be avoided in production. We have unit tests that exercise this check + // in debug mode. + DCHECK(*this != end()); + if (xy_index_.x() < max_xy_index_.x()) { + ++xy_index_.x(); + } else { + xy_index_.x() = min_xy_index_.x(); + ++xy_index_.y(); + } + return *this; + } + + Eigen::Array2i& operator*() { return xy_index_; } + + bool operator==(const XYIndexRangeIterator& other) const { + return (xy_index_ == other.xy_index_).all(); + } + + bool operator!=(const XYIndexRangeIterator& other) const { + return !operator==(other); + } + + XYIndexRangeIterator begin() { + return XYIndexRangeIterator(min_xy_index_, max_xy_index_); + } + + XYIndexRangeIterator end() { + XYIndexRangeIterator it = begin(); + it.xy_index_ = Eigen::Array2i(min_xy_index_.x(), max_xy_index_.y() + 1); + return it; + } + + private: + Eigen::Array2i min_xy_index_; + Eigen::Array2i max_xy_index_; + Eigen::Array2i xy_index_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/2d/xy_index_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/2d/xy_index_test.cc new file mode 100644 index 0000000..e80dfc4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/2d/xy_index_test.cc @@ -0,0 +1,61 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/2d/xy_index.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(XYIndexTest, CellLimitsToProto) { + const CellLimits limits(1, 2); + const auto proto = ToProto(limits); + EXPECT_EQ(limits.num_x_cells, proto.num_x_cells()); + EXPECT_EQ(limits.num_y_cells, proto.num_y_cells()); +} + +TEST(XYIndexTest, CellLimitsProtoConstructor) { + proto::CellLimits limits; + limits.set_num_x_cells(1); + limits.set_num_y_cells(2); + + auto native = CellLimits(limits); + EXPECT_EQ(limits.num_x_cells(), native.num_x_cells); + EXPECT_EQ(limits.num_y_cells(), native.num_y_cells); +} + +TEST(XYIndexTest, XYIndexRangeIterator) { + const Eigen::Array2i min(1, 2); + const Eigen::Array2i max(3, 4); + XYIndexRangeIterator it(min, max); + EXPECT_TRUE((min == *it.begin()).all()) << *it.begin(); + EXPECT_TRUE((Eigen::Array2i(1, 5) == *it.end()).all()) << *it.end(); + EXPECT_TRUE((min == *it).all()) << *it; + int num_indices = 0; + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(min, max)) { + LOG(INFO) << xy_index; + EXPECT_TRUE((xy_index >= min).all()); + EXPECT_TRUE((xy_index <= max).all()); + ++num_indices; + } + EXPECT_EQ(9, num_indices); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/hybrid_grid.h b/carto_ws/src/cartographer-master/cartographer/mapping/3d/hybrid_grid.h new file mode 100644 index 0000000..5b746b5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/hybrid_grid.h @@ -0,0 +1,576 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ +#define CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ + +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/proto/hybrid_grid.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat +// z-major index. +inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) { + DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index; + return (((index.z() << bits) + index.y()) << bits) + index.x(); +} + +// Converts a flat z-major 'index' to a 3-dimensional index with each dimension +// from 0 to 2^'bits' - 1. +inline Eigen::Array3i To3DIndex(const int index, const int bits) { + DCHECK_LT(index, 1 << (3 * bits)); + const int mask = (1 << bits) - 1; + return Eigen::Array3i(index & mask, (index >> bits) & mask, + (index >> bits) >> bits); +} + +// A function to compare value to the default value. (Allows specializations). +template +bool IsDefaultValue(const TValueType& v) { + return v == TValueType(); +} + +// Specialization to compare a std::vector to the default value. +template +bool IsDefaultValue(const std::vector& v) { + return v.empty(); +} + +// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of +// type 'ValueType' in contiguous memory. Indices in each dimension are 0-based. +template +class FlatGrid { + public: + using ValueType = TValueType; + + // Creates a new flat grid with all values being default constructed. + FlatGrid() { + for (ValueType& value : cells_) { + value = ValueType(); + } + } + + FlatGrid(const FlatGrid&) = delete; + FlatGrid& operator=(const FlatGrid&) = delete; + + // Returns the number of voxels per dimension. + static int grid_size() { return 1 << kBits; } + + // Returns the value stored at 'index', each dimension of 'index' being + // between 0 and grid_size() - 1. + ValueType value(const Eigen::Array3i& index) const { + return cells_[ToFlatIndex(index, kBits)]; + } + + // Returns a pointer to a value to allow changing it. + ValueType* mutable_value(const Eigen::Array3i& index) { + return &cells_[ToFlatIndex(index, kBits)]; + } + + // An iterator for iterating over all values not comparing equal to the + // default constructed value. + class Iterator { + public: + Iterator() : current_(nullptr), end_(nullptr) {} + + explicit Iterator(const FlatGrid& flat_grid) + : current_(flat_grid.cells_.data()), + end_(flat_grid.cells_.data() + flat_grid.cells_.size()) { + while (!Done() && IsDefaultValue(*current_)) { + ++current_; + } + } + + void Next() { + DCHECK(!Done()); + do { + ++current_; + } while (!Done() && IsDefaultValue(*current_)); + } + + bool Done() const { return current_ == end_; } + + Eigen::Array3i GetCellIndex() const { + DCHECK(!Done()); + const int index = (1 << (3 * kBits)) - (end_ - current_); + return To3DIndex(index, kBits); + } + + const ValueType& GetValue() const { + DCHECK(!Done()); + return *current_; + } + + private: + const ValueType* current_; + const ValueType* end_; + }; + + private: + std::array cells_; +}; + +// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type +// 'WrappedGrid'. Wrapped grids are constructed on first access via +// 'mutable_value()'. +template +class NestedGrid { + public: + using ValueType = typename WrappedGrid::ValueType; + + // Returns the number of voxels per dimension. + static int grid_size() { return WrappedGrid::grid_size() << kBits; } + + // Returns the value stored at 'index', each dimension of 'index' being + // between 0 and grid_size() - 1. + ValueType value(const Eigen::Array3i& index) const { + const Eigen::Array3i meta_index = GetMetaIndex(index); + const WrappedGrid* const meta_cell = + meta_cells_[ToFlatIndex(meta_index, kBits)].get(); + if (meta_cell == nullptr) { + return ValueType(); + } + const Eigen::Array3i inner_index = + index - meta_index * WrappedGrid::grid_size(); + return meta_cell->value(inner_index); + } + + // Returns a pointer to the value at 'index' to allow changing it. If + // necessary a new wrapped grid is constructed to contain that value. + ValueType* mutable_value(const Eigen::Array3i& index) { + const Eigen::Array3i meta_index = GetMetaIndex(index); + std::unique_ptr& meta_cell = + meta_cells_[ToFlatIndex(meta_index, kBits)]; + if (meta_cell == nullptr) { + meta_cell = absl::make_unique(); + } + const Eigen::Array3i inner_index = + index - meta_index * WrappedGrid::grid_size(); + return meta_cell->mutable_value(inner_index); + } + + // An iterator for iterating over all values not comparing equal to the + // default constructed value. + class Iterator { + public: + Iterator() : current_(nullptr), end_(nullptr), nested_iterator_() {} + + explicit Iterator(const NestedGrid& nested_grid) + : current_(nested_grid.meta_cells_.data()), + end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()), + nested_iterator_() { + AdvanceToValidNestedIterator(); + } + + void Next() { + DCHECK(!Done()); + nested_iterator_.Next(); + if (!nested_iterator_.Done()) { + return; + } + ++current_; + AdvanceToValidNestedIterator(); + } + + bool Done() const { return current_ == end_; } + + Eigen::Array3i GetCellIndex() const { + DCHECK(!Done()); + const int index = (1 << (3 * kBits)) - (end_ - current_); + return To3DIndex(index, kBits) * WrappedGrid::grid_size() + + nested_iterator_.GetCellIndex(); + } + + const ValueType& GetValue() const { + DCHECK(!Done()); + return nested_iterator_.GetValue(); + } + + private: + void AdvanceToValidNestedIterator() { + for (; !Done(); ++current_) { + if (*current_ != nullptr) { + nested_iterator_ = typename WrappedGrid::Iterator(**current_); + if (!nested_iterator_.Done()) { + break; + } + } + } + } + + const std::unique_ptr* current_; + const std::unique_ptr* end_; + typename WrappedGrid::Iterator nested_iterator_; + }; + + private: + // Returns the Eigen::Array3i (meta) index of the meta cell containing + // 'index'. + Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const { + DCHECK((index >= 0).all()) << index; + const Eigen::Array3i meta_index = index / WrappedGrid::grid_size(); + DCHECK((meta_index < (1 << kBits)).all()) << index; + return meta_index; + } + + std::array, 1 << (3 * kBits)> meta_cells_; +}; + +// A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped +// grids are constructed on first access via 'mutable_value()'. If necessary, +// the grid grows to twice the size in each dimension. The range of indices is +// (almost) symmetric around the origin, i.e. negative indices are allowed. +template +class DynamicGrid { + public: + using ValueType = typename WrappedGrid::ValueType; + + DynamicGrid() : bits_(1), meta_cells_(8) {} + DynamicGrid(DynamicGrid&&) = default; + DynamicGrid& operator=(DynamicGrid&&) = default; + + // Returns the current number of voxels per dimension. + int grid_size() const { return WrappedGrid::grid_size() << bits_; } + + // Returns the value stored at 'index'. + ValueType value(const Eigen::Array3i& index) const { + const Eigen::Array3i shifted_index = index + (grid_size() >> 1); + // The cast to unsigned is for performance to check with 3 comparisons + // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size. + if ((shifted_index.cast() >= grid_size()).any()) { + return ValueType(); + } + const Eigen::Array3i meta_index = GetMetaIndex(shifted_index); + const WrappedGrid* const meta_cell = + meta_cells_[ToFlatIndex(meta_index, bits_)].get(); + if (meta_cell == nullptr) { + return ValueType(); + } + const Eigen::Array3i inner_index = + shifted_index - meta_index * WrappedGrid::grid_size(); + return meta_cell->value(inner_index); + } + + // Returns a pointer to the value at 'index' to allow changing it, dynamically + // growing the DynamicGrid and constructing new WrappedGrids as needed. + ValueType* mutable_value(const Eigen::Array3i& index) { + const Eigen::Array3i shifted_index = index + (grid_size() >> 1); + // The cast to unsigned is for performance to check with 3 comparisons + // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size. + if ((shifted_index.cast() >= grid_size()).any()) { + Grow(); + return mutable_value(index); + } + const Eigen::Array3i meta_index = GetMetaIndex(shifted_index); + std::unique_ptr& meta_cell = + meta_cells_[ToFlatIndex(meta_index, bits_)]; + if (meta_cell == nullptr) { + meta_cell = absl::make_unique(); + } + const Eigen::Array3i inner_index = + shifted_index - meta_index * WrappedGrid::grid_size(); + return meta_cell->mutable_value(inner_index); + } + + // An iterator for iterating over all values not comparing equal to the + // default constructed value. + class Iterator { + public: + explicit Iterator(const DynamicGrid& dynamic_grid) + : bits_(dynamic_grid.bits_), + current_(dynamic_grid.meta_cells_.data()), + end_(dynamic_grid.meta_cells_.data() + + dynamic_grid.meta_cells_.size()), + nested_iterator_() { + AdvanceToValidNestedIterator(); + } + + void Next() { + DCHECK(!Done()); + nested_iterator_.Next(); + if (!nested_iterator_.Done()) { + return; + } + ++current_; + AdvanceToValidNestedIterator(); + } + + bool Done() const { return current_ == end_; } + + Eigen::Array3i GetCellIndex() const { + DCHECK(!Done()); + const int outer_index = (1 << (3 * bits_)) - (end_ - current_); + const Eigen::Array3i shifted_index = + To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() + + nested_iterator_.GetCellIndex(); + return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size()); + } + + const ValueType& GetValue() const { + DCHECK(!Done()); + return nested_iterator_.GetValue(); + } + + void AdvanceToEnd() { current_ = end_; } + + const std::pair operator*() const { + return std::pair(GetCellIndex(), GetValue()); + } + + Iterator& operator++() { + Next(); + return *this; + } + + bool operator!=(const Iterator& it) const { + return it.current_ != current_; + } + + private: + void AdvanceToValidNestedIterator() { + for (; !Done(); ++current_) { + if (*current_ != nullptr) { + nested_iterator_ = typename WrappedGrid::Iterator(**current_); + if (!nested_iterator_.Done()) { + break; + } + } + } + } + + int bits_; + const std::unique_ptr* current_; + const std::unique_ptr* const end_; + typename WrappedGrid::Iterator nested_iterator_; + }; + + private: + // Returns the Eigen::Array3i (meta) index of the meta cell containing + // 'index'. + Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const { + DCHECK((index >= 0).all()) << index; + const Eigen::Array3i meta_index = index / WrappedGrid::grid_size(); + DCHECK((meta_index < (1 << bits_)).all()) << index; + return meta_index; + } + + // Grows this grid by a factor of 2 in each of the 3 dimensions. + void Grow() { + const int new_bits = bits_ + 1; + CHECK_LE(new_bits, 8); + std::vector> new_meta_cells_( + 8 * meta_cells_.size()); + for (int z = 0; z != (1 << bits_); ++z) { + for (int y = 0; y != (1 << bits_); ++y) { + for (int x = 0; x != (1 << bits_); ++x) { + const Eigen::Array3i original_meta_index(x, y, z); + const Eigen::Array3i new_meta_index = + original_meta_index + (1 << (bits_ - 1)); + new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] = + std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]); + } + } + } + meta_cells_ = std::move(new_meta_cells_); + bits_ = new_bits; + } + + int bits_; + std::vector> meta_cells_; +}; + +template +using GridBase = DynamicGrid, 3>>; + +// Represents a 3D grid as a wide, shallow tree. +template +class HybridGridBase : public GridBase { + public: + using Iterator = typename GridBase::Iterator; + + // Creates a new tree-based probability grid with voxels having edge length + // 'resolution' around the origin which becomes the center of the cell at + // index (0, 0, 0). + explicit HybridGridBase(const float resolution) : resolution_(resolution) {} + + float resolution() const { return resolution_; } + + // Returns the index of the cell containing the 'point'. Indices are integer + // vectors identifying cells, for this the coordinates are rounded to the next + // multiple of the resolution. + Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const { + Eigen::Array3f index = point.array() / resolution_; + return Eigen::Array3i(common::RoundToInt(index.x()), + common::RoundToInt(index.y()), + common::RoundToInt(index.z())); + } + + // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1). + static Eigen::Array3i GetOctant(const int i) { + DCHECK_GE(i, 0); + DCHECK_LT(i, 8); + return Eigen::Array3i(static_cast(i & 1), static_cast(i & 2), + static_cast(i & 4)); + } + + // Returns the center of the cell at 'index'. + Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const { + return index.matrix().cast() * resolution_; + } + + // Iterator functions for range-based for loops. + Iterator begin() const { return Iterator(*this); } + + Iterator end() const { + Iterator it(*this); + it.AdvanceToEnd(); + return it; + } + + private: + // Edge length of each voxel. + const float resolution_; +}; + +// A grid containing probability values stored using 15 bits, and an update +// marker per voxel. +// Points are expected to be close to the origin. Points far from the origin +// require the grid to grow dynamically. For centimeter resolution, points +// can only be tens of meters from the origin. +// The hard limit of cell indexes is +/- 8192 around the origin. +class HybridGrid : public HybridGridBase { + public: + explicit HybridGrid(const float resolution) + : HybridGridBase(resolution) {} + + explicit HybridGrid(const proto::HybridGrid& proto) + : HybridGrid(proto.resolution()) { + CHECK_EQ(proto.values_size(), proto.x_indices_size()); + CHECK_EQ(proto.values_size(), proto.y_indices_size()); + CHECK_EQ(proto.values_size(), proto.z_indices_size()); + for (int i = 0; i < proto.values_size(); ++i) { + // SetProbability does some error checking for us. + SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i), + proto.z_indices(i)), + ValueToProbability(proto.values(i))); + } + } + + // Sets the probability of the cell at 'index' to the given 'probability'. + void SetProbability(const Eigen::Array3i& index, const float probability) { + *mutable_value(index) = ProbabilityToValue(probability); + } + + // Finishes the update sequence. + void FinishUpdate() { + while (!update_indices_.empty()) { + DCHECK_GE(*update_indices_.back(), kUpdateMarker); + *update_indices_.back() -= kUpdateMarker; + update_indices_.pop_back(); + } + } + + // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() + // to the probability of the cell at 'index' if the cell has not already been + // updated. Multiple updates of the same cell will be ignored until + // FinishUpdate() is called. Returns true if the cell was updated. + // + // If this is the first call to ApplyOdds() for the specified cell, its value + // will be set to probability corresponding to 'odds'. + bool ApplyLookupTable(const Eigen::Array3i& index, + const std::vector& table) { + DCHECK_EQ(table.size(), kUpdateMarker); + uint16* const cell = mutable_value(index); + if (*cell >= kUpdateMarker) { + return false; + } + update_indices_.push_back(cell); + *cell = table[*cell]; + DCHECK_GE(*cell, kUpdateMarker); + return true; + } + + // Returns the probability of the cell with 'index'. + float GetProbability(const Eigen::Array3i& index) const { + return ValueToProbability(value(index)); + } + + // Returns true if the probability at the specified 'index' is known. + bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; } + + proto::HybridGrid ToProto() const { + CHECK(update_indices_.empty()) << "Serializing a grid during an update is " + "not supported. Finish the update first."; + proto::HybridGrid result; + result.set_resolution(resolution()); + for (const auto it : *this) { + result.add_x_indices(it.first.x()); + result.add_y_indices(it.first.y()); + result.add_z_indices(it.first.z()); + result.add_values(it.second); + } + return result; + } + + private: + // Markers at changed cells. + std::vector update_indices_; +}; + +struct AverageIntensityData { + float sum = 0.f; + int count = 0; +}; + +class IntensityHybridGrid : public HybridGridBase { + public: + explicit IntensityHybridGrid(const float resolution) + : HybridGridBase(resolution) {} + + void AddIntensity(const Eigen::Array3i& index, const float intensity) { + AverageIntensityData* const cell = mutable_value(index); + cell->count += 1; + cell->sum += intensity; + } + + float GetIntensity(const Eigen::Array3i& index) const { + const AverageIntensityData& cell = value(index); + if (cell.count == 0) { + return 0.f; + } else { + return cell.sum / cell.count; + } + } +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/hybrid_grid_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/3d/hybrid_grid_test.cc new file mode 100644 index 0000000..f600032 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/hybrid_grid_test.cc @@ -0,0 +1,247 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/3d/hybrid_grid.h" + +#include +#include +#include + +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(HybridGridTest, ApplyOdds) { + HybridGrid hybrid_grid(1.f); + + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 1))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 1))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 1))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 1))); + + hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f); + + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 0, 1), + ComputeLookupTableToApplyOdds(Odds(0.9f))); + hybrid_grid.FinishUpdate(); + EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f); + + hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f); + + hybrid_grid.ApplyLookupTable(Eigen::Array3i(0, 1, 0), + ComputeLookupTableToApplyOdds(Odds(0.1f))); + hybrid_grid.FinishUpdate(); + EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f); + + // Tests adding odds to an unknown cell. + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1), + ComputeLookupTableToApplyOdds(Odds(0.42f))); + EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); + + // Tests that further updates are ignored if FinishUpdate() isn't called. + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1), + ComputeLookupTableToApplyOdds(Odds(0.9f))); + EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); + hybrid_grid.FinishUpdate(); + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1), + ComputeLookupTableToApplyOdds(Odds(0.9f))); + EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f); +} + +TEST(HybridGridTest, GetProbability) { + HybridGrid hybrid_grid(1.f); + + hybrid_grid.SetProbability( + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)), + kMaxProbability); + EXPECT_NEAR(hybrid_grid.GetProbability( + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))), + kMaxProbability, 1e-6); + for (const Eigen::Array3i& index : + {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) { + EXPECT_FALSE(hybrid_grid.IsKnown(index)); + } +} + +TEST(HybridGridTest, GetIntensity) { + IntensityHybridGrid hybrid_grid(1.f); + const Eigen::Array3i cell_index = + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)); + const float intensity = 58.0f; + + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), 0.0f, 1e-9); + hybrid_grid.AddIntensity(cell_index, intensity); + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), intensity, 1e-9); + for (const Eigen::Array3i& index : + {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) { + EXPECT_NEAR(hybrid_grid.GetIntensity(index), 0.0f, 1e-9); + } +} + +MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); } + +TEST(HybridGridTest, GetCellIndex) { + HybridGrid hybrid_grid(2.f); + + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 0.f, 0.f)), + AllCwiseEqual(Eigen::Array3i(0, 0, 0))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 26.f, 10.f)), + AllCwiseEqual(Eigen::Array3i(0, 13, 5))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 0.f, 10.f)), + AllCwiseEqual(Eigen::Array3i(7, 0, 5))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 26.f, 0.f)), + AllCwiseEqual(Eigen::Array3i(7, 13, 0))); + + // Check around the origin. + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(8.5f, 11.5f, 0.5f)), + AllCwiseEqual(Eigen::Array3i(4, 6, 0))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.5f, 12.5f, 1.5f)), + AllCwiseEqual(Eigen::Array3i(4, 6, 1))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(6.5f, 14.5f, 2.5f)), + AllCwiseEqual(Eigen::Array3i(3, 7, 1))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(5.5f, 13.5f, 3.5f)), + AllCwiseEqual(Eigen::Array3i(3, 7, 2))); +} + +TEST(HybridGridTest, GetCenterOfCell) { + HybridGrid hybrid_grid(2.f); + + const Eigen::Array3i index(3, 2, 1); + const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index); + EXPECT_NEAR(6.f, center.x(), 1e-6); + EXPECT_NEAR(4.f, center.y(), 1e-6); + EXPECT_NEAR(2.f, center.z(), 1e-6); + EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index)); +} + +class RandomHybridGridTest : public ::testing::Test { + public: + RandomHybridGridTest() : hybrid_grid_(2.f), values_() { + std::mt19937 rng(1285120005); + std::uniform_real_distribution value_distribution(kMinProbability, + kMaxProbability); + std::uniform_int_distribution xyz_distribution(-3000, 2999); + for (int i = 0; i < 10000; ++i) { + const auto x = xyz_distribution(rng); + const auto y = xyz_distribution(rng); + const auto z = xyz_distribution(rng); + values_.emplace(std::make_tuple(x, y, z), value_distribution(rng)); + } + + for (const auto& pair : values_) { + const Eigen::Array3i cell_index(std::get<0>(pair.first), + std::get<1>(pair.first), + std::get<2>(pair.first)); + hybrid_grid_.SetProbability(cell_index, pair.second); + } + } + + protected: + HybridGrid hybrid_grid_; + using ValueMap = std::map, float>; + ValueMap values_; +}; + +TEST_F(RandomHybridGridTest, TestIteration) { + for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) { + const Eigen::Array3i cell_index = it.GetCellIndex(); + const float iterator_probability = ValueToProbability(it.GetValue()); + EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index)); + const std::tuple key = + std::make_tuple(cell_index[0], cell_index[1], cell_index[2]); + EXPECT_TRUE(values_.count(key)); + EXPECT_NEAR(values_[key], iterator_probability, 1e-4); + values_.erase(key); + } + + // Test that range based loop is equivalent to using the iterator. + auto it = HybridGrid::Iterator(hybrid_grid_); + for (const auto& cell : hybrid_grid_) { + ASSERT_FALSE(it.Done()); + EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex())); + EXPECT_EQ(cell.second, it.GetValue()); + it.Next(); + } + + // Now 'values_' must not contain values. + for (const auto& pair : values_) { + const Eigen::Array3i cell_index(std::get<0>(pair.first), + std::get<1>(pair.first), + std::get<2>(pair.first)); + ADD_FAILURE() << cell_index << " Probability: " << pair.second; + } +} + +TEST_F(RandomHybridGridTest, ToProto) { + const auto proto = hybrid_grid_.ToProto(); + EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution()); + ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size()); + ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size()); + ASSERT_EQ(proto.x_indices_size(), proto.values_size()); + + ValueMap proto_map; + for (int i = 0; i < proto.x_indices_size(); ++i) { + proto_map[std::make_tuple(proto.x_indices(i), proto.y_indices(i), + proto.z_indices(i))] = proto.values(i); + } + + // Get hybrid_grid_ into the same format. + ValueMap hybrid_grid_map; + for (const auto i : hybrid_grid_) { + hybrid_grid_map[std::make_tuple(i.first.x(), i.first.y(), i.first.z())] = + i.second; + } + + EXPECT_EQ(proto_map, hybrid_grid_map); +} + +struct EigenComparator { + bool operator()(const Eigen::Vector3i& lhs, + const Eigen::Vector3i& rhs) const { + return std::forward_as_tuple(lhs.x(), lhs.y(), lhs.z()) < + std::forward_as_tuple(rhs.x(), rhs.y(), rhs.z()); + } +}; + +TEST_F(RandomHybridGridTest, FromProto) { + const HybridGrid constructed_grid(hybrid_grid_.ToProto()); + + std::map member_map; + for (const auto& cell : hybrid_grid_) { + member_map.insert(cell); + } + + std::map constructed_map; + for (const auto& cell : constructed_grid) { + constructed_map.insert(cell); + } + + EXPECT_EQ(member_map, constructed_map); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d.cc new file mode 100644 index 0000000..5553237 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d.cc @@ -0,0 +1,117 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/3d/range_data_inserter_3d.h" + +#include "Eigen/Core" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace { + +void InsertMissesIntoGrid(const std::vector& miss_table, + const Eigen::Vector3f& origin, + const sensor::PointCloud& returns, + HybridGrid* hybrid_grid, + const int num_free_space_voxels) { + const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin); + for (const sensor::RangefinderPoint& hit : returns) { + const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); + + const Eigen::Array3i delta = hit_cell - origin_cell; + const int num_samples = delta.cwiseAbs().maxCoeff(); + CHECK_LT(num_samples, 1 << 15); + // 'num_samples' is the number of samples we equi-distantly place on the + // line between 'origin' and 'hit'. (including a fractional part for sub- + // voxels) It is chosen so that between two samples we change from one voxel + // to the next on the fastest changing dimension. + // + // Only the last 'num_free_space_voxels' are updated for performance. + for (int position = std::max(0, num_samples - num_free_space_voxels); + position < num_samples; ++position) { + const Eigen::Array3i miss_cell = + origin_cell + delta * position / num_samples; + hybrid_grid->ApplyLookupTable(miss_cell, miss_table); + } + } +} + +void InsertIntensitiesIntoGrid(const sensor::PointCloud& returns, + IntensityHybridGrid* intensity_hybrid_grid, + const float intensity_threshold) { + if (returns.intensities().size() > 0) { + for (size_t i = 0; i < returns.size(); ++i) { + if (returns.intensities()[i] > intensity_threshold) { + continue; + } + const Eigen::Array3i hit_cell = + intensity_hybrid_grid->GetCellIndex(returns[i].position); + intensity_hybrid_grid->AddIntensity(hit_cell, returns.intensities()[i]); + } + } +} + +} // namespace + +proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::RangeDataInserterOptions3D options; + options.set_hit_probability( + parameter_dictionary->GetDouble("hit_probability")); + options.set_miss_probability( + parameter_dictionary->GetDouble("miss_probability")); + options.set_num_free_space_voxels( + parameter_dictionary->GetInt("num_free_space_voxels")); + options.set_intensity_threshold( + parameter_dictionary->GetDouble("intensity_threshold")); + CHECK_GT(options.hit_probability(), 0.5); + CHECK_LT(options.miss_probability(), 0.5); + return options; +} + +RangeDataInserter3D::RangeDataInserter3D( + const proto::RangeDataInserterOptions3D& options) + : options_(options), + hit_table_( + ComputeLookupTableToApplyOdds(Odds(options_.hit_probability()))), + miss_table_( + ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {} + +void RangeDataInserter3D::Insert( + const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const { + CHECK_NOTNULL(hybrid_grid); + + for (const sensor::RangefinderPoint& hit : range_data.returns) { + const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); + hybrid_grid->ApplyLookupTable(hit_cell, hit_table_); + } + + // By not starting a new update after hits are inserted, we give hits priority + // (i.e. no hits will be ignored because of a miss in the same cell). + InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns, + hybrid_grid, options_.num_free_space_voxels()); + if (intensity_hybrid_grid != nullptr) { + InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid, + options_.intensity_threshold()); + } + hybrid_grid->FinishUpdate(); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d.h new file mode 100644 index 0000000..0dd5a8e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/proto/range_data_inserter_options_3d.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( + common::LuaParameterDictionary* parameter_dictionary); + +class RangeDataInserter3D { + public: + explicit RangeDataInserter3D( + const proto::RangeDataInserterOptions3D& options); + + RangeDataInserter3D(const RangeDataInserter3D&) = delete; + RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete; + + // Inserts 'range_data' into 'hybrid_grid' and optionally into + // 'intensity_hybrid_grid'. + void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const; + + private: + const proto::RangeDataInserterOptions3D options_; + const std::vector hit_table_; + const std::vector miss_table_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d_test.cc new file mode 100644 index 0000000..a7f92b3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -0,0 +1,154 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/3d/range_data_inserter_3d.h" + +#include +#include + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +class RangeDataInserter3DTest : public ::testing::Test { + protected: + RangeDataInserter3DTest() : hybrid_grid_(1.f), intensity_hybrid_grid_(1.f) { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "num_free_space_voxels = 1000, " + "intensity_threshold = 100, " + "}"); + options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get()); + range_data_inserter_.reset(new RangeDataInserter3D(options_)); + } + + void InsertPointCloud() { + const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); + const std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns), {}}, + &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); + } + + void InsertPointCloudWithIntensities() { + const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); + const std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + const std::vector intensities{7.f, 8.f, 9.f, 10.f}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}}, + &hybrid_grid_, &intensity_hybrid_grid_); + } + + float GetProbability(float x, float y, float z) const { + return hybrid_grid_.GetProbability( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + float GetIntensity(float x, float y, float z) const { + return intensity_hybrid_grid_.GetIntensity( + intensity_hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + float IsKnown(float x, float y, float z) const { + return hybrid_grid_.IsKnown( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + const proto::RangeDataInserterOptions3D& options() const { return options_; } + + private: + HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; + std::unique_ptr range_data_inserter_; + proto::RangeDataInserterOptions3D options_; +}; + +TEST_F(RangeDataInserter3DTest, InsertPointCloud) { + InsertPointCloud(); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f), + 1e-4); + for (int x = -4; x <= 4; ++x) { + for (int y = -4; y <= 4; ++y) { + if (x < -3 || x > 0 || y != x + 2) { + EXPECT_FALSE(IsKnown(x, y, 4.f)); + } else { + EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f), + 1e-4); + } + } + } +} + +TEST_F(RangeDataInserter3DTest, InsertPointCloudWithIntensities) { + InsertPointCloudWithIntensities(); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f), + 1e-4); + for (int x = -4; x <= 4; ++x) { + for (int y = -4; y <= 4; ++y) { + if (x < -3 || x > 0 || y != x + 2) { + EXPECT_FALSE(IsKnown(x, y, 4.f)); + EXPECT_NEAR(0.f, GetIntensity(x, y, 4.f), 1e-6); + } else { + EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f), + 1e-4); + EXPECT_NEAR(10 + x, GetIntensity(x, y, 4.f), 1e-6); + } + } + } +} + +TEST_F(RangeDataInserter3DTest, ProbabilityProgression) { + InsertPointCloud(); + EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(-2.f, 0.f, 3.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), + 1e-4); + + for (int i = 0; i < 1000; ++i) { + InsertPointCloud(); + } + EXPECT_NEAR(kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3); + EXPECT_NEAR(kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3); + EXPECT_NEAR(kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d.cc new file mode 100644 index 0000000..9facbe4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d.cc @@ -0,0 +1,354 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/3d/submap_3d.h" + +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/sensor/range_data.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace { + +struct PixelData { + int min_z = INT_MAX; + int max_z = INT_MIN; + int count = 0; + float probability_sum = 0.f; + float max_probability = 0.5f; +}; + +// Filters 'range_data', retaining only the returns that have no more than +// 'max_range' distance from the origin. Removes misses. +sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data, + const float max_range) { + sensor::RangeData result{range_data.origin, {}, {}}; + result.returns = + range_data.returns.copy_if([&](const sensor::RangefinderPoint& point) { + return (point.position - range_data.origin).norm() <= max_range; + }); + return result; +} + +std::vector AccumulatePixelData( + const int width, const int height, const Eigen::Array2i& min_index, + const Eigen::Array2i& max_index, + const std::vector& voxel_indices_and_probabilities) { + std::vector accumulated_pixel_data(width * height); + for (const Eigen::Array4i& voxel_index_and_probability : + voxel_indices_and_probabilities) { + const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>(); + if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) { + // Out of bounds. This could happen because of floating point inaccuracy. + continue; + } + const int x = max_index.x() - pixel_index[0]; + const int y = max_index.y() - pixel_index[1]; + PixelData& pixel = accumulated_pixel_data[x * width + y]; + ++pixel.count; + pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]); + pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]); + const float probability = + ValueToProbability(voxel_index_and_probability[3]); + pixel.probability_sum += probability; + pixel.max_probability = std::max(pixel.max_probability, probability); + } + return accumulated_pixel_data; +} + +// The first three entries of each returned value are a cell_index and the +// last is the corresponding probability value. We batch them together like +// this to only have one vector and have better cache locality. +std::vector ExtractVoxelData( + const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, + Eigen::Array2i* min_index, Eigen::Array2i* max_index) { + std::vector voxel_indices_and_probabilities; + const float resolution_inverse = 1.f / hybrid_grid.resolution(); + + constexpr float kXrayObstructedCellProbabilityLimit = 0.501f; + for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { + const uint16 probability_value = it.GetValue(); + const float probability = ValueToProbability(probability_value); + if (probability < kXrayObstructedCellProbabilityLimit) { + // We ignore non-obstructed cells. + continue; + } + + const Eigen::Vector3f cell_center_submap = + hybrid_grid.GetCenterOfCell(it.GetCellIndex()); + const Eigen::Vector3f cell_center_global = transform * cell_center_submap; + const Eigen::Array4i voxel_index_and_probability( + common::RoundToInt(cell_center_global.x() * resolution_inverse), + common::RoundToInt(cell_center_global.y() * resolution_inverse), + common::RoundToInt(cell_center_global.z() * resolution_inverse), + probability_value); + + voxel_indices_and_probabilities.push_back(voxel_index_and_probability); + const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>(); + *min_index = min_index->cwiseMin(pixel_index); + *max_index = max_index->cwiseMax(pixel_index); + } + return voxel_indices_and_probabilities; +} + +// Builds texture data containing interleaved value and alpha for the +// visualization from 'accumulated_pixel_data'. +std::string ComputePixelValues( + const std::vector& accumulated_pixel_data) { + std::string cell_data; + cell_data.reserve(2 * accumulated_pixel_data.size()); + constexpr float kMinZDifference = 3.f; + constexpr float kFreeSpaceWeight = 0.15f; + for (const PixelData& pixel : accumulated_pixel_data) { + // TODO(whess): Take into account submap rotation. + // TODO(whess): Document the approach and make it more independent from the + // chosen resolution. + const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0; + if (z_difference < kMinZDifference) { + cell_data.push_back(0); // value + cell_data.push_back(0); // alpha + continue; + } + const float free_space = std::max(z_difference - pixel.count, 0.f); + const float free_space_weight = kFreeSpaceWeight * free_space; + const float total_weight = pixel.count + free_space_weight; + const float free_space_probability = 1.f - pixel.max_probability; + const float average_probability = ClampProbability( + (pixel.probability_sum + free_space_probability * free_space_weight) / + total_weight); + const int delta = 128 - ProbabilityToLogOddsInteger(average_probability); + const uint8 alpha = delta > 0 ? 0 : -delta; + const uint8 value = delta > 0 ? delta : 0; + cell_data.push_back(value); // value + cell_data.push_back((value || alpha) ? alpha : 1); // alpha + } + return cell_data; +} + +void AddToTextureProto( + const HybridGrid& hybrid_grid, const transform::Rigid3d& global_submap_pose, + proto::SubmapQuery::Response::SubmapTexture* const texture) { + // Generate an X-ray view through the 'hybrid_grid', aligned to the + // xy-plane in the global map frame. + const float resolution = hybrid_grid.resolution(); + texture->set_resolution(resolution); + + // Compute a bounding box for the texture. + Eigen::Array2i min_index(INT_MAX, INT_MAX); + Eigen::Array2i max_index(INT_MIN, INT_MIN); + const std::vector voxel_indices_and_probabilities = + ExtractVoxelData(hybrid_grid, global_submap_pose.cast(), + &min_index, &max_index); + + const int width = max_index.y() - min_index.y() + 1; + const int height = max_index.x() - min_index.x() + 1; + texture->set_width(width); + texture->set_height(height); + + const std::vector accumulated_pixel_data = AccumulatePixelData( + width, height, min_index, max_index, voxel_indices_and_probabilities); + const std::string cell_data = ComputePixelValues(accumulated_pixel_data); + + common::FastGzipString(cell_data, texture->mutable_cells()); + *texture->mutable_slice_pose() = transform::ToProto( + global_submap_pose.inverse() * + transform::Rigid3d::Translation(Eigen::Vector3d( + max_index.x() * resolution, max_index.y() * resolution, + global_submap_pose.translation().z()))); +} + +} // namespace + +proto::SubmapsOptions3D CreateSubmapsOptions3D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::SubmapsOptions3D options; + options.set_high_resolution( + parameter_dictionary->GetDouble("high_resolution")); + options.set_high_resolution_max_range( + parameter_dictionary->GetDouble("high_resolution_max_range")); + options.set_low_resolution(parameter_dictionary->GetDouble("low_resolution")); + options.set_num_range_data( + parameter_dictionary->GetNonNegativeInt("num_range_data")); + *options.mutable_range_data_inserter_options() = + CreateRangeDataInserterOptions3D( + parameter_dictionary->GetDictionary("range_data_inserter").get()); + CHECK_GT(options.num_range_data(), 0); + return options; +} + +Submap3D::Submap3D(const float high_resolution, const float low_resolution, + const transform::Rigid3d& local_submap_pose, + const Eigen::VectorXf& rotational_scan_matcher_histogram) + : Submap(local_submap_pose), + high_resolution_hybrid_grid_( + absl::make_unique(high_resolution)), + low_resolution_hybrid_grid_( + absl::make_unique(low_resolution)), + high_resolution_intensity_hybrid_grid_( + absl::make_unique(high_resolution)), + rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {} + +Submap3D::Submap3D(const proto::Submap3D& proto) + : Submap(transform::ToRigid3(proto.local_pose())) { + UpdateFromProto(proto); +} + +proto::Submap Submap3D::ToProto( + const bool include_probability_grid_data) const { + proto::Submap proto; + auto* const submap_3d = proto.mutable_submap_3d(); + *submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); + submap_3d->set_num_range_data(num_range_data()); + submap_3d->set_finished(insertion_finished()); + if (include_probability_grid_data) { + *submap_3d->mutable_high_resolution_hybrid_grid() = + high_resolution_hybrid_grid().ToProto(); + *submap_3d->mutable_low_resolution_hybrid_grid() = + low_resolution_hybrid_grid().ToProto(); + } + for (Eigen::VectorXf::Index i = 0; + i != rotational_scan_matcher_histogram_.size(); ++i) { + submap_3d->add_rotational_scan_matcher_histogram( + rotational_scan_matcher_histogram_(i)); + } + return proto; +} + +void Submap3D::UpdateFromProto(const proto::Submap& proto) { + CHECK(proto.has_submap_3d()); + UpdateFromProto(proto.submap_3d()); +} + +void Submap3D::UpdateFromProto(const proto::Submap3D& submap_3d) { + set_num_range_data(submap_3d.num_range_data()); + set_insertion_finished(submap_3d.finished()); + if (submap_3d.has_high_resolution_hybrid_grid()) { + high_resolution_hybrid_grid_ = + absl::make_unique(submap_3d.high_resolution_hybrid_grid()); + } + if (submap_3d.has_low_resolution_hybrid_grid()) { + low_resolution_hybrid_grid_ = + absl::make_unique(submap_3d.low_resolution_hybrid_grid()); + } + rotational_scan_matcher_histogram_ = + Eigen::VectorXf::Zero(submap_3d.rotational_scan_matcher_histogram_size()); + for (Eigen::VectorXf::Index i = 0; + i != submap_3d.rotational_scan_matcher_histogram_size(); ++i) { + rotational_scan_matcher_histogram_(i) = + submap_3d.rotational_scan_matcher_histogram(i); + } +} + +void Submap3D::ToResponseProto( + const transform::Rigid3d& global_submap_pose, + proto::SubmapQuery::Response* const response) const { + response->set_submap_version(num_range_data()); + + AddToTextureProto(*high_resolution_hybrid_grid_, global_submap_pose, + response->add_textures()); + AddToTextureProto(*low_resolution_hybrid_grid_, global_submap_pose, + response->add_textures()); +} + +void Submap3D::InsertData(const sensor::RangeData& range_data_in_local, + const RangeDataInserter3D& range_data_inserter, + const float high_resolution_max_range, + const Eigen::Quaterniond& local_from_gravity_aligned, + const Eigen::VectorXf& scan_histogram_in_gravity) { + CHECK(!insertion_finished()); + // Transform range data into submap frame. + const sensor::RangeData transformed_range_data = sensor::TransformRangeData( + range_data_in_local, local_pose().inverse().cast()); + range_data_inserter.Insert( + FilterRangeDataByMaxRange(transformed_range_data, + high_resolution_max_range), + high_resolution_hybrid_grid_.get(), + high_resolution_intensity_hybrid_grid_.get()); + range_data_inserter.Insert(transformed_range_data, + low_resolution_hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); + set_num_range_data(num_range_data() + 1); + const float yaw_in_submap_from_gravity = transform::GetYaw( + local_pose().inverse().rotation() * local_from_gravity_aligned); + rotational_scan_matcher_histogram_ += + scan_matching::RotationalScanMatcher::RotateHistogram( + scan_histogram_in_gravity, yaw_in_submap_from_gravity); +} + +void Submap3D::Finish() { + CHECK(!insertion_finished()); + set_insertion_finished(true); +} + +ActiveSubmaps3D::ActiveSubmaps3D(const proto::SubmapsOptions3D& options) + : options_(options), + range_data_inserter_(options.range_data_inserter_options()) {} + +std::vector> ActiveSubmaps3D::submaps() const { + return std::vector>(submaps_.begin(), + submaps_.end()); +} + +std::vector> ActiveSubmaps3D::InsertData( + const sensor::RangeData& range_data, + const Eigen::Quaterniond& local_from_gravity_aligned, + const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity) { + if (submaps_.empty() || + submaps_.back()->num_range_data() == options_.num_range_data()) { + AddSubmap(transform::Rigid3d(range_data.origin.cast(), + local_from_gravity_aligned), + rotational_scan_matcher_histogram_in_gravity.size()); + } + for (auto& submap : submaps_) { + submap->InsertData(range_data, range_data_inserter_, + options_.high_resolution_max_range(), + local_from_gravity_aligned, + rotational_scan_matcher_histogram_in_gravity); + } + if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { + submaps_.front()->Finish(); + } + return submaps(); +} + +void ActiveSubmaps3D::AddSubmap( + const transform::Rigid3d& local_submap_pose, + const int rotational_scan_matcher_histogram_size) { + if (submaps_.size() >= 2) { + // This will crop the finished Submap before inserting a new Submap to + // reduce peak memory usage a bit. + CHECK(submaps_.front()->insertion_finished()); + // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use + // active submaps and their associated intensity hybrid grids for scan + // matching, we call `ForgetIntensityHybridGrid` once we remove the submap + // from active submaps and no longer need the intensity hybrid grid. + submaps_.front()->ForgetIntensityHybridGrid(); + submaps_.erase(submaps_.begin()); + } + const Eigen::VectorXf initial_rotational_scan_matcher_histogram = + Eigen::VectorXf::Zero(rotational_scan_matcher_histogram_size); + submaps_.emplace_back(new Submap3D( + options_.high_resolution(), options_.low_resolution(), local_submap_pose, + initial_rotational_scan_matcher_histogram)); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d.h new file mode 100644 index 0000000..e5d7e8b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_ + +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/port.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/3d/range_data_inserter_3d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_3d.pb.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +proto::SubmapsOptions3D CreateSubmapsOptions3D( + common::LuaParameterDictionary* parameter_dictionary); + +class Submap3D : public Submap { + public: + Submap3D(float high_resolution, float low_resolution, + const transform::Rigid3d& local_submap_pose, + const Eigen::VectorXf& rotational_scan_matcher_histogram); + + explicit Submap3D(const proto::Submap3D& proto); + + proto::Submap ToProto(bool include_probability_grid_data) const override; + void UpdateFromProto(const proto::Submap& proto) override; + + void ToResponseProto(const transform::Rigid3d& global_submap_pose, + proto::SubmapQuery::Response* response) const override; + + const HybridGrid& high_resolution_hybrid_grid() const { + return *high_resolution_hybrid_grid_; + } + const HybridGrid& low_resolution_hybrid_grid() const { + return *low_resolution_hybrid_grid_; + } + const IntensityHybridGrid& high_resolution_intensity_hybrid_grid() const { + CHECK(high_resolution_intensity_hybrid_grid_ != nullptr); + return *high_resolution_intensity_hybrid_grid_; + } + void ForgetIntensityHybridGrid() { + high_resolution_intensity_hybrid_grid_.reset(); + } + + const Eigen::VectorXf& rotational_scan_matcher_histogram() const { + return rotational_scan_matcher_histogram_; + } + + // Insert 'range_data' into this submap using 'range_data_inserter'. The + // submap must not be finished yet. + void InsertData(const sensor::RangeData& range_data, + const RangeDataInserter3D& range_data_inserter, + float high_resolution_max_range, + const Eigen::Quaterniond& local_from_gravity_aligned, + const Eigen::VectorXf& scan_histogram_in_gravity); + + void Finish(); + + private: + void UpdateFromProto(const proto::Submap3D& submap_3d); + + std::unique_ptr high_resolution_hybrid_grid_; + std::unique_ptr low_resolution_hybrid_grid_; + std::unique_ptr high_resolution_intensity_hybrid_grid_; + Eigen::VectorXf rotational_scan_matcher_histogram_; +}; + +// The first active submap will be created on the insertion of the first range +// data. Except during this initialization when no or only one single submap +// exists, there are always two submaps into which range data is 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 range data 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 created. The "old" submap is forgotten by this object. +class ActiveSubmaps3D { + public: + explicit ActiveSubmaps3D(const proto::SubmapsOptions3D& options); + + ActiveSubmaps3D(const ActiveSubmaps3D&) = delete; + ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete; + + // Inserts 'range_data_in_local' into the Submap collection. + // 'local_from_gravity_aligned' is used for the orientation of new submaps so + // that the z axis approximately aligns with gravity. + // 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all + // submaps of the Submap collection. + std::vector> InsertData( + const sensor::RangeData& range_data_in_local, + const Eigen::Quaterniond& local_from_gravity_aligned, + const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity); + + std::vector> submaps() const; + + private: + void AddSubmap(const transform::Rigid3d& local_submap_pose, + int rotational_scan_matcher_histogram_size); + + const proto::SubmapsOptions3D options_; + std::vector> submaps_; + RangeDataInserter3D range_data_inserter_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d_test.cc new file mode 100644 index 0000000..3a0d084 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/3d/submap_3d_test.cc @@ -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. + */ + +#include "cartographer/mapping/3d/submap_3d.h" + +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(SubmapsTest, ToFromProto) { + Eigen::VectorXf histogram(2); + histogram << 1.0f, 2.0f; + const Submap3D expected( + 0.05, 0.25, + transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), + Eigen::Quaterniond(0., 0., 0., 1.)), + histogram); + const proto::Submap proto = + expected.ToProto(true /* include_probability_grid_data */); + EXPECT_FALSE(proto.has_submap_2d()); + EXPECT_TRUE(proto.has_submap_3d()); + const auto actual = Submap3D(proto.submap_3d()); + EXPECT_TRUE(expected.local_pose().translation().isApprox( + actual.local_pose().translation(), 1e-6)); + EXPECT_TRUE(expected.local_pose().rotation().isApprox( + actual.local_pose().rotation(), 1e-6)); + EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); + EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished()); + EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6); + EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6); + EXPECT_TRUE(expected.rotational_scan_matcher_histogram().isApprox( + actual.rotational_scan_matcher_histogram(), 1e-6)); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/detect_floors.cc b/carto_ws/src/cartographer-master/cartographer/mapping/detect_floors.cc new file mode 100644 index 0000000..8ec605d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/detect_floors.cc @@ -0,0 +1,219 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/detect_floors.h" + +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +namespace { + +// A union-find structure for assigning levels to 'spans' in the trajectory +// implemented as a disjoint-set forest: +// https://en.wikipedia.org/wiki/Disjoint-set_data_structure#Disjoint-set_forests +// TODO(hrapp): We use this elsewhere. Pull out into a common place. +using Levels = std::map; + +constexpr double kMaxShortSpanLengthMeters = 25.; +constexpr double kLevelHeightMeters = 2.5; +constexpr double kMinLevelSeparationMeters = 1.; + +// Indices into 'trajectory.node', so that 'start_index' <= i < 'end_index'. +struct Span { + int start_index; + int end_index; + std::vector z_values; + + bool operator<(const Span& other) const { + return std::forward_as_tuple(start_index, end_index) < + std::forward_as_tuple(other.start_index, other.end_index); + } +}; + +// Union-find implementation for classifying spans into levels. +int LevelFind(const int i, const Levels& levels) { + auto it = levels.find(i); + CHECK(it != levels.end()); + if (it->first == it->second) { + return it->second; + } + return LevelFind(it->second, levels); +} + +void LevelUnion(int i, int j, Levels* levels) { + const int repr_i = LevelFind(i, *levels); + const int repr_j = LevelFind(j, *levels); + (*levels)[repr_i] = repr_j; +} + +void InsertSorted(const double val, std::vector* vals) { + vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val); +} + +double Median(const std::vector& sorted) { + CHECK(!sorted.empty()); + return sorted.at(sorted.size() / 2); +} + +// Cut the trajectory at jumps in z. A new span is started when the current +// node's z differes by more than kLevelHeightMeters from the median z values. +std::vector SliceByAltitudeChange(const proto::Trajectory& trajectory) { + CHECK_GT(trajectory.node_size(), 0); + + std::vector spans; + spans.push_back(Span{0, 0, {trajectory.node(0).pose().translation().z()}}); + for (int i = 1; i < trajectory.node_size(); ++i) { + const auto& node = trajectory.node(i); + const double z = node.pose().translation().z(); + if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) { + spans.push_back(Span{i, i, {}}); + } + InsertSorted(z, &spans.back().z_values); + spans.back().end_index = i + 1; + } + return spans; +} + +// Returns the length of 'span' in meters. +double SpanLength(const proto::Trajectory& trajectory, const Span& span) { + double length = 0; + for (int i = span.start_index + 1; i < span.end_index; ++i) { + const auto a = + transform::ToEigen(trajectory.node(i - 1).pose().translation()); + const auto b = transform::ToEigen(trajectory.node(i).pose().translation()); + length += (a - b).head<2>().norm(); + } + return length; +} + +// True if 'span' is considered to be short, i.e. not interesting on its own, +// but should be folded into the levels before and after entering it. +bool IsShort(const proto::Trajectory& trajectory, const Span& span) { + return SpanLength(trajectory, span) < kMaxShortSpanLengthMeters; +} + +// Merges all 'spans' that have similar median z value into the same level. +void GroupSegmentsByAltitude(const proto::Trajectory& trajectory, + const std::vector& spans, Levels* levels) { + for (size_t i = 0; i < spans.size(); ++i) { + for (size_t j = i + 1; j < spans.size(); ++j) { + if (std::abs(Median(spans[i].z_values) - Median(spans[j].z_values)) < + kMinLevelSeparationMeters) { + LevelUnion(i, j, levels); + } + } + } +} + +std::vector FindFloors(const proto::Trajectory& trajectory, + const std::vector& spans, + const Levels& levels) { + std::map> level_spans; + + // Initialize the levels to start out with only long spans. + for (size_t i = 0; i < spans.size(); ++i) { + const Span& span = spans[i]; + if (!IsShort(trajectory, span)) { + level_spans[LevelFind(i, levels)].push_back(span); + } + } + + for (size_t i = 0; i < spans.size(); ++i) { + const Span& span = spans[i]; + if (!IsShort(trajectory, span)) { + continue; + } + + // If we have a long piece on this floor already, merge this short piece + // into it. + int level = LevelFind(i, levels); + if (!level_spans[level].empty()) { + level_spans[level].push_back(span); + continue; + } + + // Otherwise, add this short piece to the level before and after it. It is + // likely some intermediate level on stairs. + size_t index = i - 1; + if (index < spans.size()) { + level_spans[LevelFind(index, levels)].push_back(span); + } + index = i + 1; + if (index < spans.size()) { + level_spans[LevelFind(index, levels)].push_back(span); + } + } + + // Convert the level_spans structure to 'Floor'. + std::vector floors; + for (auto& level : level_spans) { + if (level.second.empty()) { + continue; + } + + std::vector z_values; + std::sort(level.second.begin(), level.second.end()); + floors.emplace_back(); + for (const auto& span : level.second) { + if (!IsShort(trajectory, span)) { + // To figure out the median height of this floor, we only care for the + // long pieces that are guaranteed to be in the structure. This is a + // heuristic to leave out intermediate (short) levels. + z_values.insert(z_values.end(), span.z_values.begin(), + span.z_values.end()); + } + floors.back().timespans.push_back(Timespan{ + common::FromUniversal(trajectory.node(span.start_index).timestamp()), + common::FromUniversal( + trajectory.node(span.end_index - 1).timestamp())}); + } + if (!z_values.empty()) { + std::sort(z_values.begin(), z_values.end()); + floors.back().z = Median(z_values); + } else { + LOG(ERROR) << "All spans in level are short"; + floors.pop_back(); + } + } + return floors; +} + +} // namespace + +std::vector DetectFloors(const proto::Trajectory& trajectory) { + const std::vector spans = SliceByAltitudeChange(trajectory); + Levels levels; + for (size_t i = 0; i < spans.size(); ++i) { + levels[i] = i; + } + GroupSegmentsByAltitude(trajectory, spans, &levels); + + std::vector floors = FindFloors(trajectory, spans, levels); + std::sort(floors.begin(), floors.end(), + [](const Floor& a, const Floor& b) { return a.z < b.z; }); + return floors; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/detect_floors.h b/carto_ws/src/cartographer-master/cartographer/mapping/detect_floors.h new file mode 100644 index 0000000..b5acde8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/detect_floors.h @@ -0,0 +1,50 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ +#define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ + +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory.pb.h" + +namespace cartographer { +namespace mapping { + +struct Timespan { + common::Time start; + common::Time end; +}; + +struct Floor { + // The spans of time we spent on this floor. Since we might have walked up and + // down many times in this place, there can be many spans of time we spent on + // a particular floor. + std::vector timespans; + + // The median z-value of this floor. + double z; +}; + +// Uses a heuristic which looks at z-values of the poses to detect individual +// floors of a building. This requires that floors are *mostly* on the same +// z-height and that level changes happen *relatively* abrubtly, e.g. by taking +// the stairs. +std::vector DetectFloors(const proto::Trajectory& trajectory); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/grid_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/grid_interface.h new file mode 100644 index 0000000..b5825a6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/grid_interface.h @@ -0,0 +1,35 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_GRID_H_ +#define CARTOGRAPHER_MAPPING_GRID_H_ + +#include +#include + +namespace cartographer { +namespace mapping { + +class GridInterface { + // todo(kdaun) move mutual functions of Grid2D/3D here + public: + virtual ~GridInterface() {} +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_GRID_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/id.h b/carto_ws/src/cartographer-master/cartographer/mapping/id.h new file mode 100644 index 0000000..c5abdf9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/id.h @@ -0,0 +1,423 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ID_H_ +#define CARTOGRAPHER_MAPPING_ID_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace internal { + +template +auto GetTimeImpl(const T& t, int) -> decltype(t.time()) { + return t.time(); +} +template +auto GetTimeImpl(const T& t, unsigned) -> decltype(t.time) { + return t.time; +} +template +common::Time GetTime(const T& t) { + return GetTimeImpl(t, 0); +} + +} // namespace internal + +// Uniquely identifies a trajectory node using a combination of a unique +// trajectory ID and a zero-based index of the node inside that trajectory. +struct NodeId { + NodeId(int trajectory_id, int node_index) + : trajectory_id(trajectory_id), node_index(node_index) {} + + int trajectory_id; + int node_index; + + bool operator==(const NodeId& other) const { + return std::forward_as_tuple(trajectory_id, node_index) == + std::forward_as_tuple(other.trajectory_id, other.node_index); + } + + bool operator!=(const NodeId& other) const { return !operator==(other); } + + bool operator<(const NodeId& other) const { + return std::forward_as_tuple(trajectory_id, node_index) < + std::forward_as_tuple(other.trajectory_id, other.node_index); + } + + void ToProto(proto::NodeId* proto) const { + proto->set_trajectory_id(trajectory_id); + proto->set_node_index(node_index); + } +}; + +inline std::ostream& operator<<(std::ostream& os, const NodeId& v) { + return os << "(" << v.trajectory_id << ", " << v.node_index << ")"; +} + +// Uniquely identifies a submap using a combination of a unique trajectory ID +// and a zero-based index of the submap inside that trajectory. +struct SubmapId { + SubmapId(int trajectory_id, int submap_index) + : trajectory_id(trajectory_id), submap_index(submap_index) {} + + int trajectory_id; + int submap_index; + + bool operator==(const SubmapId& other) const { + return std::forward_as_tuple(trajectory_id, submap_index) == + std::forward_as_tuple(other.trajectory_id, other.submap_index); + } + + bool operator!=(const SubmapId& other) const { return !operator==(other); } + + bool operator<(const SubmapId& other) const { + return std::forward_as_tuple(trajectory_id, submap_index) < + std::forward_as_tuple(other.trajectory_id, other.submap_index); + } + + void ToProto(proto::SubmapId* proto) const { + proto->set_trajectory_id(trajectory_id); + proto->set_submap_index(submap_index); + } +}; + +inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { + return os << "(" << v.trajectory_id << ", " << v.submap_index << ")"; +} + +template +class Range { + public: + Range(const IteratorType& begin, const IteratorType& end) + : begin_(begin), end_(end) {} + + IteratorType begin() const { return begin_; } + IteratorType end() const { return end_; } + + private: + IteratorType begin_; + IteratorType end_; +}; + +// Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or +// 'SubmapId'. +// Note: This container will only ever contain non-empty trajectories. Trimming +// the last remaining node of a trajectory drops the trajectory. +template +class MapById { + private: + struct MapByIndex; + + public: + struct IdDataReference { + IdType id; + const DataType& data; + }; + + class ConstIterator { + public: + using iterator_category = std::bidirectional_iterator_tag; + using value_type = IdDataReference; + using difference_type = int64; + using pointer = std::unique_ptr; + using reference = const IdDataReference&; + + explicit ConstIterator(const MapById& map_by_id, const int trajectory_id) + : current_trajectory_( + map_by_id.trajectories_.lower_bound(trajectory_id)), + end_trajectory_(map_by_id.trajectories_.end()) { + if (current_trajectory_ != end_trajectory_) { + current_data_ = current_trajectory_->second.data_.begin(); + AdvanceToValidDataIterator(); + } + } + + explicit ConstIterator(const MapById& map_by_id, const IdType& id) + : current_trajectory_(map_by_id.trajectories_.find(id.trajectory_id)), + end_trajectory_(map_by_id.trajectories_.end()) { + if (current_trajectory_ != end_trajectory_) { + current_data_ = + current_trajectory_->second.data_.find(MapById::GetIndex(id)); + if (current_data_ == current_trajectory_->second.data_.end()) { + current_trajectory_ = end_trajectory_; + } + } + } + + IdDataReference operator*() const { + CHECK(current_trajectory_ != end_trajectory_); + return IdDataReference{ + IdType{current_trajectory_->first, current_data_->first}, + current_data_->second}; + } + + std::unique_ptr operator->() const { + return absl::make_unique(this->operator*()); + } + + ConstIterator& operator++() { + CHECK(current_trajectory_ != end_trajectory_); + ++current_data_; + AdvanceToValidDataIterator(); + return *this; + } + + ConstIterator& operator--() { + while (current_trajectory_ == end_trajectory_ || + current_data_ == current_trajectory_->second.data_.begin()) { + --current_trajectory_; + current_data_ = current_trajectory_->second.data_.end(); + } + --current_data_; + return *this; + } + + bool operator==(const ConstIterator& it) const { + if (current_trajectory_ == end_trajectory_ || + it.current_trajectory_ == it.end_trajectory_) { + return current_trajectory_ == it.current_trajectory_; + } + return current_trajectory_ == it.current_trajectory_ && + current_data_ == it.current_data_; + } + + bool operator!=(const ConstIterator& it) const { return !operator==(it); } + + private: + void AdvanceToValidDataIterator() { + CHECK(current_trajectory_ != end_trajectory_); + while (current_data_ == current_trajectory_->second.data_.end()) { + ++current_trajectory_; + if (current_trajectory_ == end_trajectory_) { + return; + } + current_data_ = current_trajectory_->second.data_.begin(); + } + } + + typename std::map::const_iterator current_trajectory_; + typename std::map::const_iterator end_trajectory_; + typename std::map::const_iterator current_data_; + }; + + class ConstTrajectoryIterator { + public: + using iterator_category = std::bidirectional_iterator_tag; + using value_type = int; + using difference_type = int64; + using pointer = const int*; + using reference = const int&; + + explicit ConstTrajectoryIterator( + typename std::map::const_iterator current_trajectory) + : current_trajectory_(current_trajectory) {} + + int operator*() const { return current_trajectory_->first; } + + ConstTrajectoryIterator& operator++() { + ++current_trajectory_; + return *this; + } + + ConstTrajectoryIterator& operator--() { + --current_trajectory_; + return *this; + } + + bool operator==(const ConstTrajectoryIterator& it) const { + return current_trajectory_ == it.current_trajectory_; + } + + bool operator!=(const ConstTrajectoryIterator& it) const { + return !operator==(it); + } + + private: + typename std::map::const_iterator current_trajectory_; + }; + + // Appends data to a 'trajectory_id', creating trajectories as needed. + IdType Append(const int trajectory_id, const DataType& data) { + CHECK_GE(trajectory_id, 0); + auto& trajectory = trajectories_[trajectory_id]; + CHECK(trajectory.can_append_); + const int index = + trajectory.data_.empty() ? 0 : trajectory.data_.rbegin()->first + 1; + trajectory.data_.emplace(index, data); + return IdType{trajectory_id, index}; + } + + // Returns an iterator to the element at 'id' or the end iterator if it does + // not exist. + ConstIterator find(const IdType& id) const { + return ConstIterator(*this, id); + } + + // Inserts data (which must not exist already) into a trajectory. + void Insert(const IdType& id, const DataType& data) { + CHECK_GE(id.trajectory_id, 0); + CHECK_GE(GetIndex(id), 0); + auto& trajectory = trajectories_[id.trajectory_id]; + trajectory.can_append_ = false; + CHECK(trajectory.data_.emplace(GetIndex(id), data).second); + } + + // Removes the data for 'id' which must exist. + void Trim(const IdType& id) { + auto& trajectory = trajectories_.at(id.trajectory_id); + const auto it = trajectory.data_.find(GetIndex(id)); + CHECK(it != trajectory.data_.end()) << id; + if (std::next(it) == trajectory.data_.end()) { + // We are removing the data with the highest index from this trajectory. + // We assume that we will never append to it anymore. If we did, we would + // have to make sure that gaps in indices are properly chosen to maintain + // correct connectivity. + trajectory.can_append_ = false; + } + trajectory.data_.erase(it); + if (trajectory.data_.empty()) { + trajectories_.erase(id.trajectory_id); + } + } + + bool Contains(const IdType& id) const { + return trajectories_.count(id.trajectory_id) != 0 && + trajectories_.at(id.trajectory_id).data_.count(GetIndex(id)) != 0; + } + + const DataType& at(const IdType& id) const { + return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id)); + } + + DataType& at(const IdType& id) { + return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id)); + } + + // Support querying by trajectory. + ConstIterator BeginOfTrajectory(const int trajectory_id) const { + return ConstIterator(*this, trajectory_id); + } + ConstIterator EndOfTrajectory(const int trajectory_id) const { + return BeginOfTrajectory(trajectory_id + 1); + } + + // Returns 0 if 'trajectory_id' does not exist. + size_t SizeOfTrajectoryOrZero(const int trajectory_id) const { + return trajectories_.count(trajectory_id) + ? trajectories_.at(trajectory_id).data_.size() + : 0; + } + + // Returns count of all elements. + size_t size() const { + size_t size = 0; + for (const auto& item : trajectories_) { + size += item.second.data_.size(); + } + return size; + } + + // Returns Range object for range-based loops over the nodes of a trajectory. + Range trajectory(const int trajectory_id) const { + return Range(BeginOfTrajectory(trajectory_id), + EndOfTrajectory(trajectory_id)); + } + + // Returns Range object for range-based loops over the trajectory IDs. + Range trajectory_ids() const { + return Range( + ConstTrajectoryIterator(trajectories_.begin()), + ConstTrajectoryIterator(trajectories_.end())); + } + + ConstIterator begin() const { return BeginOfTrajectory(0); } + ConstIterator end() const { + return BeginOfTrajectory(std::numeric_limits::max()); + } + + bool empty() const { return begin() == end(); } + + // Returns an iterator to the first element in the container belonging to + // trajectory 'trajectory_id' whose time is not considered to go before + // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go + // before 'time'. + ConstIterator lower_bound(const int trajectory_id, + const common::Time time) const { + if (SizeOfTrajectoryOrZero(trajectory_id) == 0) { + return EndOfTrajectory(trajectory_id); + } + + const std::map& trajectory = + trajectories_.at(trajectory_id).data_; + + if (internal::GetTime(std::prev(trajectory.end())->second) < time) { + return EndOfTrajectory(trajectory_id); + } + + auto left = trajectory.begin(); + auto right = std::prev(trajectory.end()); + while (left != right) { + // This is never 'right' which is important to guarantee progress. + const int middle = left->first + (right->first - left->first) / 2; + // This could be 'right' in the presence of gaps, so we need to use the + // previous element in this case. + auto lower_bound_middle = trajectory.lower_bound(middle); + if (lower_bound_middle->first > middle) { + CHECK(lower_bound_middle != left); + lower_bound_middle = std::prev(lower_bound_middle); + } + if (internal::GetTime(lower_bound_middle->second) < time) { + left = std::next(lower_bound_middle); + } else { + right = lower_bound_middle; + } + } + + return ConstIterator(*this, IdType{trajectory_id, left->first}); + } + + private: + struct MapByIndex { + bool can_append_ = true; + std::map data_; + }; + + static int GetIndex(const NodeId& id) { return id.node_index; } + static int GetIndex(const SubmapId& id) { return id.submap_index; } + + std::map trajectories_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_ID_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/id_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/id_test.cc new file mode 100644 index 0000000..ab877e0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/id_test.cc @@ -0,0 +1,341 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/id.h" + +#include +#include +#include +#include +#include + +#include "cartographer/common/time.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +common::Time CreateTime(const int milliseconds) { + return common::Time(common::FromMilliseconds(milliseconds)); +} + +class Data { + public: + explicit Data(int milliseconds) : time_(CreateTime(milliseconds)) {} + + const common::Time& time() const { return time_; } + + private: + const common::Time time_; +}; + +template +static MapById CreateTestMapById() { + MapById map_by_id; + map_by_id.Append(7, 2); + map_by_id.Append(42, 3); + map_by_id.Append(0, 0); + map_by_id.Append(0, 1); + return map_by_id; +} + +TEST(IdTest, EmptyMapById) { + MapById map_by_id; + EXPECT_TRUE(map_by_id.empty()); + EXPECT_EQ(map_by_id.trajectory_ids().begin(), + map_by_id.trajectory_ids().end()); + int unknown_trajectory_id = 3; + EXPECT_EQ(map_by_id.trajectory(unknown_trajectory_id).begin(), + map_by_id.trajectory(unknown_trajectory_id).end()); + const NodeId id = map_by_id.Append(42, 42); + EXPECT_FALSE(map_by_id.empty()); + map_by_id.Trim(id); + EXPECT_TRUE(map_by_id.empty()); + EXPECT_EQ(0, map_by_id.size()); +} + +TEST(IdTest, DeleteTrajectory) { + MapById map_by_id; + int trajectory_id = 3; + int other_trajectory_id = 5; + map_by_id.Insert(NodeId{trajectory_id, 4}, 5); + map_by_id.Insert(NodeId{trajectory_id, 5}, 7); + map_by_id.Insert(NodeId{other_trajectory_id, 1}, 3); + EXPECT_EQ(map_by_id.size(), 3); + EXPECT_EQ(2, std::distance(map_by_id.trajectory_ids().begin(), + map_by_id.trajectory_ids().end())); + map_by_id.Trim(NodeId{trajectory_id, 4}); + map_by_id.Trim(NodeId{trajectory_id, 5}); + EXPECT_EQ(0, std::distance(map_by_id.trajectory(trajectory_id).begin(), + map_by_id.trajectory(trajectory_id).end())); + int invalid_trajectory_id = 2; + EXPECT_EQ(map_by_id.trajectory(invalid_trajectory_id).begin(), + map_by_id.trajectory(invalid_trajectory_id).end()); + EXPECT_EQ(map_by_id.size(), 1); + EXPECT_EQ(1, std::distance(map_by_id.trajectory(other_trajectory_id).begin(), + map_by_id.trajectory(other_trajectory_id).end())); + EXPECT_EQ(1, std::distance(map_by_id.trajectory_ids().begin(), + map_by_id.trajectory_ids().end())); + EXPECT_FALSE(map_by_id.empty()); +} + +TEST(IdTest, MapByIdIterator) { + MapById map_by_id = CreateTestMapById(); + EXPECT_EQ(4, map_by_id.size()); + EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data); + EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) == + map_by_id.EndOfTrajectory(7)); + std::deque> expected_id_data = { + {NodeId{0, 0}, 0}, + {NodeId{0, 1}, 1}, + {NodeId{7, 0}, 2}, + {NodeId{42, 0}, 3}, + }; + for (const auto& id_data : map_by_id) { + ASSERT_FALSE(expected_id_data.empty()); + EXPECT_EQ(expected_id_data.front().first, id_data.id); + EXPECT_EQ(expected_id_data.front().second, id_data.data); + expected_id_data.pop_front(); + } + EXPECT_TRUE(expected_id_data.empty()); +} + +TEST(IdTest, MapByIdTrajectoryRange) { + MapById map_by_id = CreateTestMapById(); + std::deque> expected_data = { + {NodeId{0, 0}, 0}, + {NodeId{0, 1}, 1}, + }; + for (const auto& entry : map_by_id.trajectory(0)) { + ASSERT_FALSE(expected_data.empty()); + EXPECT_EQ(expected_data.front().first, entry.id); + EXPECT_EQ(expected_data.front().second, entry.data); + expected_data.pop_front(); + } + EXPECT_TRUE(expected_data.empty()); +} + +TEST(IdTest, MapByIdTrajectoryIdRange) { + MapById map_by_id = CreateTestMapById(); + std::deque expected_data = {0, 7, 42}; + for (const int trajectory_id : map_by_id.trajectory_ids()) { + ASSERT_FALSE(expected_data.empty()); + EXPECT_EQ(expected_data.front(), trajectory_id); + expected_data.pop_front(); + } + EXPECT_TRUE(expected_data.empty()); +} + +TEST(IdTest, MapByIdIterateByTrajectories) { + MapById map_by_id = CreateTestMapById(); + std::deque> expected_id_data = { + {NodeId{0, 0}, 0}, + {NodeId{0, 1}, 1}, + {NodeId{7, 0}, 2}, + {NodeId{42, 0}, 3}, + }; + for (int trajectory_id : map_by_id.trajectory_ids()) { + for (const auto& entry : map_by_id.trajectory(trajectory_id)) { + ASSERT_FALSE(expected_id_data.empty()); + EXPECT_EQ(expected_id_data.front().first, entry.id); + EXPECT_EQ(expected_id_data.front().second, entry.data); + expected_id_data.pop_front(); + } + } + EXPECT_TRUE(expected_id_data.empty()); +} + +TEST(IdTest, MapByIdPrevIterator) { + MapById map_by_id; + map_by_id.Append(42, 42); + auto it = map_by_id.end(); + ASSERT_TRUE(it != map_by_id.begin()); + std::advance(it, -1); + EXPECT_TRUE(it == map_by_id.begin()); +} + +TEST(IdTest, InsertIntoMapById) { + MapById map_by_id; + EXPECT_EQ(0, map_by_id.SizeOfTrajectoryOrZero(42)); + map_by_id.Append(42, 42); + map_by_id.Insert(NodeId{42, 5}, 42); + EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42)); +} + +TEST(IdTest, FindNodeId) { + MapById map_by_id; + map_by_id.Append(42, 42); + map_by_id.Append(42, 43); + map_by_id.Append(42, 44); + CHECK_EQ(map_by_id.find(NodeId{42, 1})->data, 43); + EXPECT_TRUE(map_by_id.find(NodeId{41, 0}) == map_by_id.end()); + EXPECT_TRUE(map_by_id.find(NodeId{42, 3}) == map_by_id.end()); +} + +TEST(IdTest, FindSubmapId) { + MapById map_by_id; + map_by_id.Append(42, 42); + map_by_id.Append(42, 43); + map_by_id.Append(42, 44); + CHECK_EQ(map_by_id.find(SubmapId{42, 1})->data, 43); + EXPECT_TRUE(map_by_id.find(SubmapId{42, 3}) == map_by_id.end()); +} + +TEST(IdTest, LowerBoundEdgeCases) { + MapById map_by_id; + map_by_id.Append(0, Data(1)); + map_by_id.Append(2, Data(2)); + CHECK(map_by_id.lower_bound(1, CreateTime(10)) == + map_by_id.EndOfTrajectory(1)); + CHECK(map_by_id.lower_bound(2, CreateTime(3)) == + map_by_id.EndOfTrajectory(2)); + CHECK(map_by_id.lower_bound(2, CreateTime(1)) == + map_by_id.BeginOfTrajectory(2)); +} + +TEST(IdTest, LowerBound) { + MapById map_by_id; + map_by_id.Append(0, Data(1)); + map_by_id.Append(0, Data(2)); + map_by_id.Append(0, Data(4)); + map_by_id.Append(0, Data(5)); + CHECK(map_by_id.lower_bound(0, CreateTime(3)) == + (MapById::ConstIterator(map_by_id, SubmapId{0, 2}))); + CHECK(map_by_id.lower_bound(0, CreateTime(2)) == + (MapById::ConstIterator(map_by_id, SubmapId{0, 1}))); + CHECK(map_by_id.lower_bound(0, CreateTime(4)) == + (MapById::ConstIterator(map_by_id, SubmapId{0, 2}))); +} + +TEST(IdTest, LowerBoundFuzz) { + constexpr int kMaxTimeIncrement = 20; + constexpr int kMaxNumNodes = 20; + constexpr int kNumTests = 100; + constexpr int kTrajectoryId = 1; + + std::mt19937 rng; + std::uniform_int_distribution dt_dist(1, kMaxTimeIncrement); + std::uniform_int_distribution N_dist(1, kMaxNumNodes); + + for (int i = 0; i < kNumTests; ++i) { + const int N = N_dist(rng); + int t = 0; + MapById map_by_id; + for (int j = 0; j < N; ++j) { + t = t + dt_dist(rng); + map_by_id.Append(kTrajectoryId, Data(t)); + } + std::uniform_int_distribution t0_dist(1, N * kMaxTimeIncrement + 1); + int t0 = t0_dist(rng); + auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(t0)); + + auto ground_truth = std::lower_bound( + map_by_id.BeginOfTrajectory(kTrajectoryId), + map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(t0), + [](MapById::IdDataReference a, const common::Time& t) { + return a.data.time() < t; + }); + + CHECK(ground_truth == it); + } +} + +TEST(IdTest, LowerBoundTrimmedTrajectory) { + constexpr int kTrajectoryId = 1; + + std::mt19937 rng; + std::uniform_int_distribution dt_dist(1, 20); + + const int N = 500; + int t = 0; + MapById map_by_id; + for (int j = 0; j < N; ++j) { + t = t + dt_dist(rng); + map_by_id.Append(kTrajectoryId, Data(t)); + } + + // Choose random length of a trim segment. + std::uniform_int_distribution dt_trim_segment_length( + 1, static_cast(N / 2)); + size_t trim_segment_length = dt_trim_segment_length(rng); + // Choose random start for a trim_segment. + std::uniform_int_distribution dt_trim_segment_start( + 2, N - trim_segment_length - 1); + size_t trim_segment_start_index = dt_trim_segment_start(rng); + + auto trim_segment_start = map_by_id.begin(); + std::advance(trim_segment_start, trim_segment_start_index); + + auto trim_segment_end = map_by_id.begin(); + std::advance(trim_segment_end, + trim_segment_start_index + trim_segment_length); + + for (auto it = trim_segment_start; it != trim_segment_end;) { + const auto this_it = it; + ++it; + map_by_id.Trim(this_it->id); + } + + auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(0)); + + auto ground_truth = + std::lower_bound(map_by_id.BeginOfTrajectory(kTrajectoryId), + map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(0), + [](MapById::IdDataReference a, + const common::Time& t) { return a.data.time() < t; }); + + EXPECT_EQ(ground_truth, it); +} + +struct DataStruct { + const common::Time time; +}; + +TEST(IdTest, LowerBoundFuzzWithStruct) { + constexpr int kMaxTimeIncrement = 20; + constexpr int kMaxNumNodes = 20; + constexpr int kNumTests = 100; + constexpr int kTrajectoryId = 1; + + std::mt19937 rng; + std::uniform_int_distribution dt_dist(1, kMaxTimeIncrement); + std::uniform_int_distribution N_dist(1, kMaxNumNodes); + + for (int i = 0; i < kNumTests; ++i) { + const int N = N_dist(rng); + int t = 0; + MapById map_by_id; + for (int j = 0; j < N; ++j) { + t = t + dt_dist(rng); + map_by_id.Append(kTrajectoryId, DataStruct{CreateTime(t)}); + } + std::uniform_int_distribution t0_dist(1, N * kMaxTimeIncrement + 1); + int t0 = t0_dist(rng); + auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(t0)); + + auto ground_truth = std::lower_bound( + map_by_id.BeginOfTrajectory(kTrajectoryId), + map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(t0), + [](MapById::IdDataReference a, + const common::Time& t) { return a.data.time < t; }); + + CHECK(ground_truth == it); + } +} +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker.cc b/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker.cc new file mode 100644 index 0000000..8aed29c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker.cc @@ -0,0 +1,77 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/imu_tracker.h" + +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +ImuTracker::ImuTracker(const double imu_gravity_time_constant, + const common::Time time) + : imu_gravity_time_constant_(imu_gravity_time_constant), + time_(time), + last_linear_acceleration_time_(common::Time::min()), + orientation_(Eigen::Quaterniond::Identity()), + gravity_vector_(Eigen::Vector3d::UnitZ()), + imu_angular_velocity_(Eigen::Vector3d::Zero()) {} + +void ImuTracker::Advance(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_vector_ = rotation.conjugate() * gravity_vector_; + time_ = time; +} + +void ImuTracker::AddImuLinearAccelerationObservation( + const Eigen::Vector3d& imu_linear_acceleration) { + // Update the 'gravity_vector_' with an exponential moving average using the + // 'imu_gravity_time_constant'. + const double delta_t = + last_linear_acceleration_time_ > common::Time::min() + ? common::ToSeconds(time_ - last_linear_acceleration_time_) + : std::numeric_limits::infinity(); + last_linear_acceleration_time_ = time_; + const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_); + gravity_vector_ = + (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; + // Change the 'orientation_' so that it agrees with the current + // 'gravity_vector_'. + const Eigen::Quaterniond rotation = FromTwoVectors( + gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); + orientation_ = (orientation_ * rotation).normalized(); + CHECK_GT((orientation_ * gravity_vector_).z(), 0.); + CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99); +} + +void ImuTracker::AddImuAngularVelocityObservation( + const Eigen::Vector3d& imu_angular_velocity) { + imu_angular_velocity_ = imu_angular_velocity; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker.h b/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker.h new file mode 100644 index 0000000..1f4fd1f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ +#define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ + +#include "Eigen/Geometry" +#include "cartographer/common/time.h" + +namespace cartographer { +namespace mapping { + +// 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(double imu_gravity_time_constant, common::Time time); + + // Advances to the given 'time' and updates the orientation to reflect this. + void Advance(common::Time time); + + // Updates from an IMU reading (in the IMU frame). + void AddImuLinearAccelerationObservation( + const Eigen::Vector3d& imu_linear_acceleration); + void AddImuAngularVelocityObservation( + const Eigen::Vector3d& imu_angular_velocity); + + // Query the current time. + common::Time time() const { return time_; } + + // Query the current orientation estimate. + Eigen::Quaterniond orientation() const { return orientation_; } + + private: + const double imu_gravity_time_constant_; + common::Time time_; + common::Time last_linear_acceleration_time_; + Eigen::Quaterniond orientation_; + Eigen::Vector3d gravity_vector_; + Eigen::Vector3d imu_angular_velocity_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker_test.cc new file mode 100644 index 0000000..10992ed --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/imu_tracker_test.cc @@ -0,0 +1,95 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/imu_tracker.h" + +#include "absl/memory/memory.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +constexpr double kDuration = 3.f; +constexpr double kGravityTimeConstant = 0.1 * kDuration; +constexpr double kPrecision = 1e-8; +constexpr int kSteps = 10; + +class ImuTrackerTest : public ::testing::Test { + protected: + void SetUp() override { + imu_tracker_ = absl::make_unique(kGravityTimeConstant, time_); + angular_velocity_ = Eigen::Vector3d(0, 0, 0); + linear_acceleration_ = Eigen::Vector3d(0, 0, 9.9); + EXPECT_NEAR(0., + imu_tracker_->orientation().angularDistance( + Eigen::Quaterniond::Identity()), + kPrecision); + } + + void AdvanceImu() { + for (int i = 0; i < kSteps; ++i) { + imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration_); + imu_tracker_->AddImuAngularVelocityObservation(angular_velocity_); + time_ += common::FromSeconds(kDuration / kSteps); + imu_tracker_->Advance(time_); + } + EXPECT_EQ(time_, imu_tracker_->time()); + } + + Eigen::Vector3d angular_velocity_; + std::unique_ptr imu_tracker_; + Eigen::Vector3d linear_acceleration_; + common::Time time_ = common::FromUniversal(12345678); +}; + +TEST_F(ImuTrackerTest, IntegrateYawRotation) { + angular_velocity_ = Eigen::Vector3d(0, 0, 0.3); + AdvanceImu(); + Eigen::Quaterniond expected_orientation(Eigen::AngleAxisd( + kDuration * angular_velocity_.norm(), angular_velocity_.normalized())); + EXPECT_NEAR(0., + imu_tracker_->orientation().angularDistance(expected_orientation), + kPrecision); +} + +TEST_F(ImuTrackerTest, IntegrateFullRotation) { + angular_velocity_ = Eigen::Vector3d(0.1, 0.4, 0.1); + // Using a huge gravity time constant effectively disables gravity vector + // tracking. + imu_tracker_.reset(new ImuTracker(1e10 * kDuration, time_)); + AdvanceImu(); + Eigen::Quaterniond expected_orientation(Eigen::AngleAxisd( + kDuration * angular_velocity_.norm(), angular_velocity_.normalized())); + EXPECT_NEAR(0., + imu_tracker_->orientation().angularDistance(expected_orientation), + kPrecision); +} + +TEST_F(ImuTrackerTest, LearnGravityVector) { + linear_acceleration_ = Eigen::Vector3d(0.5, 0.3, 9.5); + AdvanceImu(); + const Eigen::Quaterniond expected_orientation = + FromTwoVectors(linear_acceleration_, Eigen::Vector3d::UnitZ()); + EXPECT_NEAR(0., + imu_tracker_->orientation().angularDistance(expected_orientation), + kPrecision); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_slam_result_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_slam_result_2d.cc new file mode 100644 index 0000000..d068d01 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_slam_result_2d.cc @@ -0,0 +1,55 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/local_slam_result_2d.h" + +#include "cartographer/mapping/internal/2d/pose_graph_2d.h" + +namespace cartographer { +namespace mapping { + +void LocalSlamResult2D::AddToTrajectoryBuilder( + TrajectoryBuilderInterface* const trajectory_builder) { + trajectory_builder->AddLocalSlamResultData( + absl::make_unique(*this)); +} + +void LocalSlamResult2D::AddToPoseGraph(int trajectory_id, + PoseGraph* pose_graph) const { + DCHECK(dynamic_cast(pose_graph)); + CHECK_GE(local_slam_result_data_.submaps().size(), 1); + CHECK(local_slam_result_data_.submaps(0).has_submap_2d()); + std::vector> submaps; + for (const auto& submap_proto : local_slam_result_data_.submaps()) { + auto submap_ptr = submap_controller_->UpdateSubmap(submap_proto); + if (submap_ptr) { + submaps.push_back(submap_ptr); + } else { + LOG(INFO) << "Ignoring submap"; + } + } + if (submaps.size() == 0) { + LOG(INFO) << "Ignoring node"; + return; + } + static_cast(pose_graph) + ->AddNode(std::make_shared( + mapping::FromProto(local_slam_result_data_.node_data())), + trajectory_id, submaps); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_slam_result_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_slam_result_2d.h new file mode 100644 index 0000000..71b4f18 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_slam_result_2d.h @@ -0,0 +1,52 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ + +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/internal/submap_controller.h" +#include "cartographer/mapping/trajectory_builder_interface.h" + +namespace cartographer { +namespace mapping { + +class LocalSlamResult2D : public LocalSlamResultData { + public: + LocalSlamResult2D( + const std::string& sensor_id, + const mapping::proto::LocalSlamResultData local_slam_result_data, + SubmapController* submap_controller) + : LocalSlamResultData(sensor_id, common::FromUniversal( + local_slam_result_data.timestamp())), + sensor_id_(sensor_id), + local_slam_result_data_(local_slam_result_data), + submap_controller_(submap_controller) {} + + void AddToTrajectoryBuilder( + TrajectoryBuilderInterface* const trajectory_builder) override; + void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override; + + private: + const std::string sensor_id_; + const mapping::proto::LocalSlamResultData local_slam_result_data_; + SubmapController* submap_controller_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc new file mode 100644 index 0000000..90a7e72 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -0,0 +1,521 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null(); +static auto* kLocalSlamRealTimeRatio = metrics::Gauge::Null(); +static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null(); +static auto* kRealTimeCorrelativeScanMatcherScoreMetric = + metrics::Histogram::Null(); +static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null(); + +/** + * @brief 构造函数 + * + * @param[in] options + * @param[in] expected_range_sensor_ids 所有range类型的话题 + */ +LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D( + const proto::LocalTrajectoryBuilderOptions2D& options, + const std::vector& expected_range_sensor_ids) + : options_(options), + active_submaps_(options.submaps_options()), + 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()), + range_data_collator_(expected_range_sensor_ids) {} + +LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {} + +/** + * @brief 先进行点云的旋转与z方向的滤波, 然后再进行体素滤波减少数据量 + * + * @param[in] transform_to_gravity_aligned_frame 将点云变换到原点处, 且姿态为0的坐标变换 + * @param[in] range_data 传入的点云 + * @return sensor::RangeData 处理后的点云 拷贝 + */ +sensor::RangeData LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( + const transform::Rigid3f& transform_to_gravity_aligned_frame, + const sensor::RangeData& range_data) const { + // Step: 5 将原点位于机器人当前位姿处的点云 转成 原点位于local坐标系原点处的点云, 再进行z轴上的过滤 + const sensor::RangeData cropped = + sensor::CropRangeData(sensor::TransformRangeData( + range_data, transform_to_gravity_aligned_frame), + options_.min_z(), options_.max_z()); // param: min_z max_z + // Step: 6 对点云进行体素滤波 + return sensor::RangeData{ + cropped.origin, + sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size + sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())}; +} + +sensor::RangeData LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilterNew( + const transform::Rigid3f& transform_to_gravity_aligned_frame, + sensor::RangeData& range_data) { + // Step: 5 将原点位于机器人当前位姿处的点云 转成 原点位于local坐标系原点处的点云, 再进行z轴上的过滤 + sensor::RangeData crop=sensor::TransformRangeDataNew( + range_data, transform_to_gravity_aligned_frame); + sensor::RangeData cropped = + sensor::CropRangeDataNew(crop, + options_.min_z(), options_.max_z()); // param: min_z max_z + // Step: 6 对点云进行体素滤波 + return sensor::RangeData{ + cropped.origin, + sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size + sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())}; +} + + + + +/** + * @brief 进行扫描匹配 + * + * @param[in] time 点云的时间 + * @param[in] pose_prediction 先验位姿 + * @param[in] filtered_gravity_aligned_point_cloud 匹配用的点云 + * @return std::unique_ptr 匹配后的二维位姿 + */ +std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( + const common::Time time, const transform::Rigid2d& pose_prediction, + const sensor::PointCloud& filtered_gravity_aligned_point_cloud) { + if (active_submaps_.submaps().empty()) { + return absl::make_unique(pose_prediction); + } + // 使用active_submaps_的第一个子图进行匹配 + std::shared_ptr matching_submap = + active_submaps_.submaps().front(); + // The online correlative scan matcher will refine the initial estimate for + // the Ceres scan matcher. + transform::Rigid2d initial_ceres_pose = pose_prediction; + + // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准 + if (options_.use_online_correlative_scan_matching()) { + const double score = real_time_correlative_scan_matcher_.Match( + pose_prediction, filtered_gravity_aligned_point_cloud, + *matching_submap->grid(), &initial_ceres_pose); + kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); + } + + auto pose_observation = absl::make_unique(); + ceres::Solver::Summary summary; + // 使用ceres进行扫描匹配 + ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose, + filtered_gravity_aligned_point_cloud, + *matching_submap->grid(), pose_observation.get(), + &summary); + // 一些度量 + if (pose_observation) { + kCeresScanMatcherCostMetric->Observe(summary.final_cost); + const double residual_distance = + (pose_observation->translation() - pose_prediction.translation()) + .norm(); + kScanMatcherResidualDistanceMetric->Observe(residual_distance); + const double residual_angle = + std::abs(pose_observation->rotation().angle() - + pose_prediction.rotation().angle()); + kScanMatcherResidualAngleMetric->Observe(residual_angle); + } + // 返回ceres计算后的位姿 + return pose_observation; +} + +/** + * @brief 处理点云数据, 进行扫描匹配, 将点云写成地图 + * + * @param[in] sensor_id 点云数据对应的话题名称 + * @param[in] unsynchronized_data 传入的点云数据 + * @return std::unique_ptr 匹配后的结果 + */ +std::unique_ptr +LocalTrajectoryBuilder2D::AddRangeData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& unsynchronized_data) { + + // Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的 + //。。。增加color + auto synchronized_data = + range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); + + // auto synchronized_data = unsynchronized_data; + if (synchronized_data.ranges.empty()) { + LOG(INFO) << "Range data collator filling buffer."; + return nullptr; + } + + const common::Time& time = synchronized_data.time; + // Initialize extrapolator now if we do not ever use an IMU. + // 如果不用imu, 就在雷达这初始化位姿推测器 + if (!options_.use_imu_data()) { + InitializeExtrapolator(time); + } + + if (extrapolator_ == nullptr) { + // Until we've initialized the extrapolator with our first IMU message, we + // cannot compute the orientation of the rangefinder. + LOG(INFO) << "Extrapolator not yet initialized."; + return nullptr; + } + + CHECK(!synchronized_data.ranges.empty()); + // TODO(gaschler): Check if this can strictly be 0. + CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f); + + // 计算第一个点的时间 + const common::Time time_first_point = + time + + common::FromSeconds(synchronized_data.ranges.front().point_time.time); + // 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min() + if (time_first_point < extrapolator_->GetLastPoseTime()) { + LOG(INFO) << "Extrapolator is still initializing."; + return nullptr; + } + + std::vector range_data_poses; + range_data_poses.reserve(synchronized_data.ranges.size()); + bool warned = false; + + // 预测得到每一个时间点的位姿 + for (const auto& range : synchronized_data.ranges) { + common::Time time_point = time + common::FromSeconds(range.point_time.time); + // 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错 + if (time_point < extrapolator_->GetLastExtrapolatedTime()) { + // 一个循环只报一次错 + if (!warned) { + LOG(ERROR) + << "Timestamp of individual range data point jumps backwards from " + << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; + warned = true; + } + time_point = extrapolator_->GetLastExtrapolatedTime(); + } + + // Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿 + range_data_poses.push_back( + extrapolator_->ExtrapolatePose(time_point).cast()); + } + + if (num_accumulated_ == 0) { + // 'accumulated_range_data_.origin' is uninitialized until the last + // accumulation. + accumulated_range_data_ = sensor::RangeData{{}, {}, {}}; + } + + // Drop any returns below the minimum range and convert returns beyond the + // maximum range into misses. + // 对每个数据点进行处理 + for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) { + // 获取在tracking frame 下点的坐标 + const sensor::TimedRangefinderPoint& hit = + synchronized_data.ranges[i].point_time; + // 将点云的origins坐标转到 local slam 坐标系下 + const Eigen::Vector3f origin_in_local = + range_data_poses[i] * + synchronized_data.origins.at(synchronized_data.ranges[i].origin_index); + + // Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标 + sensor::RangefinderPoint hit_in_local = + range_data_poses[i] * sensor::ToRangefinderPoint(hit); + + // 计算这个点的距离, 这里用的是去畸变之后的点的距离 + const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; + const float range = delta.norm(); + + // param: min_range max_range + if (range >= options_.min_range()) { + if (range <= options_.max_range()) { + // 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标 + //。。。accumulated_range_data_.returns.push_back(hit_in_local); + accumulated_range_data_.returns.push_back(hit_in_local,synchronized_data.ranges[i].intensity); + } else { + // Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里 + hit_in_local.position = + origin_in_local + + // param: missing_data_ray_length, 是个比例, 不是距离 + options_.missing_data_ray_length() / range * delta; + accumulated_range_data_.misses.push_back(hit_in_local); + } + } + } // end for + + // 有一帧有效的数据了 + ++num_accumulated_; + + // param: num_accumulated_range_data 几帧有效的点云数据进行一次扫描匹配 + if (num_accumulated_ >= options_.num_accumulated_range_data()) { + // 计算2次有效点云数据的的时间差 + const common::Time current_sensor_time = synchronized_data.time; + absl::optional sensor_duration; + if (last_sensor_time_.has_value()) { + sensor_duration = current_sensor_time - last_sensor_time_.value(); + } + last_sensor_time_ = current_sensor_time; + + // 重置变量 + num_accumulated_ = 0; + + // 获取机器人当前姿态 + const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( + extrapolator_->EstimateGravityOrientation(time)); + + // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time + // 'time'. + // 以最后一个点的时间戳估计出的坐标为这帧数据的原点 + accumulated_range_data_.origin = range_data_poses.back().translation(); + //...临时变量不能非const 引用传递，将临时变量转换为局部变量 + sensor::RangeData accumulated_range_data_new= TransformToGravityAlignedFrameAndFilterNew( + gravity_alignment.cast() * range_data_poses.back().inverse(), + //。。。 accumulated_range_data_增加color + accumulated_range_data_); + return AddAccumulatedRangeData( + time, + // 将点云变换到local原点处, 且姿态为0 + accumulated_range_data_new, + gravity_alignment, sensor_duration); + } + + return nullptr; +} + +/** + * @brief 进行扫描匹配, 将点云写入地图 + * + * @param[in] time 点云的时间戳 + * @param[in] gravity_aligned_range_data 原点位于local坐标系原点处的点云 + * @param[in] gravity_alignment 机器人当前姿态 + * @param[in] sensor_duration 2帧点云数据的时间差 + * @return std::unique_ptr + */ +std::unique_ptr +LocalTrajectoryBuilder2D::AddAccumulatedRangeData( + const common::Time time, + //...const sensor::RangeData& gravity_aligned_range_data, + sensor::RangeData& gravity_aligned_range_data, + const transform::Rigid3d& gravity_alignment, + const absl::optional& sensor_duration) { + // 如果处理完点云之后数据为空, 就报错. 使用单线雷达时不要设置min_z + if (gravity_aligned_range_data.returns.empty()) { + LOG(WARNING) << "Dropped empty horizontal range data."; + return nullptr; + } + + // Computes a gravity aligned pose prediction. + // 进行位姿的预测, 先验位姿 + const transform::Rigid3d non_gravity_aligned_pose_prediction = + extrapolator_->ExtrapolatePose(time); + // 将三维位姿先旋转到姿态为0, 再取xy坐标将三维位姿转成二维位姿 + const transform::Rigid2d pose_prediction = transform::Project2D( + non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); + + // Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloud + const sensor::PointCloud& filtered_gravity_aligned_point_cloud = + sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns, + options_.adaptive_voxel_filter_options()); + if (filtered_gravity_aligned_point_cloud.empty()) { + return nullptr; + } + // LOG(INFO)< pose_estimate_2d = + ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud); + + // LOG(INFO)<DebugString(); + if (pose_estimate_2d == nullptr) { + LOG(WARNING) << "Scan matching failed."; + return nullptr; + } + + // 将二维坐标旋转回之前的姿态 + const transform::Rigid3d pose_estimate = + transform::Embed3D(*pose_estimate_2d) * gravity_alignment; + // 校准位姿估计器 + extrapolator_->AddPose(time, pose_estimate); + + // Step: 8 将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云 + sensor::RangeData range_data_in_local = + TransformRangeDataNew(gravity_aligned_range_data, + transform::Embed3D(pose_estimate_2d->cast())); + + // 将校正后的雷达数据写入submap + std::unique_ptr insertion_result = InsertIntoSubmap( + time, range_data_in_local, filtered_gravity_aligned_point_cloud, + pose_estimate, gravity_alignment.rotation()); + + // 计算耗时 + const auto wall_time = std::chrono::steady_clock::now(); + if (last_wall_time_.has_value()) { + const auto wall_time_duration = wall_time - last_wall_time_.value(); + kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration)); + if (sensor_duration.has_value()) { + kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) / + common::ToSeconds(wall_time_duration)); + } + } + // 计算cpu耗时 + const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds(); + if (last_thread_cpu_time_seconds_.has_value()) { + const double thread_cpu_duration_seconds = + thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value(); + if (sensor_duration.has_value()) { + kLocalSlamCpuRealTimeRatio->Set( + common::ToSeconds(sensor_duration.value()) / + thread_cpu_duration_seconds); + } + } + last_wall_time_ = wall_time; + last_thread_cpu_time_seconds_ = thread_cpu_time_seconds; + + // 返回结果 + return absl::make_unique( + MatchingResult{time, pose_estimate, std::move(range_data_in_local), + std::move(insertion_result)}); +} + +/** + * @brief 将处理后的雷达数据写入submap + * + * @param[in] time 点云的时间 + * @param[in] range_data_in_local 校正后的点云 + * @param[in] filtered_gravity_aligned_point_cloud 自适应体素滤波后的点云 + * @param[in] pose_estimate 扫描匹配后的三维位姿 + * @param[in] gravity_alignment + * @return std::unique_ptr + */ +std::unique_ptr +LocalTrajectoryBuilder2D::InsertIntoSubmap( + const common::Time time, + //..const sensor::RangeData& range_data_in_local, + sensor::RangeData& range_data_in_local, + const sensor::PointCloud& filtered_gravity_aligned_point_cloud, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment) { + // 如果移动距离过小, 或者时间过短, 不进行地图的更新 + //if (motion_filter_.IsSimilar(time, pose_estimate)) { + if (motion_filter_.IsSimilarNew(time, pose_estimate,range_data_in_local)) { + return nullptr; + } + // 将点云数据写入到submap中 + std::vector> insertion_submaps = + active_submaps_.InsertRangeData(range_data_in_local); + + // 生成InsertionResult格式的数据进行返回 + return absl::make_unique(InsertionResult{ + std::make_shared(TrajectoryNode::Data{ + time, + gravity_alignment, + filtered_gravity_aligned_point_cloud, // 这里存的是体素滤波后的点云, 不是校准后的点云 + {}, // 'high_resolution_point_cloud' is only used in 3D. + {}, // 'low_resolution_point_cloud' is only used in 3D. + {}, // 'rotational_scan_matcher_histogram' is only used in 3D. + pose_estimate}), + std::move(insertion_submaps)}); +} + +// 将IMU数据加入到Extrapolator中 +void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) { + CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; + InitializeExtrapolator(imu_data.time); + extrapolator_->AddImuData(imu_data); +} + +// 将里程计数据加入到Extrapolator中 +void LocalTrajectoryBuilder2D::AddOdometryData( + const sensor::OdometryData& odometry_data) { + if (extrapolator_ == nullptr) { + // Until we've initialized the extrapolator we cannot add odometry data. + LOG(INFO) << "Extrapolator not yet initialized."; + return; + } + extrapolator_->AddOdometryData(odometry_data); +} + +// 如果Extrapolator没有初始化就进行初始化 +void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { + // 如果已经初始化过了就直接返回 + if (extrapolator_ != nullptr) { + return; + } + + // 注意 use_imu_based为true就会报错 + CHECK(!options_.pose_extrapolator_options().use_imu_based()); + // TODO(gaschler): Consider using InitializeWithImu as 3D does. + + // 初始化位姿推测器 + extrapolator_ = absl::make_unique( + ::cartographer::common::FromSeconds(options_.pose_extrapolator_options() + .constant_velocity() + .pose_queue_duration()), // 0.001s + options_.pose_extrapolator_options() + .constant_velocity() + .imu_gravity_time_constant()); // 10 + // 添加初始位姿 + extrapolator_->AddPose(time, transform::Rigid3d::Identity()); +} + +void LocalTrajectoryBuilder2D::RegisterMetrics( + metrics::FamilyFactory* family_factory) { + auto* latency = family_factory->NewGaugeFamily( + "mapping_2d_local_trajectory_builder_latency", + "Duration from first incoming point cloud in accumulation to local slam " + "result"); + kLocalSlamLatencyMetric = latency->Add({}); + auto* real_time_ratio = family_factory->NewGaugeFamily( + "mapping_2d_local_trajectory_builder_real_time_ratio", + "sensor duration / wall clock duration."); + kLocalSlamRealTimeRatio = real_time_ratio->Add({}); + + auto* cpu_real_time_ratio = family_factory->NewGaugeFamily( + "mapping_2d_local_trajectory_builder_cpu_real_time_ratio", + "sensor duration / cpu duration."); + kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({}); + auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = family_factory->NewHistogramFamily( + "mapping_2d_local_trajectory_builder_scores", "Local scan matcher scores", + score_boundaries); + kRealTimeCorrelativeScanMatcherScoreMetric = + scores->Add({{"scan_matcher", "real_time_correlative"}}); + auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100); + auto* costs = family_factory->NewHistogramFamily( + "mapping_2d_local_trajectory_builder_costs", "Local scan matcher costs", + cost_boundaries); + kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}}); + auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10); + auto* residuals = family_factory->NewHistogramFamily( + "mapping_2d_local_trajectory_builder_residuals", + "Local scan matcher residuals", distance_boundaries); + kScanMatcherResidualDistanceMetric = + residuals->Add({{"component", "distance"}}); + kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}}); +} + +} // namespace mapping +} // namespace cartographer + diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h new file mode 100644 index 0000000..8bce87d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_2d.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h" +#include "cartographer/mapping/internal/motion_filter.h" +#include "cartographer/mapping/internal/range_data_collator.h" +#include "cartographer/mapping/pose_extrapolator.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { + +// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) +// without loop closure. +// TODO(gaschler): Add test for this class similar to the 3D test. +class LocalTrajectoryBuilder2D { + public: + struct InsertionResult { + std::shared_ptr constant_data; + std::vector> insertion_submaps; + }; + struct MatchingResult { + common::Time time; + transform::Rigid3d local_pose; + sensor::RangeData range_data_in_local; + // 'nullptr' if dropped by the motion filter. + std::unique_ptr insertion_result; + }; + + explicit LocalTrajectoryBuilder2D( + const proto::LocalTrajectoryBuilderOptions2D& options, + const std::vector& expected_range_sensor_ids); + ~LocalTrajectoryBuilder2D(); + + LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete; + LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete; + + // Returns 'MatchingResult' when range data accumulation completed, + // otherwise 'nullptr'. Range data must be approximately horizontal + // for 2D SLAM. `TimedPointCloudData::time` is when the last point in + // `range_data` was acquired, `TimedPointCloudData::ranges` contains the + // relative time of point with respect to `TimedPointCloudData::time`. + std::unique_ptr AddRangeData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& range_data); + void AddImuData(const sensor::ImuData& imu_data); + void AddOdometryData(const sensor::OdometryData& odometry_data); + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + std::unique_ptr AddAccumulatedRangeData( + common::Time time, sensor::RangeData& gravity_aligned_range_data, + const transform::Rigid3d& gravity_alignment, + const absl::optional& sensor_duration); + + + sensor::RangeData TransformToGravityAlignedFrameAndFilter( + const transform::Rigid3f& transform_to_gravity_aligned_frame, + const sensor::RangeData& range_data) const; + + + sensor::RangeData TransformToGravityAlignedFrameAndFilterNew( + const transform::Rigid3f& transform_to_gravity_aligned_frame, + sensor::RangeData& range_data) ; + + + + std::unique_ptr InsertIntoSubmap( + common::Time time, sensor::RangeData& range_data_in_local, + const sensor::PointCloud& filtered_gravity_aligned_point_cloud, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment); + + // Scan matches 'filtered_gravity_aligned_point_cloud' and returns the + // observed pose, or nullptr on failure. + std::unique_ptr ScanMatch( + common::Time time, const transform::Rigid2d& pose_prediction, + const sensor::PointCloud& filtered_gravity_aligned_point_cloud); + + // Lazily constructs a PoseExtrapolator. + void InitializeExtrapolator(common::Time time); + + const proto::LocalTrajectoryBuilderOptions2D options_; + ActiveSubmaps2D active_submaps_; + + MotionFilter motion_filter_; + scan_matching::RealTimeCorrelativeScanMatcher2D + real_time_correlative_scan_matcher_; + scan_matching::CeresScanMatcher2D ceres_scan_matcher_; + + std::unique_ptr extrapolator_; + + int num_accumulated_ = 0; + sensor::RangeData accumulated_range_data_; + + absl::optional last_wall_time_; + absl::optional last_thread_cpu_time_seconds_; + absl::optional last_sensor_time_; + + RangeDataCollator range_data_collator_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc new file mode 100644 index 0000000..434da3d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc @@ -0,0 +1,73 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h" + +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping/internal/motion_filter.h" +#include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/sensor/internal/voxel_filter.h" + +namespace cartographer { +namespace mapping { + +proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::LocalTrajectoryBuilderOptions2D options; + options.set_min_range(parameter_dictionary->GetDouble("min_range")); + options.set_max_range(parameter_dictionary->GetDouble("max_range")); + options.set_min_z(parameter_dictionary->GetDouble("min_z")); + options.set_max_z(parameter_dictionary->GetDouble("max_z")); + options.set_missing_data_ray_length( + parameter_dictionary->GetDouble("missing_data_ray_length")); + options.set_num_accumulated_range_data( + parameter_dictionary->GetInt("num_accumulated_range_data")); + options.set_voxel_filter_size( + parameter_dictionary->GetDouble("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_loop_closure_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("loop_closure_adaptive_voxel_filter") + .get()); + *options.mutable_real_time_correlative_scan_matcher_options() = + mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary + ->GetDictionary("real_time_correlative_scan_matcher") + .get()); + *options.mutable_ceres_scan_matcher_options() = + mapping::scan_matching::CreateCeresScanMatcherOptions2D( + parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); + *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( + parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + *options.mutable_submaps_options() = CreateSubmapsOptions2D( + parameter_dictionary->GetDictionary("submaps").get()); + options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); + return options; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h new file mode 100644 index 0000000..7684920 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h @@ -0,0 +1,32 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h" + +namespace cartographer { +namespace mapping { + +proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d.cc new file mode 100644 index 0000000..a7d9542 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d.cc @@ -0,0 +1,112 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/normal_estimation_2d.h" + +namespace cartographer { +namespace mapping { +namespace { + +float NormalTo2DAngle(const Eigen::Vector3f& v) { + return std::atan2(v[1], v[0]); +} + +// Estimate the normal of an estimation_point as the arithmetic mean of the the +// normals of the vectors from estimation_point to each point in the +// sample_window. +float EstimateNormal(const sensor::PointCloud& returns, + const size_t estimation_point_index, + const size_t sample_window_begin, + const size_t sample_window_end, + const Eigen::Vector3f& sensor_origin) { + const Eigen::Vector3f& estimation_point = + returns[estimation_point_index].position; + if (sample_window_end - sample_window_begin < 2) { + return NormalTo2DAngle(sensor_origin - estimation_point); + } + Eigen::Vector3f mean_normal = Eigen::Vector3f::Zero(); + const Eigen::Vector3f& estimation_point_to_observation = + sensor_origin - estimation_point; + for (size_t sample_point_index = sample_window_begin; + sample_point_index < sample_window_end; ++sample_point_index) { + if (sample_point_index == estimation_point_index) continue; + const Eigen::Vector3f& sample_point = returns[sample_point_index].position; + const Eigen::Vector3f& tangent = estimation_point - sample_point; + Eigen::Vector3f sample_normal = {-tangent[1], tangent[0], 0.f}; + constexpr float kMinNormalLength = 1e-6f; + if (sample_normal.norm() < kMinNormalLength) { + continue; + } + // Ensure sample_normal points towards 'sensor_origin'. + if (sample_normal.dot(estimation_point_to_observation) < 0) { + sample_normal = -sample_normal; + } + sample_normal.normalize(); + mean_normal += sample_normal; + } + return NormalTo2DAngle(mean_normal); +} +} // namespace + +proto::NormalEstimationOptions2D CreateNormalEstimationOptions2D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::NormalEstimationOptions2D options; + options.set_num_normal_samples( + parameter_dictionary->GetInt("num_normal_samples")); + options.set_sample_radius(parameter_dictionary->GetDouble("sample_radius")); + CHECK_GT(options.num_normal_samples(), 0); + CHECK_GT(options.sample_radius(), 0.0); + return options; +} + +// Estimates the normal for each 'return' in 'range_data'. +// Assumes the angles in the range data returns are sorted with respect to +// the orientation of the vector from 'origin' to 'return'. +std::vector EstimateNormals( + const sensor::RangeData& range_data, + const proto::NormalEstimationOptions2D& normal_estimation_options) { + std::vector normals; + normals.reserve(range_data.returns.size()); + const size_t max_num_samples = normal_estimation_options.num_normal_samples(); + const float sample_radius = normal_estimation_options.sample_radius(); + for (size_t current_point = 0; current_point < range_data.returns.size(); + ++current_point) { + const Eigen::Vector3f& hit = range_data.returns[current_point].position; + size_t sample_window_begin = current_point; + for (; sample_window_begin > 0 && + current_point - sample_window_begin < max_num_samples / 2 && + (hit - range_data.returns[sample_window_begin - 1].position).norm() < + sample_radius; + --sample_window_begin) { + } + size_t sample_window_end = current_point; + for (; + sample_window_end < range_data.returns.size() && + sample_window_end - current_point < ceil(max_num_samples / 2.0) + 1 && + (hit - range_data.returns[sample_window_end].position).norm() < + sample_radius; + ++sample_window_end) { + } + const float normal_estimate = + EstimateNormal(range_data.returns, current_point, sample_window_begin, + sample_window_end, range_data.origin); + normals.push_back(normal_estimate); + } + return normals; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d.h new file mode 100644 index 0000000..9a34426 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ + +#include + +#include "cartographer/mapping/proto/normal_estimation_options_2d.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +proto::NormalEstimationOptions2D CreateNormalEstimationOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +// Estimates the normal for each 'return' in 'range_data'. +// Assumes the angles in the range data returns are sorted with respect to +// the orientation of the vector from 'origin' to 'return'. +std::vector EstimateNormals( + const sensor::RangeData& range_data, + const proto::NormalEstimationOptions2D& normal_estimation_options); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc new file mode 100644 index 0000000..336d31d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -0,0 +1,143 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/normal_estimation_2d.h" + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using ::testing::TestWithParam; +using ::testing::Values; + +TEST(NormalEstimation2DTest, SinglePoint) { + const auto parameter_dictionary = common::MakeDictionary( + "return { " + "num_normal_samples = 2, " + "sample_radius = 10.0, " + "}"); + const proto::NormalEstimationOptions2D options = + CreateNormalEstimationOptions2D(parameter_dictionary.get()); + auto range_data = sensor::RangeData(); + const size_t num_angles = 100; + range_data.origin.x() = 0.f; + range_data.origin.y() = 0.f; + for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { + const double angle = static_cast(angle_idx) / + static_cast(num_angles) * 2. * M_PI - + M_PI; + range_data.returns = sensor::PointCloud( + {{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.} + .cast()}}); + std::vector normals; + normals = EstimateNormals(range_data, options); + EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI), + 0.0, 2.0 * M_PI / num_angles + 1e-4); + } +} + +TEST(NormalEstimation2DTest, StraightLineGeometry) { + const auto parameter_dictionary = common::MakeDictionary( + "return { " + "num_normal_samples = 2, " + "sample_radius = 10.0, " + "}"); + const proto::NormalEstimationOptions2D options = + CreateNormalEstimationOptions2D(parameter_dictionary.get()); + auto range_data = sensor::RangeData(); + range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}}); + range_data.origin.x() = 0.f; + range_data.origin.y() = 0.f; + std::vector normals; + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(normal, -M_PI_2, 1e-4); + } + normals.clear(); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, 1.f, 0.f}}, + {Eigen::Vector3f{1.f, 0.f, 0.f}}, + {Eigen::Vector3f{1.f, -1.f, 0.f}}}); + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(std::abs(normal), M_PI, 1e-4); + } + + normals.clear(); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, -1.f, 0.f}}, + {Eigen::Vector3f{0.f, -1.f, 0.f}}, + {Eigen::Vector3f{-1.f, -1.f, 0.f}}}); + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(normal, M_PI_2, 1e-4); + } + + normals.clear(); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{-1.f, -1.f, 0.f}}, + {Eigen::Vector3f{-1.f, 0.f, 0.f}}, + {Eigen::Vector3f{-1.f, 1.f, 0.f}}}); + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(normal, 0, 1e-4); + } +} + +class CircularGeometry2DTest : public TestWithParam {}; + +TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) { + const auto parameter_dictionary = common::MakeDictionary( + "return { " + "num_normal_samples = " + + std::to_string(GetParam()) + + ", " + "sample_radius = 10.0, " + "}"); + const proto::NormalEstimationOptions2D options = + CreateNormalEstimationOptions2D(parameter_dictionary.get()); + auto range_data = sensor::RangeData(); + const size_t num_angles = 100; + for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { + const double angle = static_cast(angle_idx) / + static_cast(num_angles) * 2. * M_PI - + M_PI; + range_data.returns.push_back( + {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast()}); + } + range_data.origin.x() = 0.f; + range_data.origin.y() = 0.f; + std::vector normals; + normals = EstimateNormals(range_data, options); + for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { + const double angle = static_cast(angle_idx) / + static_cast(num_angles) * 2. * M_PI; + EXPECT_NEAR(common::NormalizeAngleDifference(normals[angle_idx] - angle), + 0.0, 2.0 * M_PI / num_angles * GetParam() / 2.0 + 1e-4); + } +} + +INSTANTIATE_TEST_CASE_P(InstantiationName, CircularGeometry2DTest, + ::testing::Values(1, 2, 4, 5, 8)); + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc new file mode 100644 index 0000000..a62b0cd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -0,0 +1,212 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h" + +#include + +#include "cartographer/mapping/2d/submap_2d.h" + +namespace cartographer { +namespace mapping { +namespace { + +class SubmapCoverageGrid2D { + public: + // Aliases for documentation only (no type-safety). + using CellId = std::pair; + using StoredType = std::vector>; + + SubmapCoverageGrid2D(const MapLimits& map_limits) + : offset_(map_limits.max()), resolution_(map_limits.resolution()) {} + + void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id, + const common::Time& time) { + CellId cell_id{common::RoundToInt64((offset_(0) - point(0)) / resolution_), + common::RoundToInt64((offset_(1) - point(1)) / resolution_)}; + cells_[cell_id].emplace_back(submap_id, time); + } + + const std::map& cells() const { return cells_; } + double resolution() const { return resolution_; } + + private: + Eigen::Vector2d offset_; + double resolution_; + std::map cells_; +}; + +// Iterates over every cell in a submap, transforms the center of the cell to +// the global frame and then adds the submap id and the timestamp of the most +// recent range data insertion into the global grid. +std::set AddSubmapsToSubmapCoverageGrid2D( + const std::map& submap_freshness, + const MapById& submap_data, + SubmapCoverageGrid2D* coverage_grid) { + std::set all_submap_ids; + + for (const auto& submap : submap_data) { + auto freshness = submap_freshness.find(submap.id); + if (freshness == submap_freshness.end()) continue; + if (!submap.data.submap->insertion_finished()) continue; + all_submap_ids.insert(submap.id); + const Grid2D& grid = + *std::static_pointer_cast(submap.data.submap)->grid(); + // Iterate over every cell in a submap. + Eigen::Array2i offset; + CellLimits cell_limits; + grid.ComputeCroppedLimits(&offset, &cell_limits); + if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { + LOG(WARNING) << "Empty grid found in submap ID = " << submap.id; + continue; + } + + const transform::Rigid3d& global_frame_from_submap_frame = submap.data.pose; + const transform::Rigid3d submap_frame_from_local_frame = + submap.data.submap->local_pose().inverse(); + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + const Eigen::Array2i index = xy_index + offset; + if (!grid.IsKnown(index)) continue; + + const transform::Rigid3d center_of_cell_in_local_frame = + transform::Rigid3d::Translation(Eigen::Vector3d( + grid.limits().max().x() - + grid.limits().resolution() * (index.y() + 0.5), + grid.limits().max().y() - + grid.limits().resolution() * (index.x() + 0.5), + 0)); + + const transform::Rigid2d center_of_cell_in_global_frame = + transform::Project2D(global_frame_from_submap_frame * + submap_frame_from_local_frame * + center_of_cell_in_local_frame); + coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(), + submap.id, freshness->second); + } + } + return all_submap_ids; +} + +// Uses intra-submap constraints and trajectory node timestamps to identify time +// of the last range data insertion to the submap. +std::map ComputeSubmapFreshness( + const MapById& submap_data, + const MapById& trajectory_nodes, + const std::vector& constraints) { + std::map submap_freshness; + + // Find the node with the largest NodeId per SubmapId. + std::map submap_to_latest_node; + for (const PoseGraphInterface::Constraint& constraint : constraints) { + if (constraint.tag != PoseGraphInterface::Constraint::INTRA_SUBMAP) { + continue; + } + auto submap_to_node = submap_to_latest_node.find(constraint.submap_id); + if (submap_to_node == submap_to_latest_node.end()) { + submap_to_latest_node.insert( + std::make_pair(constraint.submap_id, constraint.node_id)); + continue; + } + submap_to_node->second = + std::max(submap_to_node->second, constraint.node_id); + } + + // Find timestamp of every latest node. + for (const auto& submap_id_to_node_id : submap_to_latest_node) { + auto submap_data_item = submap_data.find(submap_id_to_node_id.first); + if (submap_data_item == submap_data.end()) { + LOG(WARNING) << "Intra-submap constraint between SubmapID = " + << submap_id_to_node_id.first << " and NodeID " + << submap_id_to_node_id.second << " is missing submap data"; + continue; + } + auto latest_node_id = trajectory_nodes.find(submap_id_to_node_id.second); + if (latest_node_id == trajectory_nodes.end()) continue; + submap_freshness[submap_data_item->id] = latest_node_id->data.time(); + } + return submap_freshness; +} + +// Returns IDs of submaps that have less than 'min_covered_cells_count' cells +// not overlapped by at least 'fresh_submaps_count' submaps. +std::vector FindSubmapIdsToTrim( + const SubmapCoverageGrid2D& coverage_grid, + const std::set& all_submap_ids, uint16 fresh_submaps_count, + uint16 min_covered_cells_count) { + std::map submap_to_covered_cells_count; + for (const auto& cell : coverage_grid.cells()) { + std::vector> submaps_per_cell( + cell.second); + + // In case there are several submaps covering the cell, only the freshest + // submaps are kept. + if (submaps_per_cell.size() > fresh_submaps_count) { + // Sort by time in descending order. + std::sort(submaps_per_cell.begin(), submaps_per_cell.end(), + [](const std::pair& left, + const std::pair& right) { + return left.second > right.second; + }); + submaps_per_cell.erase(submaps_per_cell.begin() + fresh_submaps_count, + submaps_per_cell.end()); + } + for (const std::pair& submap : submaps_per_cell) { + ++submap_to_covered_cells_count[submap.first]; + } + } + std::vector submap_ids_to_keep; + for (const auto& id_to_cells_count : submap_to_covered_cells_count) { + if (id_to_cells_count.second < min_covered_cells_count) continue; + submap_ids_to_keep.push_back(id_to_cells_count.first); + } + + DCHECK(std::is_sorted(submap_ids_to_keep.begin(), submap_ids_to_keep.end())); + std::vector result; + std::set_difference(all_submap_ids.begin(), all_submap_ids.end(), + submap_ids_to_keep.begin(), submap_ids_to_keep.end(), + std::back_inserter(result)); + return result; +} + +} // namespace + +void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { + const auto submap_data = pose_graph->GetOptimizedSubmapData(); + if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) { + return; + } + + const MapLimits first_submap_map_limits = + std::static_pointer_cast(submap_data.begin()->data.submap) + ->grid() + ->limits(); + SubmapCoverageGrid2D coverage_grid(first_submap_map_limits); + const std::map submap_freshness = + ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(), + pose_graph->GetConstraints()); + const std::set all_submap_ids = AddSubmapsToSubmapCoverageGrid2D( + submap_freshness, submap_data, &coverage_grid); + const std::vector submap_ids_to_remove = FindSubmapIdsToTrim( + coverage_grid, all_submap_ids, fresh_submaps_count_, + min_covered_area_ / common::Pow2(coverage_grid.resolution())); + current_submap_count_ = submap_data.size() - submap_ids_to_remove.size(); + for (const SubmapId& id : submap_ids_to_remove) { + pose_graph->TrimSubmap(id); + } +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h new file mode 100644 index 0000000..bd74691 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h @@ -0,0 +1,57 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_ + +#include "cartographer/common/port.h" +#include "cartographer/mapping/pose_graph_trimmer.h" + +namespace cartographer { +namespace mapping { + +// Trims submaps that have less than 'min_covered_cells_count' cells not +// overlapped by at least 'fresh_submaps_count` submaps. +class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer { + public: + OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count, + double min_covered_area, + uint16 min_added_submaps_count) + : fresh_submaps_count_(fresh_submaps_count), + min_covered_area_(min_covered_area), + min_added_submaps_count_(min_added_submaps_count) {} + ~OverlappingSubmapsTrimmer2D() override = default; + + void Trim(Trimmable* pose_graph) override; + bool IsFinished() override { return finished_; } + + private: + // Number of the most recent submaps to keep. + const uint16 fresh_submaps_count_; + // Minimum area of covered space to keep submap from trimming measured in m^2. + const double min_covered_area_; + // Number of added submaps before the trimmer is invoked. + const uint16 min_added_submaps_count_; + // Current finished submap count. + uint16 current_submap_count_ = 0; + + bool finished_ = false; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc new file mode 100644 index 0000000..1f2b985 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -0,0 +1,279 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h" + +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/testing/fake_trimmable.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using ::cartographer::transform::Rigid2d; +using ::cartographer::transform::Rigid3d; +using ::testing::ElementsAre; +using ::testing::Field; +using ::testing::IsEmpty; + +class OverlappingSubmapsTrimmer2DTest : public ::testing::Test { + protected: + // Creates a submap with num_cells x num_cells grid. + void AddSquareSubmap(const Rigid2d& global_to_submap_frame, + const Rigid2d& local_to_submap_frame, + const Eigen::Vector2d& submap_corner, int submap_index, + int num_cells, bool is_finished) { + proto::Submap2D submap_2d; + submap_2d.set_num_range_data(1); + submap_2d.set_finished(is_finished); + *submap_2d.mutable_local_pose() = + transform::ToProto(transform::Embed3D(local_to_submap_frame)); + + auto* grid = submap_2d.mutable_grid(); + for (int i = 0; i < num_cells * num_cells; ++i) { + grid->add_cells(1); + } + + auto* map_limits = grid->mutable_limits(); + map_limits->set_resolution(1.0); + *map_limits->mutable_max() = transform::ToProto(submap_corner); + map_limits->mutable_cell_limits()->set_num_x_cells(num_cells); + map_limits->mutable_cell_limits()->set_num_y_cells(num_cells); + + auto* know_cells_box = grid->mutable_known_cells_box(); + know_cells_box->set_min_x(0); + know_cells_box->set_min_y(0); + know_cells_box->set_max_x(num_cells - 1); + know_cells_box->set_max_y(num_cells - 1); + + grid->mutable_probability_grid_2d(); + fake_pose_graph_.mutable_submap_data()->Insert( + {0 /* trajectory_id */, submap_index}, + {std::make_shared(submap_2d, &conversion_tables_), + transform::Embed3D(global_to_submap_frame)}); + } + + void AddTrajectoryNode(int node_index, int64 timestamp) { + TrajectoryNode::Data data; + data.time = common::FromUniversal(timestamp); + + fake_pose_graph_.mutable_trajectory_nodes()->Insert( + NodeId{0 /* trajectory_id */, node_index}, + {std::make_shared(data), {} /* pose */}); + } + + void AddConstraint(int submap_index, int node_index, bool is_intra_submap) { + fake_pose_graph_.mutable_constraints()->push_back( + {SubmapId{0 /* trajectory_id */, submap_index}, + NodeId{0 /* trajectory_id */, node_index}, + {} /* pose */, + is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP + : PoseGraphInterface::Constraint::INTER_SUBMAP}); + } + + ValueConversionTables conversion_tables_; + testing::FakeTrimmable fake_pose_graph_; +}; + +::testing::Matcher EqualsSubmapId(const SubmapId& expected) { + return ::testing::AllOf( + Field(&SubmapId::trajectory_id, expected.trajectory_id), + Field(&SubmapId::submap_index, expected.submap_index)); +} + +TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) { + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_area */, + 0 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); +} + +TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) { + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 0 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 1 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_area */, + 0 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), + ElementsAre(EqualsSubmapId({0, 0}))); +} + +TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) { + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 0 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 1 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_area */, + 2 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); + + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 2 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */); + AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), + ElementsAre(EqualsSubmapId({0, 0}), EqualsSubmapId({0, 1}))); +} + +TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) { + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 0 /* submap_index */, 1 /* num_cells */, + false /* is_finished */); + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 1 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_area */, + 0 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); +} + +TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) { + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 0 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 1 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, false); + AddConstraint(0 /*submap_index*/, 2 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_area */, + 0 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), + ElementsAre(EqualsSubmapId({0, 1}))); +} + +// This test covers two 4-cells submaps that overlap each other displaced like: +// ___ +// _| | +// | |_ _| +// |___| +// +// The background submap should be trimmed, since it has only 3 cells +// not-covered by another submap. +TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) { + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 0 /* submap_index */, 2 /* num_cells */, + true /* is_finished */); + AddSquareSubmap(Rigid2d::Translation( + Eigen::Vector2d(1., 1.)) /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 1 /* submap_index */, 2 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 4 /* min_covered_cells_count */, + 0 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), + ElementsAre(EqualsSubmapId({0, 0}))); +} + +TEST_F(OverlappingSubmapsTrimmer2DTest, TestTransformations) { + AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */, + Rigid2d::Identity() /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 0 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + const Rigid2d transform(Eigen::Vector2d(1., 1.), M_PI / 2); + AddSquareSubmap(transform /* global_from_submap_frame */, + transform /* local_from_submap_frame */, + Eigen::Vector2d(1., 1.) /* submap corner */, + 1 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_area */, + 0 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), + ElementsAre(EqualsSubmapId({0, 0}))); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d.cc new file mode 100644 index 0000000..060277e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -0,0 +1,1340 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/2d/pose_graph_2d.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h" +#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); +static auto* kWorkQueueSizeMetric = metrics::Gauge::Null(); +static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); +static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null(); +static auto* kActiveSubmapsMetric = metrics::Gauge::Null(); +static auto* kFrozenSubmapsMetric = metrics::Gauge::Null(); +static auto* kDeletedSubmapsMetric = metrics::Gauge::Null(); + +PoseGraph2D::PoseGraph2D( + const proto::PoseGraphOptions& options, + std::unique_ptr optimization_problem, + common::ThreadPool* thread_pool) + : options_(options), + optimization_problem_(std::move(optimization_problem)), + constraint_builder_(options_.constraint_builder_options(), thread_pool), + thread_pool_(thread_pool) { + if (options.has_overlapping_submaps_trimmer_2d()) { + const auto& trimmer_options = options.overlapping_submaps_trimmer_2d(); + AddTrimmer(absl::make_unique( + trimmer_options.fresh_submaps_count(), + trimmer_options.min_covered_area(), + trimmer_options.min_added_submaps_count())); + } +} + +PoseGraph2D::~PoseGraph2D() { + WaitForAllComputations(); + absl::MutexLock locker(&work_queue_mutex_); + CHECK(work_queue_ == nullptr); +} + +std::vector PoseGraph2D::InitializeGlobalSubmapPoses( + const int trajectory_id, const common::Time time, + const std::vector>& insertion_submaps) { + CHECK(!insertion_submaps.empty()); + const auto& submap_data = optimization_problem_->submap_data(); + if (insertion_submaps.size() == 1) { + // If we don't already have an entry for the first submap, add one. + if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + if (data_.initial_trajectory_poses.count(trajectory_id) > 0) { + data_.trajectory_connectivity_state.Connect( + trajectory_id, + data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id, + time); + } + optimization_problem_->AddSubmap( + trajectory_id, transform::Project2D( + ComputeLocalToGlobalTransform( + data_.global_submap_poses_2d, trajectory_id) * + insertion_submaps[0]->local_pose())); + } + CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); + const SubmapId submap_id{trajectory_id, 0}; + CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front()); + return {submap_id}; + } + CHECK_EQ(2, insertion_submaps.size()); + const auto end_it = submap_data.EndOfTrajectory(trajectory_id); + CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); + const SubmapId last_submap_id = std::prev(end_it)->id; + if (data_.submap_data.at(last_submap_id).submap == + insertion_submaps.front()) { + // In this case, 'last_submap_id' is the ID of + // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new. + const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; + optimization_problem_->AddSubmap( + trajectory_id, + first_submap_pose * + constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() * + constraints::ComputeSubmapPose(*insertion_submaps[1])); + return {last_submap_id, + SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; + } + CHECK(data_.submap_data.at(last_submap_id).submap == + insertion_submaps.back()); + const SubmapId front_submap_id{trajectory_id, + last_submap_id.submap_index - 1}; + CHECK(data_.submap_data.at(front_submap_id).submap == + insertion_submaps.front()); + return {front_submap_id, last_submap_id}; +} + +NodeId PoseGraph2D::AppendNode( + std::shared_ptr constant_data, + const int trajectory_id, + const std::vector>& insertion_submaps, + const transform::Rigid3d& optimized_pose) { + absl::MutexLock locker(&mutex_); + AddTrajectoryIfNeeded(trajectory_id); + if (!CanAddWorkItemModifying(trajectory_id)) { + LOG(WARNING) << "AddNode was called for finished or deleted trajectory."; + } + const NodeId node_id = data_.trajectory_nodes.Append( + trajectory_id, TrajectoryNode{constant_data, optimized_pose}); + ++data_.num_trajectory_nodes; + // Test if the 'insertion_submap.back()' is one we never saw before. + if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + std::prev(data_.submap_data.EndOfTrajectory(trajectory_id)) + ->data.submap != insertion_submaps.back()) { + // We grow 'data_.submap_data' as needed. This code assumes that the first + // time we see a new submap is as 'insertion_submaps.back()'. + const SubmapId submap_id = + data_.submap_data.Append(trajectory_id, InternalSubmapData()); + data_.submap_data.at(submap_id).submap = insertion_submaps.back(); + LOG(INFO) << "Inserted submap " << submap_id << "."; + kActiveSubmapsMetric->Increment(); + } + return node_id; +} + +NodeId PoseGraph2D::AddNode( + std::shared_ptr constant_data, + const int trajectory_id, + const std::vector>& insertion_submaps) { + const transform::Rigid3d optimized_pose( + GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); + + const NodeId node_id = AppendNode(constant_data, trajectory_id, + insertion_submaps, optimized_pose); + // We have to check this here, because it might have changed by the time we + // execute the lambda. + const bool newly_finished_submap = + insertion_submaps.front()->insertion_finished(); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + return ComputeConstraintsForNode(node_id, insertion_submaps, + newly_finished_submap); + }); + return node_id; +} + +void PoseGraph2D::AddWorkItem( + const std::function& work_item) { + absl::MutexLock locker(&work_queue_mutex_); + if (work_queue_ == nullptr) { + work_queue_ = absl::make_unique(); + auto task = absl::make_unique(); + task->SetWorkItem([this]() { DrainWorkQueue(); }); + thread_pool_->Schedule(std::move(task)); + } + const auto now = std::chrono::steady_clock::now(); + work_queue_->push_back({now, work_item}); + kWorkQueueSizeMetric->Set(work_queue_->size()); + kWorkQueueDelayMetric->Set( + std::chrono::duration_cast>( + now - work_queue_->front().time) + .count()); +} + +void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { + data_.trajectories_state[trajectory_id]; + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::FINISHED); + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::DELETED); + CHECK(data_.trajectories_state.at(trajectory_id).deletion_state == + InternalTrajectoryState::DeletionState::NORMAL); + data_.trajectory_connectivity_state.Add(trajectory_id); + // Make sure we have a sampler for this trajectory. + if (!global_localization_samplers_[trajectory_id]) { + global_localization_samplers_[trajectory_id] = + absl::make_unique( + options_.global_sampling_ratio()); + } +} + +void PoseGraph2D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddImuData(trajectory_id, imu_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::AddOdometryData(const int trajectory_id, + const sensor::OdometryData& odometry_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddOdometryData(trajectory_id, odometry_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddFixedFramePoseData(trajectory_id, + fixed_frame_pose_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + for (const auto& observation : landmark_data.landmark_observations) { + data_.landmark_nodes[observation.id].landmark_observations.emplace_back( + PoseGraphInterface::LandmarkNode::LandmarkObservation{ + trajectory_id, landmark_data.time, + observation.landmark_to_tracking_transform, + observation.translation_weight, observation.rotation_weight}); + } + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::ComputeConstraint(const NodeId& node_id, + const SubmapId& submap_id) { + bool maybe_add_local_constraint = false; + bool maybe_add_global_constraint = false; + const TrajectoryNode::Data* constant_data; + const Submap2D* submap; + { + absl::MutexLock locker(&mutex_); + CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); + if (!data_.submap_data.at(submap_id).submap->insertion_finished()) { + // Uplink server only receives grids when they are finished, so skip + // constraint search before that. + return; + } + + const common::Time node_time = GetLatestNodeTime(node_id, submap_id); + const common::Time last_connection_time = + data_.trajectory_connectivity_state.LastConnectionTime( + node_id.trajectory_id, submap_id.trajectory_id); + if (node_id.trajectory_id == submap_id.trajectory_id || + node_time < + last_connection_time + + common::FromSeconds( + options_.global_constraint_search_after_n_seconds())) { + // If the node and the submap belong to the same trajectory or if there + // has been a recent global constraint that ties that node's trajectory to + // the submap's trajectory, it suffices to do a match constrained to a + // local search window. + maybe_add_local_constraint = true; + } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { + maybe_add_global_constraint = true; + } + constant_data = data_.trajectory_nodes.at(node_id).constant_data.get(); + submap = static_cast( + data_.submap_data.at(submap_id).submap.get()); + } + + if (maybe_add_local_constraint) { + const transform::Rigid2d initial_relative_pose = + optimization_problem_->submap_data() + .at(submap_id) + .global_pose.inverse() * + optimization_problem_->node_data().at(node_id).global_pose_2d; + constraint_builder_.MaybeAddConstraint( + submap_id, submap, node_id, constant_data, initial_relative_pose); + } else if (maybe_add_global_constraint) { + constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id, + constant_data); + } +} + +WorkItem::Result PoseGraph2D::ComputeConstraintsForNode( + const NodeId& node_id, + std::vector> insertion_submaps, + const bool newly_finished_submap) { + std::vector submap_ids; + std::vector finished_submap_ids; + std::set newly_finished_submap_node_ids; + { + absl::MutexLock locker(&mutex_); + const auto& constant_data = + data_.trajectory_nodes.at(node_id).constant_data; + submap_ids = InitializeGlobalSubmapPoses( + node_id.trajectory_id, constant_data->time, insertion_submaps); + CHECK_EQ(submap_ids.size(), insertion_submaps.size()); + const SubmapId matching_id = submap_ids.front(); + const transform::Rigid2d local_pose_2d = + transform::Project2D(constant_data->local_pose * + transform::Rigid3d::Rotation( + constant_data->gravity_alignment.inverse())); + const transform::Rigid2d global_pose_2d = + optimization_problem_->submap_data().at(matching_id).global_pose * + constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() * + local_pose_2d; + optimization_problem_->AddTrajectoryNode( + matching_id.trajectory_id, + optimization::NodeSpec2D{constant_data->time, local_pose_2d, + global_pose_2d, + constant_data->gravity_alignment}); + for (size_t i = 0; i < insertion_submaps.size(); ++i) { + const SubmapId submap_id = submap_ids[i]; + // Even if this was the last node added to 'submap_id', the submap will + // only be marked as finished in 'data_.submap_data' further below. + CHECK(data_.submap_data.at(submap_id).state == + SubmapState::kNoConstraintSearch); + data_.submap_data.at(submap_id).node_ids.emplace(node_id); + const transform::Rigid2d constraint_transform = + constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * + local_pose_2d; + data_.constraints.push_back( + Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + options_.matcher_translation_weight(), + options_.matcher_rotation_weight()}, + Constraint::INTRA_SUBMAP}); + } + + // TODO(gaschler): Consider not searching for constraints against + // trajectories scheduled for deletion. + // TODO(danielsievers): Add a member variable and avoid having to copy + // them out here. + for (const auto& submap_id_data : data_.submap_data) { + if (submap_id_data.data.state == SubmapState::kFinished) { + CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); + finished_submap_ids.emplace_back(submap_id_data.id); + } + } + if (newly_finished_submap) { + const SubmapId newly_finished_submap_id = submap_ids.front(); + InternalSubmapData& finished_submap_data = + data_.submap_data.at(newly_finished_submap_id); + CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch); + finished_submap_data.state = SubmapState::kFinished; + newly_finished_submap_node_ids = finished_submap_data.node_ids; + } + } + + for (const auto& submap_id : finished_submap_ids) { + ComputeConstraint(node_id, submap_id); + } + + if (newly_finished_submap) { + const SubmapId newly_finished_submap_id = submap_ids.front(); + // We have a new completed submap, so we look into adding constraints for + // old nodes. + for (const auto& node_id_data : optimization_problem_->node_data()) { + const NodeId& node_id = node_id_data.id; + if (newly_finished_submap_node_ids.count(node_id) == 0) { + ComputeConstraint(node_id, newly_finished_submap_id); + } + } + } + constraint_builder_.NotifyEndOfNode(); + absl::MutexLock locker(&mutex_); + ++num_nodes_since_last_loop_closure_; + if (options_.optimize_every_n_nodes() > 0 && + num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) { + return WorkItem::Result::kRunOptimization; + } + return WorkItem::Result::kDoNotRunOptimization; +} + +common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id, + const SubmapId& submap_id) const { + common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time; + const InternalSubmapData& submap_data = data_.submap_data.at(submap_id); + if (!submap_data.node_ids.empty()) { + const NodeId last_submap_node_id = + *data_.submap_data.at(submap_id).node_ids.rbegin(); + time = std::max( + time, + data_.trajectory_nodes.at(last_submap_node_id).constant_data->time); + } + return time; +} + +void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) { + CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP); + const common::Time time = + GetLatestNodeTime(constraint.node_id, constraint.submap_id); + data_.trajectory_connectivity_state.Connect( + constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, + time); +} + +void PoseGraph2D::DeleteTrajectoriesIfNeeded() { + TrimmingHandle trimming_handle(this); + for (auto& it : data_.trajectories_state) { + if (it.second.deletion_state == + InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION) { + // TODO(gaschler): Consider directly deleting from data_, which may be + // more complete. + auto submap_ids = trimming_handle.GetSubmapIds(it.first); + for (auto& submap_id : submap_ids) { + trimming_handle.TrimSubmap(submap_id); + } + it.second.state = TrajectoryState::DELETED; + it.second.deletion_state = InternalTrajectoryState::DeletionState::NORMAL; + } + } +} + +void PoseGraph2D::HandleWorkQueue( + const constraints::ConstraintBuilder2D::Result& result) { + { + absl::MutexLock locker(&mutex_); + data_.constraints.insert(data_.constraints.end(), result.begin(), + result.end()); + } + RunOptimization(); + + if (global_slam_optimization_callback_) { + std::map trajectory_id_to_last_optimized_node_id; + std::map trajectory_id_to_last_optimized_submap_id; + { + absl::MutexLock locker(&mutex_); + const auto& submap_data = optimization_problem_->submap_data(); + const auto& node_data = optimization_problem_->node_data(); + for (const int trajectory_id : node_data.trajectory_ids()) { + if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + continue; + } + trajectory_id_to_last_optimized_node_id.emplace( + trajectory_id, + std::prev(node_data.EndOfTrajectory(trajectory_id))->id); + trajectory_id_to_last_optimized_submap_id.emplace( + trajectory_id, + std::prev(submap_data.EndOfTrajectory(trajectory_id))->id); + } + } + global_slam_optimization_callback_( + trajectory_id_to_last_optimized_submap_id, + trajectory_id_to_last_optimized_node_id); + } + + { + absl::MutexLock locker(&mutex_); + for (const Constraint& constraint : result) { + UpdateTrajectoryConnectivity(constraint); + } + DeleteTrajectoriesIfNeeded(); + TrimmingHandle trimming_handle(this); + for (auto& trimmer : trimmers_) { + trimmer->Trim(&trimming_handle); + } + trimmers_.erase( + std::remove_if(trimmers_.begin(), trimmers_.end(), + [](std::unique_ptr& trimmer) { + return trimmer->IsFinished(); + }), + trimmers_.end()); + + num_nodes_since_last_loop_closure_ = 0; + + // Update the gauges that count the current number of constraints. + double inter_constraints_same_trajectory = 0; + double inter_constraints_different_trajectory = 0; + for (const auto& constraint : data_.constraints) { + if (constraint.tag == + cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + if (constraint.node_id.trajectory_id == + constraint.submap_id.trajectory_id) { + ++inter_constraints_same_trajectory; + } else { + ++inter_constraints_different_trajectory; + } + } + kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory); + kConstraintsDifferentTrajectoryMetric->Set( + inter_constraints_different_trajectory); + } + + DrainWorkQueue(); +} + +void PoseGraph2D::DrainWorkQueue() { + bool process_work_queue = true; + size_t work_queue_size; + while (process_work_queue) { + std::function work_item; + { + absl::MutexLock locker(&work_queue_mutex_); + if (work_queue_->empty()) { + work_queue_.reset(); + return; + } + work_item = work_queue_->front().task; + work_queue_->pop_front(); + work_queue_size = work_queue_->size(); + kWorkQueueSizeMetric->Set(work_queue_size); + } + process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; + } + LOG(INFO) << "Remaining work items in queue: " << work_queue_size; + // We have to optimize again. + constraint_builder_.WhenDone( + [this](const constraints::ConstraintBuilder2D::Result& result) { + HandleWorkQueue(result); + }); +} + +void PoseGraph2D::WaitForAllComputations() { + int num_trajectory_nodes; + { + absl::MutexLock locker(&mutex_); + num_trajectory_nodes = data_.num_trajectory_nodes; + } + + const int num_finished_nodes_at_start = + constraint_builder_.GetNumFinishedNodes(); + + auto report_progress = [this, num_trajectory_nodes, + num_finished_nodes_at_start]() { + // Log progress on nodes only when we are actually processing nodes. + if (num_trajectory_nodes != num_finished_nodes_at_start) { + std::ostringstream progress_info; + progress_info << "Optimizing: " << std::fixed << std::setprecision(1) + << 100. * + (constraint_builder_.GetNumFinishedNodes() - + num_finished_nodes_at_start) / + (num_trajectory_nodes - num_finished_nodes_at_start) + << "%..."; + std::cout << "\r\x1b[K" << progress_info.str() << std::flush; + } + }; + + // First wait for the work queue to drain so that it's safe to schedule + // a WhenDone() callback. + { + const auto predicate = [this]() + EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) { + return work_queue_ == nullptr; + }; + absl::MutexLock locker(&work_queue_mutex_); + while (!work_queue_mutex_.AwaitWithTimeout( + absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { + report_progress(); + } + } + + // Now wait for any pending constraint computations to finish. + absl::MutexLock locker(&mutex_); + bool notification = false; + constraint_builder_.WhenDone( + [this, + ¬ification](const constraints::ConstraintBuilder2D::Result& result) + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.constraints.insert(data_.constraints.end(), result.begin(), + result.end()); + notification = true; + }); + const auto predicate = [¬ification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return notification; + }; + while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { + report_progress(); + } + CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes); + std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; +} + +void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { + { + absl::MutexLock locker(&mutex_); + auto it = data_.trajectories_state.find(trajectory_id); + if (it == data_.trajectories_state.end()) { + LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " + << trajectory_id; + return; + } + it->second.deletion_state = + InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; + } + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::ACTIVE); + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::DELETED); + CHECK(data_.trajectories_state.at(trajectory_id).deletion_state == + InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION); + data_.trajectories_state.at(trajectory_id).deletion_state = + InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::FinishTrajectory(const int trajectory_id) { + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + CHECK(!IsTrajectoryFinished(trajectory_id)); + data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; + + for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) { + data_.submap_data.at(submap.id).state = SubmapState::kFinished; + } + return WorkItem::Result::kRunOptimization; + }); +} + +bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { + return data_.trajectories_state.count(trajectory_id) != 0 && + data_.trajectories_state.at(trajectory_id).state == + TrajectoryState::FINISHED; +} + +void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { + { + absl::MutexLock locker(&mutex_); + data_.trajectory_connectivity_state.Add(trajectory_id); + } + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + CHECK(!IsTrajectoryFrozen(trajectory_id)); + // Connect multiple frozen trajectories among each other. + // This is required for localization against multiple frozen trajectories + // because we lose inter-trajectory constraints when freezing. + for (const auto& entry : data_.trajectories_state) { + const int other_trajectory_id = entry.first; + if (!IsTrajectoryFrozen(other_trajectory_id)) { + continue; + } + if (data_.trajectory_connectivity_state.TransitivelyConnected( + trajectory_id, other_trajectory_id)) { + // Already connected, nothing to do. + continue; + } + data_.trajectory_connectivity_state.Connect( + trajectory_id, other_trajectory_id, common::FromUniversal(0)); + } + data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const { + return data_.trajectories_state.count(trajectory_id) != 0 && + data_.trajectories_state.at(trajectory_id).state == + TrajectoryState::FROZEN; +} + +void PoseGraph2D::AddSubmapFromProto( + const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { + if (!submap.has_submap_2d()) { + return; + } + + const SubmapId submap_id = {submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; + + const transform::Rigid2d global_submap_pose_2d = + transform::Project2D(global_submap_pose); + { + absl::MutexLock locker(&mutex_); + const std::shared_ptr submap_ptr = + std::make_shared(submap.submap_2d(), + &conversion_tables_); + AddTrajectoryIfNeeded(submap_id.trajectory_id); + if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; + data_.submap_data.Insert(submap_id, InternalSubmapData()); + data_.submap_data.at(submap_id).submap = submap_ptr; + // Immediately show the submap at the 'global_submap_pose'. + data_.global_submap_poses_2d.Insert( + submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); + } + + // TODO(MichaelGrupp): MapBuilder does freezing before deserializing submaps, + // so this should be fine. + if (IsTrajectoryFrozen(submap_id.trajectory_id)) { + kFrozenSubmapsMetric->Increment(); + } else { + kActiveSubmapsMetric->Increment(); + } + + AddWorkItem( + [this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.submap_data.at(submap_id).state = SubmapState::kFinished; + optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, + const proto::Node& node) { + const NodeId node_id = {node.node_id().trajectory_id(), + node.node_id().node_index()}; + std::shared_ptr constant_data = + std::make_shared(FromProto(node.node_data())); + + { + absl::MutexLock locker(&mutex_); + AddTrajectoryIfNeeded(node_id.trajectory_id); + if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; + data_.trajectory_nodes.Insert(node_id, + TrajectoryNode{constant_data, global_pose}); + } + + AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + const auto& constant_data = + data_.trajectory_nodes.at(node_id).constant_data; + const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( + constant_data->gravity_alignment.inverse()); + optimization_problem_->InsertTrajectoryNode( + node_id, + optimization::NodeSpec2D{ + constant_data->time, + transform::Project2D(constant_data->local_pose * + gravity_alignment_inverse), + transform::Project2D(global_pose * gravity_alignment_inverse), + constant_data->gravity_alignment}); + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::SetTrajectoryDataFromProto( + const proto::TrajectoryData& data) { + TrajectoryData trajectory_data; + // gravity_constant and imu_calibration are omitted as its not used in 2d + + if (data.has_fixed_frame_origin_in_map()) { + trajectory_data.fixed_frame_origin_in_map = + transform::ToRigid3(data.fixed_frame_origin_in_map()); + + const int trajectory_id = data.trajectory_id(); + AddWorkItem([this, trajectory_id, trajectory_data]() + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->SetTrajectoryData( + trajectory_id, trajectory_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); + } +} + +void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) { + AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(submap_id.trajectory_id)) { + data_.submap_data.at(submap_id).node_ids.insert(node_id); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::AddSerializedConstraints( + const std::vector& constraints) { + AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + for (const auto& constraint : constraints) { + CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); + CHECK(data_.submap_data.Contains(constraint.submap_id)); + CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data != + nullptr); + CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr); + switch (constraint.tag) { + case Constraint::Tag::INTRA_SUBMAP: + CHECK(data_.submap_data.at(constraint.submap_id) + .node_ids.emplace(constraint.node_id) + .second); + break; + case Constraint::Tag::INTER_SUBMAP: + UpdateTrajectoryConnectivity(constraint); + break; + } + const Constraint::Pose pose = { + constraint.pose.zbar_ij * + transform::Rigid3d::Rotation( + data_.trajectory_nodes.at(constraint.node_id) + .constant_data->gravity_alignment.inverse()), + constraint.pose.translation_weight, constraint.pose.rotation_weight}; + data_.constraints.push_back(Constraint{ + constraint.submap_id, constraint.node_id, pose, constraint.tag}); + } + LOG(INFO) << "Loaded " << constraints.size() << " constraints."; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::AddTrimmer(std::unique_ptr trimmer) { + // C++11 does not allow us to move a unique_ptr into a lambda. + PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); + AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + trimmers_.emplace_back(trimmer_ptr); + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph2D::RunFinalOptimization() { + { + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + optimization_problem_->SetMaxNumIterations( + options_.max_num_final_iterations()); + return WorkItem::Result::kRunOptimization; + }); + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + optimization_problem_->SetMaxNumIterations( + options_.optimization_problem_options() + .ceres_solver_options() + .max_num_iterations()); + return WorkItem::Result::kDoNotRunOptimization; + }); + } + WaitForAllComputations(); +} + +void PoseGraph2D::RunOptimization() { + if (optimization_problem_->submap_data().empty()) { + return; + } + + // No other thread is accessing the optimization_problem_, + // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes + // when executing the Solve. Solve is time consuming, so not taking the mutex + // before Solve to avoid blocking foreground processing. + optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), + data_.landmark_nodes); + absl::MutexLock locker(&mutex_); + + const auto& submap_data = optimization_problem_->submap_data(); + const auto& node_data = optimization_problem_->node_data(); + for (const int trajectory_id : node_data.trajectory_ids()) { + for (const auto& node : node_data.trajectory(trajectory_id)) { + auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id); + mutable_trajectory_node.global_pose = + transform::Embed3D(node.data.global_pose_2d) * + transform::Rigid3d::Rotation( + mutable_trajectory_node.constant_data->gravity_alignment); + } + + // Extrapolate all point cloud poses that were not included in the + // 'optimization_problem_' yet. + const auto local_to_new_global = + ComputeLocalToGlobalTransform(submap_data, trajectory_id); + const auto local_to_old_global = ComputeLocalToGlobalTransform( + data_.global_submap_poses_2d, trajectory_id); + const transform::Rigid3d old_global_to_new_global = + local_to_new_global * local_to_old_global.inverse(); + + const NodeId last_optimized_node_id = + std::prev(node_data.EndOfTrajectory(trajectory_id))->id; + auto node_it = + std::next(data_.trajectory_nodes.find(last_optimized_node_id)); + for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id); + ++node_it) { + auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id); + mutable_trajectory_node.global_pose = + old_global_to_new_global * mutable_trajectory_node.global_pose; + } + } + for (const auto& landmark : optimization_problem_->landmark_data()) { + data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second; + } + data_.global_submap_poses_2d = submap_data; +} + +bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) { + auto it = data_.trajectories_state.find(trajectory_id); + if (it == data_.trajectories_state.end()) { + return true; + } + if (it->second.state == TrajectoryState::FINISHED) { + // TODO(gaschler): Replace all FATAL to WARNING after some testing. + LOG(FATAL) << "trajectory_id " << trajectory_id + << " has finished " + "but modification is requested, skipping."; + return false; + } + if (it->second.deletion_state != + InternalTrajectoryState::DeletionState::NORMAL) { + LOG(FATAL) << "trajectory_id " << trajectory_id + << " has been scheduled for deletion " + "but modification is requested, skipping."; + return false; + } + if (it->second.state == TrajectoryState::DELETED) { + LOG(FATAL) << "trajectory_id " << trajectory_id + << " has been deleted " + "but modification is requested, skipping."; + return false; + } + return true; +} + +MapById PoseGraph2D::GetTrajectoryNodes() const { + absl::MutexLock locker(&mutex_); + return data_.trajectory_nodes; +} + +MapById PoseGraph2D::GetTrajectoryNodePoses() + const { + MapById node_poses; + absl::MutexLock locker(&mutex_); + for (const auto& node_id_data : data_.trajectory_nodes) { + absl::optional constant_pose_data; + if (node_id_data.data.constant_data != nullptr) { + constant_pose_data = TrajectoryNodePose::ConstantPoseData{ + node_id_data.data.constant_data->time, + node_id_data.data.constant_data->local_pose}; + } + node_poses.Insert( + node_id_data.id, + TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data}); + } + return node_poses; +} + +std::map +PoseGraph2D::GetTrajectoryStates() const { + std::map trajectories_state; + absl::MutexLock locker(&mutex_); + for (const auto& it : data_.trajectories_state) { + trajectories_state[it.first] = it.second.state; + } + return trajectories_state; +} + +std::map PoseGraph2D::GetLandmarkPoses() + const { + std::map landmark_poses; + absl::MutexLock locker(&mutex_); + for (const auto& landmark : data_.landmark_nodes) { + // Landmark without value has not been optimized yet. + if (!landmark.second.global_landmark_pose.has_value()) continue; + landmark_poses[landmark.first] = + landmark.second.global_landmark_pose.value(); + } + return landmark_poses; +} + +void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; + data_.landmark_nodes[landmark_id].frozen = frozen; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +sensor::MapByTime PoseGraph2D::GetImuData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->imu_data(); +} + +sensor::MapByTime PoseGraph2D::GetOdometryData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->odometry_data(); +} + +std::map +PoseGraph2D::GetLandmarkNodes() const { + absl::MutexLock locker(&mutex_); + return data_.landmark_nodes; +} + +std::map +PoseGraph2D::GetTrajectoryData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->trajectory_data(); +} + +sensor::MapByTime +PoseGraph2D::GetFixedFramePoseData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->fixed_frame_pose_data(); +} + +std::vector PoseGraph2D::constraints() const { + std::vector result; + absl::MutexLock locker(&mutex_); + for (const Constraint& constraint : data_.constraints) { + result.push_back(Constraint{ + constraint.submap_id, constraint.node_id, + Constraint::Pose{constraint.pose.zbar_ij * + transform::Rigid3d::Rotation( + data_.trajectory_nodes.at(constraint.node_id) + .constant_data->gravity_alignment), + constraint.pose.translation_weight, + constraint.pose.rotation_weight}, + constraint.tag}); + } + return result; +} + +void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id, + const int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) { + absl::MutexLock locker(&mutex_); + data_.initial_trajectory_poses[from_trajectory_id] = + InitialTrajectoryPose{to_trajectory_id, pose, time}; +} + +transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose( + const int trajectory_id, const common::Time time) const { + CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0); + const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time); + if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) { + return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id) + ->data.global_pose; + } + if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) { + return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) + ->data.global_pose; + } + return transform::Interpolate( + transform::TimestampedTransform{std::prev(it)->data.time(), + std::prev(it)->data.global_pose}, + transform::TimestampedTransform{it->data.time(), + it->data.global_pose}, + time) + .transform; +} + +transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( + const int trajectory_id) const { + absl::MutexLock locker(&mutex_); + return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, + trajectory_id); +} + +std::vector> PoseGraph2D::GetConnectedTrajectories() const { + absl::MutexLock locker(&mutex_); + return data_.trajectory_connectivity_state.Components(); +} + +PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData( + const SubmapId& submap_id) const { + absl::MutexLock locker(&mutex_); + return GetSubmapDataUnderLock(submap_id); +} + +MapById +PoseGraph2D::GetAllSubmapData() const { + absl::MutexLock locker(&mutex_); + return GetSubmapDataUnderLock(); +} + +MapById +PoseGraph2D::GetAllSubmapPoses() const { + absl::MutexLock locker(&mutex_); + MapById submap_poses; + for (const auto& submap_id_data : data_.submap_data) { + auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); + submap_poses.Insert( + submap_id_data.id, + PoseGraph::SubmapPose{submap_data.submap->num_range_data(), + submap_data.pose}); + } + return submap_poses; +} + +transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( + const MapById& global_submap_poses, + const int trajectory_id) const { + auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); + auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); + if (begin_it == end_it) { + const auto it = data_.initial_trajectory_poses.find(trajectory_id); + if (it != data_.initial_trajectory_poses.end()) { + return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, + it->second.time) * + it->second.relative_pose; + } else { + return transform::Rigid3d::Identity(); + } + } + const SubmapId last_optimized_submap_id = std::prev(end_it)->id; + // Accessing 'local_pose' in Submap is okay, since the member is const. + return transform::Embed3D( + global_submap_poses.at(last_optimized_submap_id).global_pose) * + data_.submap_data.at(last_optimized_submap_id) + .submap->local_pose() + .inverse(); +} + +PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock( + const SubmapId& submap_id) const { + const auto it = data_.submap_data.find(submap_id); + if (it == data_.submap_data.end()) { + return {}; + } + auto submap = it->data.submap; + if (data_.global_submap_poses_2d.Contains(submap_id)) { + // We already have an optimized pose. + return {submap, + transform::Embed3D( + data_.global_submap_poses_2d.at(submap_id).global_pose)}; + } + // We have to extrapolate. + return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, + submap_id.trajectory_id) * + submap->local_pose()}; +} + +PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent) + : parent_(parent) {} + +int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const { + const auto& submap_data = parent_->optimization_problem_->submap_data(); + return submap_data.SizeOfTrajectoryOrZero(trajectory_id); +} + +MapById +PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const { + MapById submaps; + for (const auto& submap_id_data : parent_->data_.submap_data) { + if (submap_id_data.data.state != SubmapState::kFinished || + !parent_->data_.global_submap_poses_2d.Contains(submap_id_data.id)) { + continue; + } + submaps.Insert( + submap_id_data.id, + SubmapData{submap_id_data.data.submap, + transform::Embed3D(parent_->data_.global_submap_poses_2d + .at(submap_id_data.id) + .global_pose)}); + } + return submaps; +} + +std::vector PoseGraph2D::TrimmingHandle::GetSubmapIds( + int trajectory_id) const { + std::vector submap_ids; + const auto& submap_data = parent_->optimization_problem_->submap_data(); + for (const auto& it : submap_data.trajectory(trajectory_id)) { + submap_ids.push_back(it.id); + } + return submap_ids; +} + +const MapById& +PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const { + return parent_->data_.trajectory_nodes; +} + +const std::vector& +PoseGraph2D::TrimmingHandle::GetConstraints() const { + return parent_->data_.constraints; +} + +bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const { + return parent_->IsTrajectoryFinished(trajectory_id); +} + +void PoseGraph2D::TrimmingHandle::SetTrajectoryState(int trajectory_id, + TrajectoryState state) { + parent_->data_.trajectories_state[trajectory_id].state = state; +} + +void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { + // TODO(hrapp): We have to make sure that the trajectory has been finished + // if we want to delete the last submaps. + CHECK(parent_->data_.submap_data.at(submap_id).state == + SubmapState::kFinished); + + // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps + // once the submap with 'submap_id' is gone. + // We need to use node_ids instead of constraints here to be also compatible + // with frozen trajectories that don't have intra-constraints. + std::set nodes_to_retain; + for (const auto& submap_data : parent_->data_.submap_data) { + if (submap_data.id != submap_id) { + nodes_to_retain.insert(submap_data.data.node_ids.begin(), + submap_data.data.node_ids.end()); + } + } + + // Remove all nodes that are exlusively associated to 'submap_id'. + std::set nodes_to_remove; + std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(), + parent_->data_.submap_data.at(submap_id).node_ids.end(), + nodes_to_retain.begin(), nodes_to_retain.end(), + std::inserter(nodes_to_remove, nodes_to_remove.begin())); + + // Remove all 'data_.constraints' related to 'submap_id'. + { + std::vector constraints; + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.submap_id != submap_id) { + constraints.push_back(constraint); + } + } + parent_->data_.constraints = std::move(constraints); + } + + // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. + { + std::vector constraints; + std::set other_submap_ids_losing_constraints; + for (const Constraint& constraint : parent_->data_.constraints) { + if (nodes_to_remove.count(constraint.node_id) == 0) { + constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); + } + } + parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } + } + + // Mark the submap with 'submap_id' as trimmed and remove its data. + CHECK(parent_->data_.submap_data.at(submap_id).state == + SubmapState::kFinished); + parent_->data_.submap_data.Trim(submap_id); + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + parent_->optimization_problem_->TrimSubmap(submap_id); + + // We have one submap less, update the gauge metrics. + kDeletedSubmapsMetric->Increment(); + if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) { + kFrozenSubmapsMetric->Decrement(); + } else { + kActiveSubmapsMetric->Decrement(); + } + + // Remove the 'nodes_to_remove' from the pose graph and the optimization + // problem. + for (const NodeId& node_id : nodes_to_remove) { + parent_->data_.trajectory_nodes.Trim(node_id); + parent_->optimization_problem_->TrimTrajectoryNode(node_id); + } +} + +MapById +PoseGraph2D::GetSubmapDataUnderLock() const { + MapById submaps; + for (const auto& submap_id_data : data_.submap_data) { + submaps.Insert(submap_id_data.id, + GetSubmapDataUnderLock(submap_id_data.id)); + } + return submaps; +} + +void PoseGraph2D::SetGlobalSlamOptimizationCallback( + PoseGraphInterface::GlobalSlamOptimizationCallback callback) { + global_slam_optimization_callback_ = callback; +} + +void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) { + auto* latency = family_factory->NewGaugeFamily( + "mapping_2d_pose_graph_work_queue_delay", + "Age of the oldest entry in the work queue in seconds"); + kWorkQueueDelayMetric = latency->Add({}); + auto* queue_size = + family_factory->NewGaugeFamily("mapping_2d_pose_graph_work_queue_size", + "Number of items in the work queue"); + kWorkQueueSizeMetric = queue_size->Add({}); + auto* constraints = family_factory->NewGaugeFamily( + "mapping_2d_pose_graph_constraints", + "Current number of constraints in the pose graph"); + kConstraintsDifferentTrajectoryMetric = + constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}}); + kConstraintsSameTrajectoryMetric = + constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}}); + auto* submaps = family_factory->NewGaugeFamily( + "mapping_2d_pose_graph_submaps", "Number of submaps in the pose graph."); + kActiveSubmapsMetric = submaps->Add({{"state", "active"}}); + kFrozenSubmapsMetric = submaps->Add({{"state", "frozen"}}); + kDeletedSubmapsMetric = submaps->Add({{"state", "deleted"}}); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d.h new file mode 100644 index 0000000..3733425 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -0,0 +1,301 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_2D_POSE_GRAPH_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/container/flat_hash_map.h" +#include "absl/synchronization/mutex.h" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" +#include "cartographer/mapping/internal/pose_graph_data.h" +#include "cartographer/mapping/internal/trajectory_connectivity_state.h" +#include "cartographer/mapping/internal/work_queue.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph_trimmer.h" +#include "cartographer/mapping/value_conversion_tables.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +// Implements the loop closure method called Sparse Pose Adjustment (SPA) from +// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." +// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference +// on (pp. 22--29). IEEE, 2010. +// +// It is extended for submapping: +// Each node has been matched against one or more submaps (adding a constraint +// for each match), both poses of nodes and of submaps are to be optimized. +// All constraints are between a submap i and a node j. +class PoseGraph2D : public PoseGraph { + public: + PoseGraph2D( + const proto::PoseGraphOptions& options, + std::unique_ptr optimization_problem, + common::ThreadPool* thread_pool); + ~PoseGraph2D() override; + + PoseGraph2D(const PoseGraph2D&) = delete; + PoseGraph2D& operator=(const PoseGraph2D&) = delete; + + // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was + // determined by scan matching against 'insertion_submaps.front()' and the + // node data was inserted into the 'insertion_submaps'. If + // 'insertion_submaps.front().finished()' is 'true', data was inserted into + // this submap for the last time. + NodeId AddNode( + std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& insertion_submaps) + LOCKS_EXCLUDED(mutex_); + + void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override + LOCKS_EXCLUDED(mutex_); + void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) override + LOCKS_EXCLUDED(mutex_); + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) override + LOCKS_EXCLUDED(mutex_); + void AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) override + LOCKS_EXCLUDED(mutex_); + + void DeleteTrajectory(int trajectory_id) override; + void FinishTrajectory(int trajectory_id) override; + bool IsTrajectoryFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void FreezeTrajectory(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, + const proto::Submap& submap) override; + void AddNodeFromProto(const transform::Rigid3d& global_pose, + const proto::Node& node) override; + void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override; + void AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) override; + void AddSerializedConstraints( + const std::vector& constraints) override; + void AddTrimmer(std::unique_ptr trimmer) override; + void RunFinalOptimization() override; + std::vector> GetConnectedTrajectories() const override + LOCKS_EXCLUDED(mutex_); + PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const + LOCKS_EXCLUDED(mutex_) override; + MapById GetAllSubmapData() const + LOCKS_EXCLUDED(mutex_) override; + MapById GetAllSubmapPoses() const + LOCKS_EXCLUDED(mutex_) override; + transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const + LOCKS_EXCLUDED(mutex_) override; + MapById GetTrajectoryNodes() const override + LOCKS_EXCLUDED(mutex_); + MapById GetTrajectoryNodePoses() const override + LOCKS_EXCLUDED(mutex_); + std::map GetTrajectoryStates() const override + LOCKS_EXCLUDED(mutex_); + std::map GetLandmarkPoses() const override + LOCKS_EXCLUDED(mutex_); + void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen = false) override + LOCKS_EXCLUDED(mutex_); + sensor::MapByTime GetImuData() const override + LOCKS_EXCLUDED(mutex_); + sensor::MapByTime GetOdometryData() const override + LOCKS_EXCLUDED(mutex_); + sensor::MapByTime GetFixedFramePoseData() + const override LOCKS_EXCLUDED(mutex_); + std::map + GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_); + std::map GetTrajectoryData() const override + LOCKS_EXCLUDED(mutex_); + std::vector constraints() const override LOCKS_EXCLUDED(mutex_); + void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) override + LOCKS_EXCLUDED(mutex_); + void SetGlobalSlamOptimizationCallback( + PoseGraphInterface::GlobalSlamOptimizationCallback callback) override; + transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( + int trajectory_id, const common::Time time) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + MapById GetSubmapDataUnderLock() + const EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Handles a new work item. + void AddWorkItem(const std::function& work_item) + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_); + + // Adds connectivity and sampler for a trajectory if it does not exist. + void AddTrajectoryIfNeeded(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Appends the new node and submap (if needed) to the internal data + // structures. + NodeId AppendNode( + std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& insertion_submaps, + const transform::Rigid3d& optimized_pose) LOCKS_EXCLUDED(mutex_); + + // Grows the optimization problem to have an entry for every element of + // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. + std::vector InitializeGlobalSubmapPoses( + int trajectory_id, const common::Time time, + const std::vector>& insertion_submaps) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Adds constraints for a node, and starts scan matching in the background. + WorkItem::Result ComputeConstraintsForNode( + const NodeId& node_id, + std::vector> insertion_submaps, + bool newly_finished_submap) LOCKS_EXCLUDED(mutex_); + + // Computes constraints for a node and submap pair. + void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) + LOCKS_EXCLUDED(mutex_); + + // Deletes trajectories waiting for deletion. Must not be called during + // constraint search. + void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Runs the optimization, executes the trimmers and processes the work queue. + void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_); + + // Process pending tasks in the work queue on the calling thread, until the + // queue is either empty or an optimization is required. + void DrainWorkQueue() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(work_queue_mutex_); + + // Waits until we caught up (i.e. nothing is waiting to be scheduled), and + // all computations have finished. + void WaitForAllComputations() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(work_queue_mutex_); + + // Runs the optimization. Callers have to make sure, that there is only one + // optimization being run at a time. + void RunOptimization() LOCKS_EXCLUDED(mutex_); + + bool CanAddWorkItemModifying(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Computes the local to global map frame transform based on the given + // 'global_submap_poses'. + transform::Rigid3d ComputeLocalToGlobalTransform( + const MapById& global_submap_poses, + int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + common::Time GetLatestNodeTime(const NodeId& node_id, + const SubmapId& submap_id) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Updates the trajectory connectivity structure with a new constraint. + void UpdateTrajectoryConnectivity(const Constraint& constraint) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + const proto::PoseGraphOptions options_; + GlobalSlamOptimizationCallback global_slam_optimization_callback_; + mutable absl::Mutex mutex_; + absl::Mutex work_queue_mutex_; + + // If it exists, further work items must be added to this queue, and will be + // considered later. + std::unique_ptr work_queue_ GUARDED_BY(work_queue_mutex_); + + // We globally localize a fraction of the nodes from each trajectory. + absl::flat_hash_map> + global_localization_samplers_ GUARDED_BY(mutex_); + + // Number of nodes added since last loop closure. + int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; + + // Current optimization problem. + std::unique_ptr optimization_problem_; + constraints::ConstraintBuilder2D constraint_builder_; + + // Thread pool used for handling the work queue. + common::ThreadPool* const thread_pool_; + + // List of all trimmers to consult when optimizations finish. + std::vector> trimmers_ GUARDED_BY(mutex_); + + PoseGraphData data_ GUARDED_BY(mutex_); + + ValueConversionTables conversion_tables_; + + // Allows querying and manipulating the pose graph by the 'trimmers_'. The + // 'mutex_' of the pose graph is held while this class is used. + class TrimmingHandle : public Trimmable { + public: + TrimmingHandle(PoseGraph2D* parent); + ~TrimmingHandle() override {} + + int num_submaps(int trajectory_id) const override; + std::vector GetSubmapIds(int trajectory_id) const override; + MapById GetOptimizedSubmapData() const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + const MapById& GetTrajectoryNodes() const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + const std::vector& GetConstraints() const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + void TrimSubmap(const SubmapId& submap_id) + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override; + bool IsFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + void SetTrajectoryState(int trajectory_id, TrajectoryState state) override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + + private: + PoseGraph2D* const parent_; + }; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d_test.cc new file mode 100644 index 0000000..2ecdfd1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -0,0 +1,318 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/pose_graph_2d.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +class PoseGraph2DTest : public ::testing::Test { + protected: + PoseGraph2DTest() : thread_pool_(1) { + // Builds a wavy, irregularly circular point cloud that is unique + // rotationally. This gives us good rotational texture and avoids any + // possibility of the CeresScanMatcher2D preferring free space (> + // kMinProbability) to unknown space (== kMinProbability). + for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) { + const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f); + point_cloud_.push_back( + {Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}}); + } + + { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + num_range_data = 1, + grid_options_2d = { + grid_type = "PROBABILITY_GRID", + resolution = 0.05, + }, + range_data_inserter = { + range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", + probability_grid_range_data_inserter = { + insert_free_space = true, + hit_probability = 0.53, + miss_probability = 0.495, + }, + tsdf_range_data_inserter = { + truncation_distance = 0.3, + maximum_weight = 10., + update_free_space = false, + normal_estimation_options = { + num_normal_samples = 4, + sample_radius = 0.5, + }, + project_sdf_distance_to_scan_normal = false, + update_weight_range_exponent = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0, + update_weight_distance_cell_to_hit_kernel_bandwidth = 0, + }, + }, + })text"); + active_submaps_ = absl::make_unique( + mapping::CreateSubmapsOptions2D(parameter_dictionary.get())); + } + + { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + optimize_every_n_nodes = 1000, + constraint_builder = { + sampling_ratio = 1., + max_constraint_distance = 6., + min_score = 0.5, + global_localization_min_score = 0.6, + loop_closure_translation_weight = 1., + loop_closure_rotation_weight = 1., + log_matches = true, + fast_correlative_scan_matcher = { + linear_search_window = 3., + angular_search_window = 0.1, + branch_and_bound_depth = 3, + }, + ceres_scan_matcher = { + occupied_space_weight = 20., + translation_weight = 10., + rotation_weight = 1., + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + }, + fast_correlative_scan_matcher_3d = { + branch_and_bound_depth = 3, + full_resolution_depth = 3, + min_rotational_score = 0.1, + min_low_resolution_score = 0.5, + linear_xy_search_window = 4., + linear_z_search_window = 4., + angular_search_window = 0.1, + }, + ceres_scan_matcher_3d = { + occupied_space_weight_0 = 20., + translation_weight = 10., + rotation_weight = 1., + only_optimize_yaw = true, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + }, + }, + matcher_translation_weight = 1., + matcher_rotation_weight = 1., + optimization_problem = { + acceleration_weight = 1., + rotation_weight = 1e2, + huber_scale = 1., + local_slam_pose_translation_weight = 0., + local_slam_pose_rotation_weight = 0., + odometry_translation_weight = 0., + odometry_rotation_weight = 0., + fixed_frame_pose_translation_weight = 1e1, + fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, + log_solver_summary = true, + use_online_imu_extrinsics_in_3d = true, + fix_z_in_3d = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 200, + num_threads = 1, + }, + }, + max_num_final_iterations = 200, + global_sampling_ratio = 0.01, + log_residual_histograms = true, + global_constraint_search_after_n_seconds = 10.0, + })text"); + auto options = CreatePoseGraphOptions(parameter_dictionary.get()); + pose_graph_ = absl::make_unique( + options, + absl::make_unique( + options.optimization_problem_options()), + &thread_pool_); + } + + current_pose_ = transform::Rigid2d::Identity(); + } + + void MoveRelativeWithNoise(const transform::Rigid2d& movement, + const transform::Rigid2d& noise) { + current_pose_ = current_pose_ * movement; + const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( + point_cloud_, + transform::Embed3D(current_pose_.inverse().cast())); + const sensor::RangeData range_data{ + Eigen::Vector3f::Zero(), new_point_cloud, {}}; + const transform::Rigid2d pose_estimate = noise * current_pose_; + constexpr int kTrajectoryId = 0; + active_submaps_->InsertRangeData(TransformRangeData( + range_data, transform::Embed3D(pose_estimate.cast()))); + std::vector> insertion_submaps; + for (const auto& submap : active_submaps_->submaps()) { + insertion_submaps.push_back(submap); + } + pose_graph_->AddNode( + std::make_shared( + TrajectoryNode::Data{common::FromUniversal(0), + Eigen::Quaterniond::Identity(), + range_data.returns, + {}, + {}, + {}, + transform::Embed3D(pose_estimate)}), + kTrajectoryId, insertion_submaps); + } + + void MoveRelative(const transform::Rigid2d& movement) { + MoveRelativeWithNoise(movement, transform::Rigid2d::Identity()); + } + + template + std::vector ToVectorInt(const Range& range) { + return std::vector(range.begin(), range.end()); + } + + sensor::PointCloud point_cloud_; + std::unique_ptr active_submaps_; + common::ThreadPool thread_pool_; + std::unique_ptr pose_graph_; + transform::Rigid2d current_pose_; +}; + +TEST_F(PoseGraph2DTest, EmptyMap) { + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); + EXPECT_TRUE(nodes.empty()); +} + +TEST_F(PoseGraph2DTest, NoMovement) { + MoveRelative(transform::Rigid2d::Identity()); + MoveRelative(transform::Rigid2d::Identity()); + MoveRelative(transform::Rigid2d::Identity()); + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), + ::testing::ContainerEq(std::vector{0})); + EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u)); + EXPECT_THAT(nodes.at(NodeId{0, 0}).global_pose, + transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); + EXPECT_THAT(nodes.at(NodeId{0, 1}).global_pose, + transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); + EXPECT_THAT(nodes.at(NodeId{0, 2}).global_pose, + transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); +} + +TEST_F(PoseGraph2DTest, NoOverlappingNodes) { + std::mt19937 rng(0); + std::uniform_real_distribution distribution(-1., 1.); + std::vector poses; + for (int i = 0; i != 4; ++i) { + MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.)); + poses.emplace_back(current_pose_); + } + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), + ::testing::ContainerEq(std::vector{0})); + for (int i = 0; i != 4; ++i) { + EXPECT_THAT( + poses[i], + IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose), + 1e-2)) + << i; + } +} + +TEST_F(PoseGraph2DTest, ConsecutivelyOverlappingNodes) { + std::mt19937 rng(0); + std::uniform_real_distribution distribution(-1., 1.); + std::vector poses; + for (int i = 0; i != 5; ++i) { + MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.)); + poses.emplace_back(current_pose_); + } + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), + ::testing::ContainerEq(std::vector{0})); + for (int i = 0; i != 5; ++i) { + EXPECT_THAT( + poses[i], + IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose), + 1e-2)) + << i; + } +} + +TEST_F(PoseGraph2DTest, OverlappingNodes) { + std::mt19937 rng(0); + std::uniform_real_distribution distribution(-1., 1.); + std::vector ground_truth; + std::vector poses; + for (int i = 0; i != 5; ++i) { + const double noise_x = 0.1 * distribution(rng); + const double noise_y = 0.1 * distribution(rng); + const double noise_orientation = 0.1 * distribution(rng); + transform::Rigid2d noise({noise_x, noise_y}, noise_orientation); + MoveRelativeWithNoise( + transform::Rigid2d({0.15 * distribution(rng), 0.4}, 0.), noise); + ground_truth.emplace_back(current_pose_); + poses.emplace_back(noise * current_pose_); + } + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), + ::testing::ContainerEq(std::vector{0})); + transform::Rigid2d true_movement = + ground_truth.front().inverse() * ground_truth.back(); + transform::Rigid2d movement_before = poses.front().inverse() * poses.back(); + transform::Rigid2d error_before = movement_before.inverse() * true_movement; + transform::Rigid3d optimized_movement = + nodes.BeginOfTrajectory(0)->data.global_pose.inverse() * + std::prev(nodes.EndOfTrajectory(0))->data.global_pose; + transform::Rigid2d optimized_error = + transform::Project2D(optimized_movement).inverse() * true_movement; + EXPECT_THAT(std::abs(optimized_error.normalized_angle()), + ::testing::Lt(std::abs(error_before.normalized_angle()))); + EXPECT_THAT(optimized_error.translation().norm(), + ::testing::Lt(error_before.translation().norm())); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc new file mode 100644 index 0000000..1c02b15 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc @@ -0,0 +1,701 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/2d/ray_to_pixel_mask.h" + +#include "Eigen/Dense" +int cnew[4000000]={0}; +namespace cartographer { +namespace mapping { +namespace { + +bool isEqual(const Eigen::Array2i& lhs, const Eigen::Array2i& rhs) { + return ((lhs - rhs).matrix().lpNorm<1>() == 0); +} +} // namespace + +// Compute all pixels that contain some part of the line segment connecting +// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled +// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be +// greater than zero. Return values are in pixels and not scaled. +std::vector RayToPixelMask(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale) { + // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x + // coordinate. + if (scaled_begin.x() > scaled_end.x()) { + return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale); + } + + CHECK_GE(scaled_begin.x(), 0); + CHECK_GE(scaled_begin.y(), 0); + CHECK_GE(scaled_end.y(), 0); + std::vector pixel_mask; + // Special case: We have to draw a vertical line in full pixels, as + // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate. + if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) { + Eigen::Array2i current( + scaled_begin.x() / subpixel_scale, + std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale); + pixel_mask.push_back(current); + const int end_y = + std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale; + for (; current.y() <= end_y; ++current.y()) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + return pixel_mask; + } + + const int64 dx = scaled_end.x() - scaled_begin.x(); + const int64 dy = scaled_end.y() - scaled_begin.y(); + const int64 denominator = 2 * subpixel_scale * dx; + + // The current full pixel coordinates. We scaled_begin at 'scaled_begin'. + Eigen::Array2i current = scaled_begin / subpixel_scale; + pixel_mask.push_back(current); + + // To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in + // the denominator. + // +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale) + // | | | | + // +-+-+-+ + // | | | | + // +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale) + // | | | | -- center of first subpixel = 1 / (2 * subpixel_scale) + // +-+-+-+ -- 0 = 0 / (2 * subpixel_scale) + + // The center of the subpixel part of 'scaled_begin.y()' assuming the + // 'denominator', i.e., sub_y / denominator is in (0, 1). + int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx; + + // The distance from the from 'scaled_begin' to the right pixel border, to be + // divided by 2 * 'subpixel_scale'. + const int first_pixel = + 2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1; + // The same from the left pixel border to 'scaled_end'. + const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1; + + // The full pixel x coordinate of 'scaled_end'. + const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale; + + // Move from 'scaled_begin' to the next pixel border to the right. + sub_y += dy * first_pixel; + if (dy > 0) { + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(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 * subpixel_scale; + } + // Move from the pixel border on the right to 'scaled_end'. + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, denominator); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + return pixel_mask; + } + + // Same for lines non-ascending in y coordinates. + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + ++current.x(); + if (sub_y == 0) { + sub_y += denominator; + --current.y(); + } + if (current.x() == end_x) { + break; + } + sub_y += dy * 2 * subpixel_scale; + } + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, 0); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + return pixel_mask; +} + +Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) { + return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation(); +} +/** + * @brief 根据local_pose将miss点转换成全局坐标 + * + * @param[in] + * @param[in] + * @param[in] + * @param[in] + * @param[in] + */ + + +std::vector LocalToMap(const Eigen::Array2i& pixel, + const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + int flag, + const transform::Rigid3d local_pose_){ + int o_x=1000; + int o_y=1000; + int xm; + int ym; + int xm_begin; + int ym_begin; + int xm_end; + int ym_end; + + const Eigen::Matrix4d homo =ToEigen(local_pose_).matrix(); + if (flag==0){ + if (scaled_begin.y()/1000>150){ + xm=-(pixel).y()+20*homo(0,3)+(o_x+200); + ym=(pixel).x()-20*homo(1,3)+(o_y-200); + xm_begin=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200); + ym_begin=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200); + xm_end=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200); + ym_end=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200); + }else{ + xm=-(pixel).y()+20*homo(0,3)+(o_x+100); + ym=(pixel).x()-20*homo(1,3)+(o_y-100); + + xm_begin=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+100); + ym_begin=scaled_begin.x()/1000-20*homo(1,3)+(o_y-100); + + xm_end=-scaled_end.y()/1000+20*homo(0,3)+(o_x+100); + ym_end=scaled_end.x()/1000-20*homo(1,3)+(o_y-100); + } + }else{ + if (scaled_end.y()/1000>150){ + xm=-(pixel).y()+20*homo(0,3)+(o_x+200); + ym=(pixel).x()-20*homo(1,3)+(o_y-200); + xm_begin=-scaled_end.y()/1000+20*homo(0,3)+(o_x+200); + ym_begin=scaled_end.x()/1000-20*homo(1,3)+(o_y-200); + xm_end=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200); + ym_end=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200); + }else{ + xm=-(pixel).y()+20*homo(0,3)+(o_x+100); + ym=(pixel).x()-20*homo(1,3)+(o_y-100); + xm_begin=-scaled_end.y()/1000+20*homo(0,3)+(o_x+100); + ym_begin=scaled_end.x()/1000-20*homo(1,3)+(o_y-100); + xm_end=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+100); + ym_end=scaled_begin.x()/1000-20*homo(1,3)+(o_y-100); + // LOG(INFO)<<"wyk--begin is:"< map_position; + map_position.push_back(Eigen::Array2i(xm_begin, ym_begin)); + map_position.push_back(Eigen::Array2i(xm,ym)); + map_position.push_back(Eigen::Array2i(xm_end,ym_end)); + return map_position; +} +/** + * @brief 非摄像头视域或摄像头视域超过两米范围的小物体不更新，即该范围内的删去miss点 + * + */ + +std::vector ProcessPixelMask(std::vector pixel_mask, + const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + int flag, + const uint8 intensities, + const transform::Rigid3d local_pose_){ + + int xm; + int ym; + int xm_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].x(); + int ym_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].y(); + int xm_end; + int ym_end; + for(std::vector::iterator pixel=pixel_mask.begin();pixel!=pixel_mask.end(); ){ + xm=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].x(); + ym=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].y(); + if(0ApplyLookupTable(*pixel, hit_table); + flag_miss=1; + break; + //在摄像头视域 + }else{ + //超过两米 小物体不更新 + if ((xm-xm_begin)*(xm-xm_begin)+(ym-ym_begin)*(ym-ym_begin)>1600){ + pixel=pixel_mask.erase(pixel); + flag_miss=1; + break; + } + } + } + + + } + if (flag_miss==1) + break; + } + if (flag_miss == 0) + ++pixel; + } + else{ + LOG(INFO)<<"frame erro:"< ProcessPixelMaskHit(std::vector pixel_mask, + const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + int flag, + const uint8 intensities, + const transform::Rigid3d local_pose_, + const std::vector& hit_table, + cartographer::mapping::ProbabilityGrid* probability_grid){ + + int xm; + int ym; + int xm_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].x(); + int ym_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].y(); + int xm_end; + int ym_end; + for(std::vector::iterator pixel=pixel_mask.begin();pixel!=pixel_mask.end(); ){ + xm=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].x(); + ym=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].y(); + if(0100 ){ + //如果在非摄像头视域 + if (intensities == 0) { + //pixel=pixel_mask.erase(pixel); + probability_grid->ApplyLookupTable(*pixel, hit_table); + // for(std::vector::iterator pixel_hit=pixel_mask.begin();pixel!=pixel_mask.end(); ){ + // probability_grid->ApplyLookupTable(*pixel_hit, hit_table); + // } + ++pixel; + flag_miss=1; + break; + } + } + + + } + if (flag_miss==1) + break; + } + if (flag_miss == 0) + ++pixel; + } + else{ + LOG(INFO)<<"frame erro:"< RayToPixelMaskVisualNew(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + const uint8 intensities, + const transform::Rigid3d local_pose_, + int flag, + const std::vector& hit_table, + cartographer::mapping::ProbabilityGrid* probability_grid) { + int o_x=1000; + int o_y=1000; + int xm; + int ym; + int xm_begin; + int ym_begin; + int xm_end; + int ym_end; + // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x + // coordinate. + //测试地图数据是否收到 + // int flag = 0; + // for (int y = 2000 - 1; y >= 0;--y) { + // for (int x = 0; x < 2000; ++x) { + // if (cnew[y*2000+x]>100){ + // std::cout<150){ +// int o_x=1000; +// int o_y=1000; +// int xm=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200); +// int ym=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200); + +// LOG(INFO)<<"wyk--guiji jububiao:"<=900 ;--y) { + // for (int x = 1000; x <1200 ; ++x) { + // if(cnew[y*2000+x]==200){ + // std::cout< scaled_end.x()) { + //flag=1代表end和begin交换 + flag=1; + return RayToPixelMaskVisualNew(scaled_end, scaled_begin, subpixel_scale,intensities,local_pose_,flag,hit_table, + probability_grid); + } + + // if(intensities >100){ + // std::cout< pixel_mask; + // Special case: We have to draw a vertical line in full pixels, as + // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate. + if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) { + Eigen::Array2i current( + scaled_begin.x() / subpixel_scale, + std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale); + pixel_mask.push_back(current); + const int end_y = + std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale; + for (; current.y() <= end_y; ++current.y()) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + + const Eigen::Matrix4d homo =ToEigen(local_pose_).matrix(); + // std::cout< 0) { + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(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 * subpixel_scale; + } + // Move from the pixel border on the right to 'scaled_end'. + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, denominator); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + + + + //return pixel_mask; + return ProcessPixelMask(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_); + // return ProcessPixelMaskHit(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_,hit_table,probability_grid); + } + + // Same for lines non-ascending in y coordinates. + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + ++current.x(); + if (sub_y == 0) { + sub_y += denominator; + --current.y(); + } + if (current.x() == end_x) { + break; + } + sub_y += dy * 2 * subpixel_scale; + } + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, 0); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + + + //return pixel_mask; + return ProcessPixelMask(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_); + //return ProcessPixelMaskHit(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_,hit_table,probability_grid); + } + + + +std::vector RayToPixelMaskVisual(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + const uint8 intensities) { + // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x + // coordinate. + if (intensities != 0){ + if (scaled_begin.x() > scaled_end.x()) { + return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale); + } + + CHECK_GE(scaled_begin.x(), 0); + CHECK_GE(scaled_begin.y(), 0); + CHECK_GE(scaled_end.y(), 0); + std::vector pixel_mask; + // Special case: We have to draw a vertical line in full pixels, as + // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate. + if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) { + Eigen::Array2i current( + scaled_begin.x() / subpixel_scale, + std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale); + pixel_mask.push_back(current); + const int end_y = + std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale; + for (; current.y() <= end_y; ++current.y()) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + return pixel_mask; + } + + const int64 dx = scaled_end.x() - scaled_begin.x(); + const int64 dy = scaled_end.y() - scaled_begin.y(); + const int64 denominator = 2 * subpixel_scale * dx; + + // The current full pixel coordinates. We scaled_begin at 'scaled_begin'. + Eigen::Array2i current = scaled_begin / subpixel_scale; + pixel_mask.push_back(current); + + // To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in + // the denominator. + // +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale) + // | | | | + // +-+-+-+ + // | | | | + // +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale) + // | | | | -- center of first subpixel = 1 / (2 * subpixel_scale) + // +-+-+-+ -- 0 = 0 / (2 * subpixel_scale) + + // The center of the subpixel part of 'scaled_begin.y()' assuming the + // 'denominator', i.e., sub_y / denominator is in (0, 1). + int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx; + + // The distance from the from 'scaled_begin' to the right pixel border, to be + // divided by 2 * 'subpixel_scale'. + const int first_pixel = + 2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1; + // The same from the left pixel border to 'scaled_end'. + const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1; + + // The full pixel x coordinate of 'scaled_end'. + const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale; + + // Move from 'scaled_begin' to the next pixel border to the right. + sub_y += dy * first_pixel; + if (dy > 0) { + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(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 * subpixel_scale; + } + // Move from the pixel border on the right to 'scaled_end'. + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y > denominator) { + sub_y -= denominator; + ++current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, denominator); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + return pixel_mask; + } + + // Same for lines non-ascending in y coordinates. + while (true) { + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + ++current.x(); + if (sub_y == 0) { + sub_y += denominator; + --current.y(); + } + if (current.x() == end_x) { + break; + } + sub_y += dy * 2 * subpixel_scale; + } + sub_y += dy * last_pixel; + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + while (sub_y < 0) { + sub_y += denominator; + --current.y(); + if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); + } + CHECK_NE(sub_y, 0); + CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale); + return pixel_mask; + }else { + return std::vector (); + } + +} + + + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.h new file mode 100644 index 0000000..9d20c9b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.h @@ -0,0 +1,73 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ + +#include + +#include "cartographer/transform/transform.h" +#include "cartographer/mapping/2d/probability_grid.h" +namespace cartographer { +namespace mapping { + +// Compute all pixels that contain some part of the line segment connecting +// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled +// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be +// greater than zero. Return values are in pixels and not scaled. +std::vector RayToPixelMask(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale); +std::vector RayToPixelMaskVisualNew(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + const uint8 intensities, + const transform::Rigid3d local_pose_, + int flag, + const std::vector& hit_table, + cartographer::mapping::ProbabilityGrid* probability_grid); +std::vector RayToPixelMaskVisual(const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + const uint8 intensities); +std::vector ProcessPixelMask(std::vector pixel_mask, + const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + int flag, + const uint8 intensities, + const transform::Rigid3d local_pose_); +std::vector ProcessPixelMaskHit(std::vector pixel_mask, + const Eigen::Array2i& scaled_begin, + const Eigen::Array2i& scaled_end, + int subpixel_scale, + int flag, + const uint8 intensities, + const transform::Rigid3d local_pose_, + const std::vector& hit_table, + cartographer::mapping::ProbabilityGrid* probability_grid); +std::vector LocalToMap(const Eigen::Array2i& pixel, + const Eigen::Array2i& begin, + const Eigen::Array2i& end, + int subpixel_scale, + int flag, + const transform::Rigid3d local_pose_); + + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc new file mode 100644 index 0000000..bfc7650 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc @@ -0,0 +1,207 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" + +#include "cartographer/mapping/2d/map_limits.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using ::testing::ElementsAre; + +MATCHER_P(PixelMaskEqual, value, "") { + Eigen::Array2i residual = value - arg; + return residual.matrix().lpNorm<1>() == 0; +} + +TEST(RayToPixelMaskTest, SingleCell) { + const Eigen::Array2i& begin = {1, 1}; + const Eigen::Array2i& end = {1, 1}; + const int subpixel_scale = 1; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + std::vector ray_reference = + std::vector{begin}; + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(begin))); +} + +TEST(RayToPixelMaskTest, AxisAlignedX) { + const Eigen::Array2i& begin = {1, 1}; + const Eigen::Array2i& end = {3, 1}; + const int subpixel_scale = 1; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); +} + +TEST(RayToPixelMaskTest, AxisAlignedY) { + const Eigen::Array2i& begin = {1, 1}; + const Eigen::Array2i& end = {1, 3}; + const int subpixel_scale = 1; + std::vector ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3})}; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({1, 3})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({1, 3})))); +} + +TEST(RayToPixelMaskTest, Diagonal) { + Eigen::Array2i begin = {1, 1}; + Eigen::Array2i end = {3, 3}; + const int subpixel_scale = 1; + std::vector ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})}; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 3})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 3})))); + begin = Eigen::Array2i({1, 3}); + end = Eigen::Array2i({3, 1}); + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); + ray = RayToPixelMask(end, begin, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})), + PixelMaskEqual(Eigen::Array2i({2, 2})), + PixelMaskEqual(Eigen::Array2i({3, 1})))); +} + +TEST(RayToPixelMaskTest, SteepLine) { + Eigen::Array2i begin = {1, 1}; + Eigen::Array2i end = {2, 5}; + const int subpixel_scale = 1; + std::vector ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}), + Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})}; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({1, 3})), + PixelMaskEqual(Eigen::Array2i({2, 3})), + PixelMaskEqual(Eigen::Array2i({2, 4})), + PixelMaskEqual(Eigen::Array2i({2, 5})))); + begin = {1, 1}; + end = {2, 4}; + ray_reference = std::vector{ + Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}), + Eigen::Array2i({2, 4})}; + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({1, 2})), + PixelMaskEqual(Eigen::Array2i({2, 3})), + PixelMaskEqual(Eigen::Array2i({2, 4})))); +} + +TEST(RayToPixelMaskTest, FlatLine) { + Eigen::Array2i begin = {1, 1}; + Eigen::Array2i end = {5, 2}; + const int subpixel_scale = 1; + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 1})), + PixelMaskEqual(Eigen::Array2i({3, 2})), + PixelMaskEqual(Eigen::Array2i({4, 2})), + PixelMaskEqual(Eigen::Array2i({5, 2})))); + begin = {1, 1}; + end = {4, 2}; + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})), + PixelMaskEqual(Eigen::Array2i({2, 1})), + PixelMaskEqual(Eigen::Array2i({3, 2})), + PixelMaskEqual(Eigen::Array2i({4, 2})))); +} + +TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) { + int subpixel_scale; + const int num_cells_x = 20; + const int num_cells_y = 20; + double resolution = 0.1; + Eigen::Vector2d max = {1.0, 1.0}; + std::vector base_scale_ray; + for (subpixel_scale = 1; subpixel_scale < 10000; subpixel_scale *= 2) { + double superscaled_resolution = resolution / subpixel_scale; + MapLimits superscaled_limits( + superscaled_resolution, max, + CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale)); + Eigen::Array2i begin = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.05, 0.05})); + Eigen::Array2i end = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.35, 0.05})); + std::vector ray = + RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({9, 6})), + PixelMaskEqual(Eigen::Array2i({9, 7})), + PixelMaskEqual(Eigen::Array2i({9, 8})), + PixelMaskEqual(Eigen::Array2i({9, 9})))); + } +} + +TEST(RayToPixelMaskTest, MultiScaleSkewedLine) { + int subpixel_scale = 1; + const int num_cells_x = 20; + const int num_cells_y = 20; + double resolution = 0.1; + Eigen::Vector2d max = {1.0, 1.0}; + double superscaled_resolution = resolution / subpixel_scale; + MapLimits superscaled_limits( + superscaled_resolution, max, + CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale)); + Eigen::Array2i begin = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09})); + Eigen::Array2i end = + superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19})); + + std::vector ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})), + PixelMaskEqual(Eigen::Array2i({8, 8})), + PixelMaskEqual(Eigen::Array2i({9, 8})), + PixelMaskEqual(Eigen::Array2i({9, 9})))); + subpixel_scale = 20; + superscaled_resolution = resolution / subpixel_scale; + superscaled_limits = MapLimits( + superscaled_resolution, max, + CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale)); + begin = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09})); + end = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19})); + ray = RayToPixelMask(begin, end, subpixel_scale); + EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})), + PixelMaskEqual(Eigen::Array2i({8, 8})), + PixelMaskEqual(Eigen::Array2i({8, 9})), + PixelMaskEqual(Eigen::Array2i({9, 9})))); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc new file mode 100644 index 0000000..2fe2219 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -0,0 +1,111 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/2d/scan_matching/ceres_scan_matcher_2d.h" + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::CeresScanMatcherOptions2D options; + options.set_occupied_space_weight( + parameter_dictionary->GetDouble("occupied_space_weight")); + options.set_translation_weight( + parameter_dictionary->GetDouble("translation_weight")); + options.set_rotation_weight( + parameter_dictionary->GetDouble("rotation_weight")); + *options.mutable_ceres_solver_options() = + common::CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("ceres_solver_options").get()); + return options; +} + +CeresScanMatcher2D::CeresScanMatcher2D( + const proto::CeresScanMatcherOptions2D& options) + : options_(options), + ceres_solver_options_( + common::CreateCeresSolverOptions(options.ceres_solver_options())) { + ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; +} + +CeresScanMatcher2D::~CeresScanMatcher2D() {} + +void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, + const Grid2D& grid, + transform::Rigid2d* const pose_estimate, + ceres::Solver::Summary* const summary) const { + double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), + initial_pose_estimate.translation().y(), + initial_pose_estimate.rotation().angle()}; + ceres::Problem problem; + CHECK_GT(options_.occupied_space_weight(), 0.); + switch (grid.GetGridType()) { + case GridType::PROBABILITY_GRID: + problem.AddResidualBlock( + CreateOccupiedSpaceCostFunction2D( + options_.occupied_space_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, grid), + nullptr /* loss function */, ceres_pose_estimate); + break; + case GridType::TSDF: + problem.AddResidualBlock( + CreateTSDFMatchCostFunction2D( + options_.occupied_space_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, static_cast(grid)), + nullptr /* loss function */, ceres_pose_estimate); + break; + } + CHECK_GT(options_.translation_weight(), 0.); + problem.AddResidualBlock( + TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( + options_.translation_weight(), target_translation), + nullptr /* loss function */, ceres_pose_estimate); + CHECK_GT(options_.rotation_weight(), 0.); + problem.AddResidualBlock( + RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction( + options_.rotation_weight(), ceres_pose_estimate[2]), + nullptr /* loss function */, ceres_pose_estimate); + + ceres::Solve(ceres_solver_options_, &problem, summary); + + *pose_estimate = transform::Rigid2d( + {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h new file mode 100644 index 0000000..48108d5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h @@ -0,0 +1,64 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +// Align scans with an existing map using Ceres. +class CeresScanMatcher2D { + public: + explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options); + virtual ~CeresScanMatcher2D(); + + CeresScanMatcher2D(const CeresScanMatcher2D&) = delete; + CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete; + + // Aligns 'point_cloud' within the 'grid' given an + // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver + // 'summary'. + void Match(const Eigen::Vector2d& target_translation, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, const Grid2D& grid, + transform::Rigid2d* pose_estimate, + ceres::Solver::Summary* summary) const; + + private: + const proto::CeresScanMatcherOptions2D options_; + ceres::Solver::Options ceres_solver_options_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc new file mode 100644 index 0000000..74036ad --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc @@ -0,0 +1,102 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/2d/scan_matching/ceres_scan_matcher_2d.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/probability_values.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class CeresScanMatcherTest : public ::testing::Test { + protected: + CeresScanMatcherTest() + : probability_grid_( + MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20)), + &conversion_tables_) { + probability_grid_.SetProbability( + probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)), + kMaxProbability); + + point_cloud_.push_back({Eigen::Vector3f{-3.f, 2.f, 0.f}}); + + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + occupied_space_weight = 1., + translation_weight = 0.1, + rotation_weight = 1.5, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + })text"); + const proto::CeresScanMatcherOptions2D options = + CreateCeresScanMatcherOptions2D(parameter_dictionary.get()); + ceres_scan_matcher_ = absl::make_unique(options); + } + + void TestFromInitialPose(const transform::Rigid2d& initial_pose) { + transform::Rigid2d pose; + const transform::Rigid2d expected_pose = + transform::Rigid2d::Translation({-0.5, 0.5}); + ceres::Solver::Summary summary; + ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose, + point_cloud_, probability_grid_, &pose, + &summary); + EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); + EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2)) + << "Actual: " << transform::ToProto(pose).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } + + ValueConversionTables conversion_tables_; + ProbabilityGrid probability_grid_; + sensor::PointCloud point_cloud_; + std::unique_ptr ceres_scan_matcher_; +}; + +TEST_F(CeresScanMatcherTest, testPerfectEstimate) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.5, 0.5})); +} + +TEST_F(CeresScanMatcherTest, testOptimizeAlongX) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.5})); +} + +TEST_F(CeresScanMatcherTest, testOptimizeAlongY) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.45, 0.3})); +} + +TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.3})); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc new file mode 100644 index 0000000..1747165 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc @@ -0,0 +1,131 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" + +#include + +#include "cartographer/common/math.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +SearchParameters::SearchParameters(const double linear_search_window, + const double angular_search_window, + const sensor::PointCloud& point_cloud, + const double resolution) + : resolution(resolution) { + // We set this value to something on the order of resolution to make sure that + // the std::acos() below is defined. + float max_scan_range = 3.f * resolution; + for (const sensor::RangefinderPoint& point : point_cloud) { + const float range = point.position.head<2>().norm(); + max_scan_range = std::max(range, max_scan_range); + } + const double kSafetyMargin = 1. - 1e-3; + angular_perturbation_step_size = + kSafetyMargin * std::acos(1. - common::Pow2(resolution) / + (2. * common::Pow2(max_scan_range))); + num_angular_perturbations = + std::ceil(angular_search_window / angular_perturbation_step_size); + num_scans = 2 * num_angular_perturbations + 1; + + const int num_linear_perturbations = + std::ceil(linear_search_window / resolution); + linear_bounds.reserve(num_scans); + for (int i = 0; i != num_scans; ++i) { + linear_bounds.push_back( + LinearBounds{-num_linear_perturbations, num_linear_perturbations, + -num_linear_perturbations, num_linear_perturbations}); + } +} + +SearchParameters::SearchParameters(const int num_linear_perturbations, + const int num_angular_perturbations, + const double angular_perturbation_step_size, + const double resolution) + : num_angular_perturbations(num_angular_perturbations), + angular_perturbation_step_size(angular_perturbation_step_size), + resolution(resolution), + num_scans(2 * num_angular_perturbations + 1) { + linear_bounds.reserve(num_scans); + for (int i = 0; i != num_scans; ++i) { + linear_bounds.push_back( + LinearBounds{-num_linear_perturbations, num_linear_perturbations, + -num_linear_perturbations, num_linear_perturbations}); + } +} + +void SearchParameters::ShrinkToFit(const std::vector& scans, + const CellLimits& cell_limits) { + CHECK_EQ(scans.size(), num_scans); + CHECK_EQ(linear_bounds.size(), num_scans); + for (int i = 0; i != num_scans; ++i) { + Eigen::Array2i min_bound = Eigen::Array2i::Zero(); + Eigen::Array2i max_bound = Eigen::Array2i::Zero(); + for (const Eigen::Array2i& xy_index : scans[i]) { + min_bound = min_bound.min(-xy_index); + max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1, + cell_limits.num_y_cells - 1) - + xy_index); + } + linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x()); + linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x()); + linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y()); + linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y()); + } +} + +std::vector GenerateRotatedScans( + const sensor::PointCloud& point_cloud, + const SearchParameters& search_parameters) { + std::vector rotated_scans; + rotated_scans.reserve(search_parameters.num_scans); + + double delta_theta = -search_parameters.num_angular_perturbations * + search_parameters.angular_perturbation_step_size; + for (int scan_index = 0; scan_index < search_parameters.num_scans; + ++scan_index, + delta_theta += search_parameters.angular_perturbation_step_size) { + rotated_scans.push_back(sensor::TransformPointCloud( + point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf( + delta_theta, Eigen::Vector3f::UnitZ())))); + } + return rotated_scans; +} + +std::vector DiscretizeScans( + const MapLimits& map_limits, const std::vector& scans, + const Eigen::Translation2f& initial_translation) { + std::vector discrete_scans; + discrete_scans.reserve(scans.size()); + for (const sensor::PointCloud& scan : scans) { + discrete_scans.emplace_back(); + discrete_scans.back().reserve(scan.size()); + for (const sensor::RangefinderPoint& point : scan) { + const Eigen::Vector2f translated_point = + Eigen::Affine2f(initial_translation) * point.position.head<2>(); + discrete_scans.back().push_back( + map_limits.GetCellIndex(translated_point)); + } + } + return discrete_scans; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h new file mode 100644 index 0000000..a27c806 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h @@ -0,0 +1,109 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +typedef std::vector DiscreteScan2D; + +// Describes the search space. +struct SearchParameters { + // Linear search window in pixel offsets; bounds are inclusive. + struct LinearBounds { + int min_x; + int max_x; + int min_y; + int max_y; + }; + + SearchParameters(double linear_search_window, double angular_search_window, + const sensor::PointCloud& point_cloud, double resolution); + + // For testing. + SearchParameters(int num_linear_perturbations, int num_angular_perturbations, + double angular_perturbation_step_size, double resolution); + + // Tightens the search window as much as possible. + void ShrinkToFit(const std::vector& scans, + const CellLimits& cell_limits); + + int num_angular_perturbations; + double angular_perturbation_step_size; + double resolution; + int num_scans; + std::vector linear_bounds; // Per rotated scans. +}; + +// Generates a collection of rotated scans. +std::vector GenerateRotatedScans( + const sensor::PointCloud& point_cloud, + const SearchParameters& search_parameters); + +// Translates and discretizes the rotated scans into a vector of integer +// indices. +std::vector DiscretizeScans( + const MapLimits& map_limits, const std::vector& scans, + const Eigen::Translation2f& initial_translation); + +// A possible solution. +struct Candidate2D { + Candidate2D(const int init_scan_index, const int init_x_index_offset, + const int init_y_index_offset, + const SearchParameters& search_parameters) + : scan_index(init_scan_index), + x_index_offset(init_x_index_offset), + y_index_offset(init_y_index_offset), + x(-y_index_offset * search_parameters.resolution), + y(-x_index_offset * search_parameters.resolution), + orientation((scan_index - search_parameters.num_angular_perturbations) * + search_parameters.angular_perturbation_step_size) {} + + // Index into the rotated scans vector. + int scan_index = 0; + + // Linear offset from the initial pose. + int x_index_offset = 0; + int y_index_offset = 0; + + // Pose of this Candidate2D relative to the initial pose. + double x = 0.; + double y = 0.; + double orientation = 0.; + + // Score, higher is better. + float score = 0.f; + + bool operator<(const Candidate2D& other) const { return score < other.score; } + bool operator>(const Candidate2D& other) const { return score > other.score; } +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc new file mode 100644 index 0000000..ce513a9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc @@ -0,0 +1,101 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" +#include "cartographer/sensor/point_cloud.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +TEST(SearchParameters, Construction) { + const SearchParameters search_parameters(4, 5, 0.03, 0.05); + EXPECT_EQ(5, search_parameters.num_angular_perturbations); + EXPECT_NEAR(0.03, search_parameters.angular_perturbation_step_size, 1e-9); + EXPECT_NEAR(0.05, search_parameters.resolution, 1e-9); + EXPECT_EQ(11, search_parameters.num_scans); + EXPECT_EQ(11, search_parameters.linear_bounds.size()); + for (const SearchParameters::LinearBounds linear_bounds : + search_parameters.linear_bounds) { + EXPECT_EQ(-4, linear_bounds.min_x); + EXPECT_EQ(4, linear_bounds.max_x); + EXPECT_EQ(-4, linear_bounds.min_y); + EXPECT_EQ(4, linear_bounds.max_y); + } +} + +TEST(Candidate, Construction) { + const SearchParameters search_parameters(4, 5, 0.03, 0.05); + const Candidate2D candidate(3, 4, -5, search_parameters); + EXPECT_EQ(3, candidate.scan_index); + EXPECT_EQ(4, candidate.x_index_offset); + EXPECT_EQ(-5, candidate.y_index_offset); + EXPECT_NEAR(0.25, candidate.x, 1e-9); + EXPECT_NEAR(-0.2, candidate.y, 1e-9); + EXPECT_NEAR(-0.06, candidate.orientation, 1e-9); + EXPECT_NEAR(0., candidate.score, 1e-9); + + Candidate2D bigger_candidate(3, 4, 5, search_parameters); + bigger_candidate.score = 1.; + EXPECT_LT(candidate, bigger_candidate); +} + +TEST(GenerateRotatedScans, GenerateRotatedScans) { + sensor::PointCloud point_cloud; + point_cloud.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}}); + const std::vector scans = + GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.)); + EXPECT_EQ(3, scans.size()); + EXPECT_NEAR(1., scans[0][0].position.x(), 1e-6); + EXPECT_NEAR(1., scans[0][0].position.y(), 1e-6); + EXPECT_NEAR(-1., scans[1][0].position.x(), 1e-6); + EXPECT_NEAR(1., scans[1][0].position.y(), 1e-6); + EXPECT_NEAR(-1., scans[2][0].position.x(), 1e-6); + EXPECT_NEAR(-1., scans[2][0].position.y(), 1e-6); +} + +TEST(DiscretizeScans, DiscretizeScans) { + sensor::PointCloud point_cloud; + point_cloud.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}}); + const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), + CellLimits(6, 6)); + const std::vector scans = + GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity()); + EXPECT_EQ(1, discrete_scans.size()); + EXPECT_EQ(7, discrete_scans[0].size()); + EXPECT_TRUE((Eigen::Array2i(1, 0) == discrete_scans[0][0]).all()); + EXPECT_TRUE((Eigen::Array2i(1, 1) == discrete_scans[0][1]).all()); + EXPECT_TRUE((Eigen::Array2i(1, 2) == discrete_scans[0][2]).all()); + EXPECT_TRUE((Eigen::Array2i(1, 3) == discrete_scans[0][3]).all()); + EXPECT_TRUE((Eigen::Array2i(2, 3) == discrete_scans[0][4]).all()); + EXPECT_TRUE((Eigen::Array2i(3, 3) == discrete_scans[0][5]).all()); + EXPECT_TRUE((Eigen::Array2i(4, 3) == discrete_scans[0][6]).all()); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc new file mode 100644 index 0000000..4f20363 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc @@ -0,0 +1,382 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" + +#include +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +// A collection of values which can be added and later removed, and the maximum +// of the current values in the collection can be retrieved. +// All of it in (amortized) O(1). +class SlidingWindowMaximum { + public: + void AddValue(const float value) { + while (!non_ascending_maxima_.empty() && + value > non_ascending_maxima_.back()) { + non_ascending_maxima_.pop_back(); + } + non_ascending_maxima_.push_back(value); + } + + void RemoveValue(const float value) { + // DCHECK for performance, since this is done for every value in the + // precomputation grid. + DCHECK(!non_ascending_maxima_.empty()); + DCHECK_LE(value, non_ascending_maxima_.front()); + if (value == non_ascending_maxima_.front()) { + non_ascending_maxima_.pop_front(); + } + } + + float GetMaximum() const { + // DCHECK for performance, since this is done for every value in the + // precomputation grid. + DCHECK_GT(non_ascending_maxima_.size(), 0); + return non_ascending_maxima_.front(); + } + + void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); } + + private: + // Maximum of the current sliding window at the front. Then the maximum of the + // remaining window that came after this values first occurrence, and so on. + std::deque non_ascending_maxima_; +}; + +} // namespace + +proto::FastCorrelativeScanMatcherOptions2D +CreateFastCorrelativeScanMatcherOptions2D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::FastCorrelativeScanMatcherOptions2D options; + options.set_linear_search_window( + parameter_dictionary->GetDouble("linear_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + options.set_branch_and_bound_depth( + parameter_dictionary->GetInt("branch_and_bound_depth")); + return options; +} + +PrecomputationGrid2D::PrecomputationGrid2D( + const Grid2D& grid, const CellLimits& limits, const int width, + std::vector* reusable_intermediate_grid) + : offset_(-width + 1, -width + 1), + wide_limits_(limits.num_x_cells + width - 1, + limits.num_y_cells + width - 1), + min_score_(1.f - grid.GetMaxCorrespondenceCost()), + max_score_(1.f - grid.GetMinCorrespondenceCost()), + cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) { + CHECK_GE(width, 1); + CHECK_GE(limits.num_x_cells, 1); + CHECK_GE(limits.num_y_cells, 1); + const int stride = wide_limits_.num_x_cells; + // First we compute the maximum probability for each (x0, y) achieved in the + // span defined by x0 <= x < x0 + width. + std::vector& intermediate = *reusable_intermediate_grid; + intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells); + for (int y = 0; y != limits.num_y_cells; ++y) { + SlidingWindowMaximum current_values; + current_values.AddValue( + 1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y)))); + for (int x = -width + 1; x != 0; ++x) { + intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); + if (x + width < limits.num_x_cells) { + current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost( + Eigen::Array2i(x + width, y)))); + } + } + for (int x = 0; x < limits.num_x_cells - width; ++x) { + intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); + current_values.RemoveValue( + 1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y)))); + current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost( + Eigen::Array2i(x + width, y)))); + } + for (int x = std::max(limits.num_x_cells - width, 0); + x != limits.num_x_cells; ++x) { + intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); + current_values.RemoveValue( + 1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y)))); + } + current_values.CheckIsEmpty(); + } + // For each (x, y), we compute the maximum probability in the width x width + // region starting at each (x, y) and precompute the resulting bound on the + // score. + for (int x = 0; x != wide_limits_.num_x_cells; ++x) { + SlidingWindowMaximum current_values; + current_values.AddValue(intermediate[x]); + for (int y = -width + 1; y != 0; ++y) { + cells_[x + (y + width - 1) * stride] = + ComputeCellValue(current_values.GetMaximum()); + if (y + width < limits.num_y_cells) { + current_values.AddValue(intermediate[x + (y + width) * stride]); + } + } + for (int y = 0; y < limits.num_y_cells - width; ++y) { + cells_[x + (y + width - 1) * stride] = + ComputeCellValue(current_values.GetMaximum()); + current_values.RemoveValue(intermediate[x + y * stride]); + current_values.AddValue(intermediate[x + (y + width) * stride]); + } + for (int y = std::max(limits.num_y_cells - width, 0); + y != limits.num_y_cells; ++y) { + cells_[x + (y + width - 1) * stride] = + ComputeCellValue(current_values.GetMaximum()); + current_values.RemoveValue(intermediate[x + y * stride]); + } + current_values.CheckIsEmpty(); + } +} + +uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const { + const int cell_value = common::RoundToInt( + (probability - min_score_) * (255.f / (max_score_ - min_score_))); + CHECK_GE(cell_value, 0); + CHECK_LE(cell_value, 255); + return cell_value; +} + +PrecomputationGridStack2D::PrecomputationGridStack2D( + const Grid2D& grid, + const proto::FastCorrelativeScanMatcherOptions2D& options) { + CHECK_GE(options.branch_and_bound_depth(), 1); + const int max_width = 1 << (options.branch_and_bound_depth() - 1); + precomputation_grids_.reserve(options.branch_and_bound_depth()); + std::vector reusable_intermediate_grid; + const CellLimits limits = grid.limits().cell_limits(); + reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * + limits.num_y_cells); + for (int i = 0; i != options.branch_and_bound_depth(); ++i) { + const int width = 1 << i; + precomputation_grids_.emplace_back(grid, limits, width, + &reusable_intermediate_grid); + } +} + +FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D( + const Grid2D& grid, + const proto::FastCorrelativeScanMatcherOptions2D& options) + : options_(options), + limits_(grid.limits()), + precomputation_grid_stack_( + absl::make_unique(grid, options)) {} + +FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {} + +bool FastCorrelativeScanMatcher2D::Match( + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, const float min_score, float* score, + transform::Rigid2d* pose_estimate) const { + const SearchParameters search_parameters(options_.linear_search_window(), + options_.angular_search_window(), + point_cloud, limits_.resolution()); + return MatchWithSearchParameters(search_parameters, initial_pose_estimate, + point_cloud, min_score, score, + pose_estimate); +} + +bool FastCorrelativeScanMatcher2D::MatchFullSubmap( + const sensor::PointCloud& point_cloud, float min_score, float* score, + transform::Rigid2d* pose_estimate) const { + // Compute a search window around the center of the submap that includes it + // fully. + const SearchParameters search_parameters( + 1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction. + M_PI, // Angular search window, 180 degrees in both directions. + point_cloud, limits_.resolution()); + const transform::Rigid2d center = transform::Rigid2d::Translation( + limits_.max() - 0.5 * limits_.resolution() * + Eigen::Vector2d(limits_.cell_limits().num_y_cells, + limits_.cell_limits().num_x_cells)); + return MatchWithSearchParameters(search_parameters, center, point_cloud, + min_score, score, pose_estimate); +} + +bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters( + SearchParameters search_parameters, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, float min_score, float* score, + transform::Rigid2d* pose_estimate) const { + CHECK(score != nullptr); + CHECK(pose_estimate != nullptr); + + const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); + const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud( + point_cloud, + transform::Rigid3f::Rotation(Eigen::AngleAxisf( + initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); + const std::vector rotated_scans = + GenerateRotatedScans(rotated_point_cloud, search_parameters); + const std::vector discrete_scans = DiscretizeScans( + limits_, rotated_scans, + Eigen::Translation2f(initial_pose_estimate.translation().x(), + initial_pose_estimate.translation().y())); + search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits()); + + const std::vector lowest_resolution_candidates = + ComputeLowestResolutionCandidates(discrete_scans, search_parameters); + const Candidate2D best_candidate = BranchAndBound( + discrete_scans, search_parameters, lowest_resolution_candidates, + precomputation_grid_stack_->max_depth(), min_score); + if (best_candidate.score > min_score) { + *score = best_candidate.score; + *pose_estimate = transform::Rigid2d( + {initial_pose_estimate.translation().x() + best_candidate.x, + initial_pose_estimate.translation().y() + best_candidate.y}, + initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); + return true; + } + return false; +} + +std::vector +FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates( + const std::vector& discrete_scans, + const SearchParameters& search_parameters) const { + std::vector lowest_resolution_candidates = + GenerateLowestResolutionCandidates(search_parameters); + ScoreCandidates( + precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()), + discrete_scans, search_parameters, &lowest_resolution_candidates); + return lowest_resolution_candidates; +} + +std::vector +FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates( + const SearchParameters& search_parameters) const { + const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); + int num_candidates = 0; + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + const int num_lowest_resolution_linear_x_candidates = + (search_parameters.linear_bounds[scan_index].max_x - + search_parameters.linear_bounds[scan_index].min_x + linear_step_size) / + linear_step_size; + const int num_lowest_resolution_linear_y_candidates = + (search_parameters.linear_bounds[scan_index].max_y - + search_parameters.linear_bounds[scan_index].min_y + linear_step_size) / + linear_step_size; + num_candidates += num_lowest_resolution_linear_x_candidates * + num_lowest_resolution_linear_y_candidates; + } + std::vector candidates; + candidates.reserve(num_candidates); + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x; + x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; + x_index_offset += linear_step_size) { + for (int y_index_offset = + search_parameters.linear_bounds[scan_index].min_y; + y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; + y_index_offset += linear_step_size) { + candidates.emplace_back(scan_index, x_index_offset, y_index_offset, + search_parameters); + } + } + } + CHECK_EQ(candidates.size(), num_candidates); + return candidates; +} + +void FastCorrelativeScanMatcher2D::ScoreCandidates( + const PrecomputationGrid2D& precomputation_grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* const candidates) const { + for (Candidate2D& candidate : *candidates) { + int sum = 0; + for (const Eigen::Array2i& xy_index : + discrete_scans[candidate.scan_index]) { + const Eigen::Array2i proposed_xy_index( + xy_index.x() + candidate.x_index_offset, + xy_index.y() + candidate.y_index_offset); + sum += precomputation_grid.GetValue(proposed_xy_index); + } + candidate.score = precomputation_grid.ToScore( + sum / static_cast(discrete_scans[candidate.scan_index].size())); + } + std::sort(candidates->begin(), candidates->end(), + std::greater()); +} + +Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound( + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + const std::vector& candidates, const int candidate_depth, + float min_score) const { + if (candidate_depth == 0) { + // Return the best candidate. + return *candidates.begin(); + } + + Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters); + best_high_resolution_candidate.score = min_score; + for (const Candidate2D& candidate : candidates) { + if (candidate.score <= min_score) { + break; + } + std::vector higher_resolution_candidates; + const int half_width = 1 << (candidate_depth - 1); + for (int x_offset : {0, half_width}) { + if (candidate.x_index_offset + x_offset > + search_parameters.linear_bounds[candidate.scan_index].max_x) { + break; + } + for (int y_offset : {0, half_width}) { + if (candidate.y_index_offset + y_offset > + search_parameters.linear_bounds[candidate.scan_index].max_y) { + break; + } + higher_resolution_candidates.emplace_back( + candidate.scan_index, candidate.x_index_offset + x_offset, + candidate.y_index_offset + y_offset, search_parameters); + } + } + ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1), + discrete_scans, search_parameters, + &higher_resolution_candidates); + best_high_resolution_candidate = std::max( + best_high_resolution_candidate, + BranchAndBound(discrete_scans, search_parameters, + higher_resolution_candidates, candidate_depth - 1, + best_high_resolution_candidate.score)); + } + return best_high_resolution_candidate; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h new file mode 100644 index 0000000..6498307 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.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. + */ + +// This is an implementation of the algorithm described in "Real-Time +// Correlative Scan Matching" by Olson. +// +// It is similar to the RealTimeCorrelativeScanMatcher but has a different +// trade-off: Scan matching is faster because more effort is put into the +// precomputation done for a given map. However, this map is immutable after +// construction. + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::FastCorrelativeScanMatcherOptions2D +CreateFastCorrelativeScanMatcherOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +// A precomputed grid that contains in each cell (x0, y0) the maximum +// probability in the width x width area defined by x0 <= x < x0 + width and +// y0 <= y < y0. +class PrecomputationGrid2D { + public: + PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width, + std::vector* reusable_intermediate_grid); + + // Returns a value between 0 and 255 to represent probabilities between + // min_score and max_score. + int GetValue(const Eigen::Array2i& xy_index) const { + const Eigen::Array2i local_xy_index = xy_index - offset_; + // The static_cast is for performance to check with 2 comparisons + // xy_index.x() < offset_.x() || xy_index.y() < offset_.y() || + // local_xy_index.x() >= wide_limits_.num_x_cells || + // local_xy_index.y() >= wide_limits_.num_y_cells + // instead of using 4 comparisons. + if (static_cast(local_xy_index.x()) >= + static_cast(wide_limits_.num_x_cells) || + static_cast(local_xy_index.y()) >= + static_cast(wide_limits_.num_y_cells)) { + return 0; + } + const int stride = wide_limits_.num_x_cells; + return cells_[local_xy_index.x() + local_xy_index.y() * stride]; + } + + // Maps values from [0, 255] to [min_score, max_score]. + float ToScore(float value) const { + return min_score_ + value * ((max_score_ - min_score_) / 255.f); + } + + private: + uint8 ComputeCellValue(float probability) const; + + // Offset of the precomputation grid in relation to the 'grid' + // including the additional 'width' - 1 cells. + const Eigen::Array2i offset_; + + // Size of the precomputation grid. + const CellLimits wide_limits_; + + const float min_score_; + const float max_score_; + + // Probabilites mapped to 0 to 255. + std::vector cells_; +}; + +class PrecomputationGridStack2D { + public: + PrecomputationGridStack2D( + const Grid2D& grid, + const proto::FastCorrelativeScanMatcherOptions2D& options); + + const PrecomputationGrid2D& Get(int index) { + return precomputation_grids_[index]; + } + + int max_depth() const { return precomputation_grids_.size() - 1; } + + private: + std::vector precomputation_grids_; +}; + +// An implementation of "Real-Time Correlative Scan Matching" by Olson. +class FastCorrelativeScanMatcher2D { + public: + FastCorrelativeScanMatcher2D( + const Grid2D& grid, + const proto::FastCorrelativeScanMatcherOptions2D& options); + ~FastCorrelativeScanMatcher2D(); + + FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete; + FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) = + delete; + + // Aligns 'point_cloud' within the 'grid' given an + // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) + // is possible, true is returned, and 'score' and 'pose_estimate' are updated + // with the result. + bool Match(const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, float min_score, + float* score, transform::Rigid2d* pose_estimate) const; + + // Aligns 'point_cloud' within the full 'grid', i.e., not + // restricted to the configured search window. If a score above 'min_score' + // (excluding equality) is possible, true is returned, and 'score' and + // 'pose_estimate' are updated with the result. + bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score, + float* score, transform::Rigid2d* pose_estimate) const; + + private: + // The actual implementation of the scan matcher, called by Match() and + // MatchFullSubmap() with appropriate 'initial_pose_estimate' and + // 'search_parameters'. + bool MatchWithSearchParameters( + SearchParameters search_parameters, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, float min_score, float* score, + transform::Rigid2d* pose_estimate) const; + std::vector ComputeLowestResolutionCandidates( + const std::vector& discrete_scans, + const SearchParameters& search_parameters) const; + std::vector GenerateLowestResolutionCandidates( + const SearchParameters& search_parameters) const; + void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* const candidates) const; + Candidate2D BranchAndBound(const std::vector& discrete_scans, + const SearchParameters& search_parameters, + const std::vector& candidates, + int candidate_depth, float min_score) const; + + const proto::FastCorrelativeScanMatcherOptions2D options_; + MapLimits limits_; + std::unique_ptr precomputation_grid_stack_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc new file mode 100644 index 0000000..2de8393 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -0,0 +1,251 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" + +#include +#include +#include +#include +#include + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +TEST(PrecomputationGridTest, CorrectValues) { + // Create a probability grid with random values that can be exactly + // represented by uint8 values. + std::mt19937 prng(42); + std::uniform_int_distribution distribution(0, 255); + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)), + &conversion_tables); + std::vector reusable_intermediate_grid; + PrecomputationGrid2D precomputation_grid_dummy( + probability_grid, probability_grid.limits().cell_limits(), 1, + &reusable_intermediate_grid); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { + probability_grid.SetProbability( + xy_index, precomputation_grid_dummy.ToScore(distribution(prng))); + } + + reusable_intermediate_grid.clear(); + for (const int width : {1, 2, 3, 8}) { + PrecomputationGrid2D precomputation_grid( + probability_grid, probability_grid.limits().cell_limits(), width, + &reusable_intermediate_grid); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + float max_score = -std::numeric_limits::infinity(); + for (int y = 0; y != width; ++y) { + for (int x = 0; x != width; ++x) { + max_score = std::max( + max_score, + probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); + } + } + EXPECT_NEAR( + max_score, + precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)), + 1e-4); + } + } +} + +TEST(PrecomputationGridTest, TinyProbabilityGrid) { + std::mt19937 prng(42); + std::uniform_int_distribution distribution(0, 255); + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)), + &conversion_tables); + std::vector reusable_intermediate_grid; + PrecomputationGrid2D precomputation_grid_dummy( + probability_grid, probability_grid.limits().cell_limits(), 1, + &reusable_intermediate_grid); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + probability_grid.SetProbability( + xy_index, precomputation_grid_dummy.ToScore(distribution(prng))); + } + + reusable_intermediate_grid.clear(); + for (const int width : {1, 2, 3, 8, 200}) { + PrecomputationGrid2D precomputation_grid( + probability_grid, probability_grid.limits().cell_limits(), width, + &reusable_intermediate_grid); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + float max_score = -std::numeric_limits::infinity(); + for (int y = 0; y != width; ++y) { + for (int x = 0; x != width; ++x) { + max_score = std::max( + max_score, + probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); + } + } + EXPECT_NEAR( + max_score, + precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)), + 1e-4); + } + } +} + +proto::FastCorrelativeScanMatcherOptions2D +CreateFastCorrelativeScanMatcherTestOptions2D( + const int branch_and_bound_depth) { + auto parameter_dictionary = + common::MakeDictionary(R"text( + return { + linear_search_window = 3., + angular_search_window = 1., + branch_and_bound_depth = )text" + + std::to_string(branch_and_bound_depth) + "}"); + return CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get()); +} + +mapping::proto::ProbabilityGridRangeDataInserterOptions2D +CreateRangeDataInserterTestOptions2D() { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + insert_free_space = true, + hit_probability = 0.7, + miss_probability = 0.4, + })text"); + return mapping::CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary.get()); +} + +TEST(FastCorrelativeScanMatcherTest, CorrectPose) { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + ProbabilityGridRangeDataInserter2D range_data_inserter( + CreateRangeDataInserterTestOptions2D()); + constexpr float kMinScore = 0.1f; + const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3); + + sensor::PointCloud point_cloud; + point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}}); + + for (int i = 0; i != 50; ++i) { + const transform::Rigid2f expected_pose( + {2. * distribution(prng), 2. * distribution(prng)}, + 0.5 * distribution(prng)); + + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)), + &conversion_tables); + range_data_inserter.Insert( + sensor::RangeData{ + Eigen::Vector3f(expected_pose.translation().x(), + expected_pose.translation().y(), 0.f), + sensor::TransformPointCloud( + point_cloud, transform::Embed3D(expected_pose.cast())), + {}}, + &probability_grid); + probability_grid.FinishUpdate(); + + FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid, + options); + transform::Rigid2d pose_estimate; + float score; + EXPECT_TRUE(fast_correlative_scan_matcher.Match( + transform::Rigid2d::Identity(), point_cloud, kMinScore, &score, + &pose_estimate)); + EXPECT_LT(kMinScore, score); + EXPECT_THAT(expected_pose, + transform::IsNearly(pose_estimate.cast(), 0.03f)) + << "Actual: " << transform::ToProto(pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } +} + +TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + ProbabilityGridRangeDataInserter2D range_data_inserter( + CreateRangeDataInserterTestOptions2D()); + constexpr float kMinScore = 0.1f; + const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6); + + sensor::PointCloud unperturbed_point_cloud; + unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.25f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{0.25f, 1.6f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}}); + + for (int i = 0; i != 20; ++i) { + const transform::Rigid2f perturbation( + {10. * distribution(prng), 10. * distribution(prng)}, + 1.6 * distribution(prng)); + const sensor::PointCloud point_cloud = sensor::TransformPointCloud( + unperturbed_point_cloud, transform::Embed3D(perturbation)); + const transform::Rigid2f expected_pose = + transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)}, + 0.5 * distribution(prng)) * + perturbation.inverse(); + + ValueConversionTables conversion_tables; + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)), + &conversion_tables); + range_data_inserter.Insert( + sensor::RangeData{ + transform::Embed3D(expected_pose * perturbation).translation(), + sensor::TransformPointCloud(point_cloud, + transform::Embed3D(expected_pose)), + {}}, + &probability_grid); + probability_grid.FinishUpdate(); + + FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid, + options); + transform::Rigid2d pose_estimate; + float score; + EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap( + point_cloud, kMinScore, &score, &pose_estimate)); + EXPECT_LT(kMinScore, score); + EXPECT_THAT(expected_pose, + transform::IsNearly(pose_estimate.cast(), 0.03f)) + << "Actual: " << transform::ToProto(pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h new file mode 100644 index 0000000..5b014fd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h @@ -0,0 +1,136 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ + +#include + +#include "cartographer/mapping/internal/2d/tsdf_2d.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Interpolates between TSDF2D pixels with bilinear interpolation. +// +// This class is templated to work with Ceres autodiff. +class InterpolatedTSDF2D { + public: + explicit InterpolatedTSDF2D(const TSDF2D& grid) : tsdf_(grid) {} + + InterpolatedTSDF2D(const InterpolatedTSDF2D&) = delete; + InterpolatedTSDF2D& operator=(const InterpolatedTSDF2D&) = delete; + + // Returns the interpolated correspondence cost at (x,y). Cells with at least + // one 'unknown' interpolation point result in "MaxCorrespondenceCost()" with + // zero gradient. + template + T GetCorrespondenceCost(const T& x, const T& y) const { + float x1, y1, x2, y2; + ComputeInterpolationDataPoints(x, y, &x1, &y1, &x2, &y2); + + const Eigen::Array2i index1 = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1)); + + const float w11 = tsdf_.GetWeight(index1); + const float w12 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, 0)); + const float w21 = tsdf_.GetWeight(index1 + Eigen::Array2i(0, -1)); + const float w22 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, -1)); + if (w11 == 0.0 || w12 == 0.0 || w21 == 0.0 || w22 == 0.0) { + return T(tsdf_.GetMaxCorrespondenceCost()); + } + + const float q11 = tsdf_.GetCorrespondenceCost(index1); + const float q12 = + tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(-1, 0)); + const float q21 = + tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(0, -1)); + const float q22 = + tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(-1, -1)); + + return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22); + } + + // Returns the interpolated weight at (x,y). + template + T GetWeight(const T& x, const T& y) const { + float x1, y1, x2, y2; + ComputeInterpolationDataPoints(x, y, &x1, &y1, &x2, &y2); + + const Eigen::Array2i index1 = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1)); + const float q11 = tsdf_.GetWeight(index1); + const float q12 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, 0)); + const float q21 = tsdf_.GetWeight(index1 + Eigen::Array2i(0, -1)); + const float q22 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, -1)); + + return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22); + } + + private: + template + void ComputeInterpolationDataPoints(const T& x, const T& y, float* x1, + float* y1, float* x2, float* y2) const { + const Eigen::Vector2f lower = CenterOfLowerPixel(x, y); + *x1 = lower.x(); + *y1 = lower.y(); + *x2 = lower.x() + static_cast(tsdf_.limits().resolution()); + *y2 = lower.y() + static_cast(tsdf_.limits().resolution()); + } + + template + T InterpolateBilinear(const T& x, const T& y, float x1, float y1, float x2, + float y2, float q11, float q12, float q21, + float q22) const { + const T normalized_x = (x - T(x1)) / T(x2 - x1); + const T normalized_y = (y - T(y1)) / T(y2 - y1); + const T q1 = T(q12 - q11) * normalized_y + T(q11); + const T q2 = T(q22 - q21) * normalized_y + T(q21); + return T(q2 - q1) * normalized_x + T(q1); + } + + // Center of the next lower pixel, i.e., not necessarily the pixel containing + // (x, y). For each dimension, the largest pixel index so that the + // corresponding center is at most the given coordinate. + Eigen::Vector2f CenterOfLowerPixel(double x, double y) const { + // Center of the cell containing (x, y). + Eigen::Vector2f center = tsdf_.limits().GetCellCenter( + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y))); + // Move to the next lower pixel center. + if (center.x() > x) { + center.x() -= tsdf_.limits().resolution(); + } + if (center.y() > y) { + center.y() -= tsdf_.limits().resolution(); + } + return center; + } + + // Uses the scalar part of a Ceres Jet. + template + Eigen::Vector2f CenterOfLowerPixel(const T& jet_x, const T& jet_y) const { + return CenterOfLowerPixel(jet_x.a, jet_y.a); + } + + const TSDF2D& tsdf_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc new file mode 100644 index 0000000..c1f4fa6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc @@ -0,0 +1,127 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h" + +#include "Eigen/Core" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class InterpolatedTSDF2DTest : public ::testing::Test { + protected: + InterpolatedTSDF2DTest() + : tsdf_(MapLimits(1., Eigen::Vector2d(5.5, 5.5), CellLimits(10, 10)), + 1.0f, 10.0f, &conversion_tables_), + interpolated_tsdf_(tsdf_) {} + + float GetUninterpolatedCorrespondenceCost(float x, float y) const { + return tsdf_.GetCorrespondenceCost( + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y))); + } + + float GetUninterpolatedWeight(float x, float y) const { + return tsdf_.GetWeight(tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y))); + } + + ValueConversionTables conversion_tables_; + TSDF2D tsdf_; + InterpolatedTSDF2D interpolated_tsdf_; +}; + +TEST_F(InterpolatedTSDF2DTest, InterpolatesGridPoints) { + std::vector inner_points = { + Eigen::Vector2f(1.f, 1.f), Eigen::Vector2f(2.f, 1.f), + Eigen::Vector2f(1.f, 2.f), Eigen::Vector2f(2.f, 2.f)}; + for (const Eigen::Vector2f& point : inner_points) { + tsdf_.SetCell(tsdf_.limits().GetCellIndex(point), 0.1f, 1.0f); + } + // Insert surrounding points. + for (size_t x = 0; x < 4; ++x) { + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(static_cast(x), 0.f)), + 0.1f, 1.0f); + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(static_cast(x), 3.f)), + 0.1f, 1.0f); + } + for (size_t y = 1; y < 3; ++y) { + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(0.f, static_cast(y))), + 0.1f, 1.0f); + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(3.f, static_cast(y))), + 0.1f, 1.0f); + } + for (const Eigen::Vector2f& point : inner_points) { + EXPECT_NEAR( + GetUninterpolatedCorrespondenceCost(point[0], point[1]), + interpolated_tsdf_.GetCorrespondenceCost(static_cast(point[0]), + static_cast(point[1])), + 1e-4); + EXPECT_NEAR(GetUninterpolatedWeight(point[0], point[1]), + interpolated_tsdf_.GetWeight(static_cast(point[0]), + static_cast(point[1])), + 1e-4); + } + // Check unknown cell. + EXPECT_NEAR(tsdf_.GetMaxCorrespondenceCost(), + interpolated_tsdf_.GetCorrespondenceCost(3.0, 2.0), 1e-4); + EXPECT_NEAR(GetUninterpolatedWeight(3.f, 2.f), + interpolated_tsdf_.GetWeight(3.0, 2.0), 1e-4); +} + +TEST_F(InterpolatedTSDF2DTest, InterpolatesWithinCell) { + const float tsd_00 = 0.1f; + const float tsd_01 = 0.2f; + const float tsd_10 = 0.3f; + const float tsd_11 = 0.4f; + const float w_00 = 1.f; + const float w_01 = 2.f; + const float w_10 = 3.f; + const float w_11 = 4.f; + + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.f, 0.f)), tsd_00, + w_00); + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.f, 1.f)), tsd_01, + w_01); + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(1.f, 0.f)), tsd_10, + w_10); + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(1.f, 1.f)), tsd_11, + w_11); + + const double kSampleStep = tsdf_.limits().resolution() / 100.; + for (double x = 0. + kSampleStep; x < 1.; x += tsdf_.limits().resolution()) { + for (double y = 0. + kSampleStep; y < 1.; + y += tsdf_.limits().resolution()) { + const float tsd_expected = (x * tsd_10 + (1.f - x) * tsd_00) * (1.f - y) + + (x * tsd_11 + (1.f - x) * tsd_01) * y; + EXPECT_NEAR(interpolated_tsdf_.GetCorrespondenceCost(x, y), tsd_expected, + 1e-3); + const float w_expected = (x * w_10 + (1.f - x) * w_00) * (1.f - y) + + (x * w_11 + (1.f - x) * w_01) * y; + EXPECT_NEAR(interpolated_tsdf_.GetWeight(x, y), w_expected, 1e-3); + } + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc new file mode 100644 index 0000000..0c4422e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc @@ -0,0 +1,121 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" + +#include "cartographer/mapping/probability_values.h" +#include "ceres/cubic_interpolation.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +// Computes a cost for matching the 'point_cloud' to the 'grid' with +// a 'pose'. The cost increases with poorer correspondence of the grid and the +// point observation (e.g. points falling into less occupied space). +class OccupiedSpaceCostFunction2D { + public: + OccupiedSpaceCostFunction2D(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const Grid2D& grid) + : scaling_factor_(scaling_factor), + point_cloud_(point_cloud), + grid_(grid) {} + + template + bool operator()(const T* const pose, T* residual) const { + Eigen::Matrix translation(pose[0], pose[1]); + Eigen::Rotation2D rotation(pose[2]); + Eigen::Matrix rotation_matrix = rotation.toRotationMatrix(); + Eigen::Matrix transform; + transform << rotation_matrix, translation, T(0.), T(0.), T(1.); + + const GridArrayAdapter adapter(grid_); + ceres::BiCubicInterpolator interpolator(adapter); + const MapLimits& limits = grid_.limits(); + + for (size_t i = 0; i < point_cloud_.size(); ++i) { + // Note that this is a 2D point. The third component is a scaling factor. + const Eigen::Matrix point((T(point_cloud_[i].position.x())), + (T(point_cloud_[i].position.y())), + T(1.)); + const Eigen::Matrix world = transform * point; + interpolator.Evaluate( + (limits.max().x() - world[0]) / limits.resolution() - 0.5 + + static_cast(kPadding), + (limits.max().y() - world[1]) / limits.resolution() - 0.5 + + static_cast(kPadding), + &residual[i]); + residual[i] = scaling_factor_ * residual[i]; + } + return true; + } + + private: + static constexpr int kPadding = INT_MAX / 4; + class GridArrayAdapter { + public: + enum { DATA_DIMENSION = 1 }; + + explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {} + + void GetValue(const int row, const int column, double* const value) const { + if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || + column >= NumCols() - kPadding) { + *value = kMaxCorrespondenceCost; + } else { + *value = static_cast(grid_.GetCorrespondenceCost( + Eigen::Array2i(column - kPadding, row - kPadding))); + } + } + + int NumRows() const { + return grid_.limits().cell_limits().num_y_cells + 2 * kPadding; + } + + int NumCols() const { + return grid_.limits().cell_limits().num_x_cells + 2 * kPadding; + } + + private: + const Grid2D& grid_; + }; + + OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete; + OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) = + delete; + + const double scaling_factor_; + const sensor::PointCloud& point_cloud_; + const Grid2D& grid_; +}; + +} // namespace + +ceres::CostFunction* CreateOccupiedSpaceCostFunction2D( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const Grid2D& grid) { + return new ceres::AutoDiffCostFunction( + new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h new file mode 100644 index 0000000..7e0c7f7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h @@ -0,0 +1,39 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_ + +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/sensor/point_cloud.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Creates a cost function for matching the 'point_cloud' to the 'grid' with +// a 'pose'. The cost increases with poorer correspondence of the grid and the +// point observation (e.g. points falling into less occupied space). +ceres::CostFunction* CreateOccupiedSpaceCostFunction2D( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const Grid2D& grid); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc new file mode 100644 index 0000000..c9ef182 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -0,0 +1,57 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" + +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/probability_values.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleEq; +using ::testing::ElementsAre; + +TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { + ValueConversionTables conversion_tables; + ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), + &conversion_tables); + sensor::PointCloud point_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}}); + ceres::Problem problem; + std::unique_ptr cost_function( + CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); + + const std::array pose_estimate{{0., 0., 0.}}; + const std::array parameter_blocks{{pose_estimate.data()}}; + + std::array residuals; + std::array, 1> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data(); + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + jacobians_ptrs.data()); + + EXPECT_THAT(residuals, ElementsAre(DoubleEq(kMaxProbability))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc new file mode 100644 index 0000000..60b00fe --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -0,0 +1,180 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h" + +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +float ComputeCandidateScore(const TSDF2D& tsdf, + const DiscreteScan2D& discrete_scan, + int x_index_offset, int y_index_offset) { + float candidate_score = 0.f; + float summed_weight = 0.f; + for (const Eigen::Array2i& xy_index : discrete_scan) { + const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset, + xy_index.y() + y_index_offset); + const std::pair tsd_and_weight = + tsdf.GetTSDAndWeight(proposed_xy_index); + const float normalized_tsd_score = + (tsdf.GetMaxCorrespondenceCost() - std::abs(tsd_and_weight.first)) / + tsdf.GetMaxCorrespondenceCost(); + const float weight = tsd_and_weight.second; + candidate_score += normalized_tsd_score * weight; + summed_weight += weight; + } + if (summed_weight == 0.f) return 0.f; + candidate_score /= summed_weight; + CHECK_GE(candidate_score, 0.f); + return candidate_score; +} + +float ComputeCandidateScore(const ProbabilityGrid& probability_grid, + const DiscreteScan2D& discrete_scan, + int x_index_offset, int y_index_offset) { + float candidate_score = 0.f; + for (const Eigen::Array2i& xy_index : discrete_scan) { + const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset, + xy_index.y() + y_index_offset); + const float probability = + probability_grid.GetProbability(proposed_xy_index); + candidate_score += probability; + } + candidate_score /= static_cast(discrete_scan.size()); + CHECK_GT(candidate_score, 0.f); + return candidate_score; +} + +} // namespace + +RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D( + const proto::RealTimeCorrelativeScanMatcherOptions& options) + : options_(options) {} + +std::vector +RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates( + const SearchParameters& search_parameters) const { + int num_candidates = 0; + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + const int num_linear_x_candidates = + (search_parameters.linear_bounds[scan_index].max_x - + search_parameters.linear_bounds[scan_index].min_x + 1); + const int num_linear_y_candidates = + (search_parameters.linear_bounds[scan_index].max_y - + search_parameters.linear_bounds[scan_index].min_y + 1); + num_candidates += num_linear_x_candidates * num_linear_y_candidates; + } + std::vector candidates; + candidates.reserve(num_candidates); + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x; + x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; + ++x_index_offset) { + for (int y_index_offset = + search_parameters.linear_bounds[scan_index].min_y; + y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; + ++y_index_offset) { + candidates.emplace_back(scan_index, x_index_offset, y_index_offset, + search_parameters); + } + } + } + CHECK_EQ(candidates.size(), num_candidates); + return candidates; +} + +double RealTimeCorrelativeScanMatcher2D::Match( + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, const Grid2D& grid, + transform::Rigid2d* pose_estimate) const { + CHECK(pose_estimate != nullptr); + + const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); + const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud( + point_cloud, + transform::Rigid3f::Rotation(Eigen::AngleAxisf( + initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); + const SearchParameters search_parameters( + options_.linear_search_window(), options_.angular_search_window(), + rotated_point_cloud, grid.limits().resolution()); + + const std::vector rotated_scans = + GenerateRotatedScans(rotated_point_cloud, search_parameters); + const std::vector discrete_scans = DiscretizeScans( + grid.limits(), rotated_scans, + Eigen::Translation2f(initial_pose_estimate.translation().x(), + initial_pose_estimate.translation().y())); + std::vector candidates = + GenerateExhaustiveSearchCandidates(search_parameters); + ScoreCandidates(grid, discrete_scans, search_parameters, &candidates); + + const Candidate2D& best_candidate = + *std::max_element(candidates.begin(), candidates.end()); + *pose_estimate = transform::Rigid2d( + {initial_pose_estimate.translation().x() + best_candidate.x, + initial_pose_estimate.translation().y() + best_candidate.y}, + initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); + return best_candidate.score; +} + +void RealTimeCorrelativeScanMatcher2D::ScoreCandidates( + const Grid2D& grid, const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* const candidates) const { + for (Candidate2D& candidate : *candidates) { + switch (grid.GetGridType()) { + case GridType::PROBABILITY_GRID: + candidate.score = ComputeCandidateScore( + static_cast(grid), + discrete_scans[candidate.scan_index], candidate.x_index_offset, + candidate.y_index_offset); + break; + case GridType::TSDF: + candidate.score = ComputeCandidateScore( + static_cast(grid), + discrete_scans[candidate.scan_index], candidate.x_index_offset, + candidate.y_index_offset); + break; + } + candidate.score *= + std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) * + options_.translation_delta_cost_weight() + + std::abs(candidate.orientation) * + options_.rotation_delta_cost_weight())); + } +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h new file mode 100644 index 0000000..32789ad --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h @@ -0,0 +1,91 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_ + +// This is an implementation of the algorithm described in "Real-Time +// Correlative Scan Matching" by Olson. +// +// The correlative scan matching algorithm is exhaustively evaluating the scan +// matching search space. As described by the paper, the basic steps are: +// +// 1) Evaluate the probability p(z|xi, m) over the entire 3D search window using +// the low-resolution table. +// 2) Find the best voxel in the low-resolution 3D space that has not already +// been considered. Denote this value as Li. If Li < Hbest, terminate: Hbest is +// the best scan matching alignment. +// 3) Evaluate the search volume inside voxel i using the high resolution table. +// Suppose the log-likelihood of this voxel is Hi. Note that Hi <= Li since the +// low-resolution map overestimates the log likelihoods. If Hi > Hbest, set +// Hbest = Hi. +// +// This can be made even faster by transforming the scan exactly once over some +// discretized range. + +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" +#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// An implementation of "Real-Time Correlative Scan Matching" by Olson. +class RealTimeCorrelativeScanMatcher2D { + public: + explicit RealTimeCorrelativeScanMatcher2D( + const proto::RealTimeCorrelativeScanMatcherOptions& options); + + RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) = + delete; + RealTimeCorrelativeScanMatcher2D& operator=( + const RealTimeCorrelativeScanMatcher2D&) = delete; + + // Aligns 'point_cloud' within the 'grid' given an + // 'initial_pose_estimate' then updates 'pose_estimate' with the result and + // returns the score. + double Match(const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, const Grid2D& grid, + transform::Rigid2d* pose_estimate) const; + + // Computes the score for each Candidate2D in a collection. The cost is + // computed as the sum of probabilities or normalized TSD values, different + // from the Ceres CostFunctions: http://ceres-solver.org/modeling.html + // + // Visible for testing. + void ScoreCandidates(const Grid2D& grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* candidates) const; + + private: + std::vector GenerateExhaustiveSearchCandidates( + const SearchParameters& search_parameters) const; + + const proto::RealTimeCorrelativeScanMatcherOptions options_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc new file mode 100644 index 0000000..71fb0a9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -0,0 +1,203 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h" + +#include +#include + +#include "Eigen/Geometry" +#include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherTestOptions2D() { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "linear_search_window = 0.6, " + "angular_search_window = 0.16, " + "translation_delta_cost_weight = 0., " + "rotation_delta_cost_weight = 0., " + "}"); + return CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary.get()); +} + +class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { + protected: + RealTimeCorrelativeScanMatcherTest() { + point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}}); + real_time_correlative_scan_matcher_ = + absl::make_unique( + CreateRealTimeCorrelativeScanMatcherTestOptions2D()); + } + + void SetUpTSDF() { + grid_ = absl::make_unique( + MapLimits(0.05, Eigen::Vector2d(0.3, 0.5), CellLimits(20, 20)), 0.3, + 1.0, &conversion_tables_); + { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + truncation_distance = 0.3, + maximum_weight = 10., + update_free_space = false, + normal_estimation_options = { + num_normal_samples = 4, + sample_radius = 0.5, + }, + project_sdf_distance_to_scan_normal = true, + update_weight_range_exponent = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5, + update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5, + })text"); + range_data_inserter_ = absl::make_unique( + CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get())); + } + range_data_inserter_->Insert( + sensor::RangeData{Eigen::Vector3f(0.5f, -0.5f, 0.f), point_cloud_, {}}, + grid_.get()); + grid_->FinishUpdate(); + } + + void SetUpProbabilityGrid() { + grid_ = absl::make_unique( + MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)), + &conversion_tables_); + { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "insert_free_space = true, " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "}"); + range_data_inserter_ = + absl::make_unique( + CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary.get())); + } + range_data_inserter_->Insert( + sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, + grid_.get()); + grid_->FinishUpdate(); + } + + ValueConversionTables conversion_tables_; + std::unique_ptr grid_; + std::unique_ptr range_data_inserter_; + sensor::PointCloud point_cloud_; + std::unique_ptr + real_time_correlative_scan_matcher_; +}; + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePerfectHighResolutionCandidateProbabilityGrid) { + SetUpProbabilityGrid(); + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(0, candidates[0].y_index_offset); + // Every point should align perfectly. + EXPECT_NEAR(0.7, candidates[0].score, 1e-2); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePerfectHighResolutionCandidateTSDF) { + SetUpTSDF(); + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(0, candidates[0].y_index_offset); + // Every point should align perfectly. + EXPECT_NEAR(1.0, candidates[0].score, 1e-1); + EXPECT_LT(0.95, candidates[0].score); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePartiallyCorrectHighResolutionCandidateProbabilityGrid) { + SetUpProbabilityGrid(); + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(1, candidates[0].y_index_offset); + // 3 points should align perfectly. + EXPECT_LT(0.7 * 3. / 7., candidates[0].score); + EXPECT_GT(0.7, candidates[0].score); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePartiallyCorrectHighResolutionCandidateTSDF) { + SetUpTSDF(); + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(1, candidates[0].y_index_offset); + // 3 points should align perfectly. + EXPECT_LT(1.0 - 4. / (7. * 6.), candidates[0].score); + EXPECT_GT(1.0, candidates[0].score); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h new file mode 100644 index 0000000..4c1e25a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ + +#include "Eigen/Core" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with +// the solution's distance from 'target_angle'. +class RotationDeltaCostFunctor2D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const double target_angle) { + return new ceres::AutoDiffCostFunction< + RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>( + new RotationDeltaCostFunctor2D(scaling_factor, target_angle)); + } + + template + bool operator()(const T* const pose, T* residual) const { + residual[0] = scaling_factor_ * (pose[2] - angle_); + return true; + } + + private: + explicit RotationDeltaCostFunctor2D(const double scaling_factor, + const double target_angle) + : scaling_factor_(scaling_factor), angle_(target_angle) {} + + RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete; + RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) = + delete; + + const double scaling_factor_; + const double angle_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h new file mode 100644 index 0000000..b92f7c7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ + +#include "Eigen/Core" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes the cost of translating 'pose' to 'target_translation'. +// Cost increases with the solution's distance from 'target_translation'. +class TranslationDeltaCostFunctor2D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const Eigen::Vector2d& target_translation) { + return new ceres::AutoDiffCostFunction( + new TranslationDeltaCostFunctor2D(scaling_factor, target_translation)); + } + + template + bool operator()(const T* const pose, T* residual) const { + residual[0] = scaling_factor_ * (pose[0] - x_); + residual[1] = scaling_factor_ * (pose[1] - y_); + return true; + } + + private: + // Constructs a new TranslationDeltaCostFunctor2D from the given + // 'target_translation' (x, y). + explicit TranslationDeltaCostFunctor2D( + const double scaling_factor, const Eigen::Vector2d& target_translation) + : scaling_factor_(scaling_factor), + x_(target_translation.x()), + y_(target_translation.y()) {} + + TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete; + TranslationDeltaCostFunctor2D& operator=( + const TranslationDeltaCostFunctor2D&) = delete; + + const double scaling_factor_; + const double x_; + const double y_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc new file mode 100644 index 0000000..3259c8d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc @@ -0,0 +1,89 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h" +#include "cartographer/sensor/point_cloud.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes a cost for matching the 'point_cloud' in the 'grid' at +// a 'pose'. The cost increases with the signed distance of the matched point +// location in the 'grid'. +class TSDFMatchCostFunction2D { + public: + TSDFMatchCostFunction2D(const double residual_scaling_factor, + const sensor::PointCloud& point_cloud, + const TSDF2D& grid) + : residual_scaling_factor_(residual_scaling_factor), + point_cloud_(point_cloud), + interpolated_grid_(grid) {} + + template + bool operator()(const T* const pose, T* residual) const { + const Eigen::Matrix translation(pose[0], pose[1]); + const Eigen::Rotation2D rotation(pose[2]); + const Eigen::Matrix rotation_matrix = rotation.toRotationMatrix(); + Eigen::Matrix transform; + transform << rotation_matrix, translation, T(0.), T(0.), T(1.); + + T summed_weight = T(0); + for (size_t i = 0; i < point_cloud_.size(); ++i) { + // Note that this is a 2D point. The third component is a scaling factor. + const Eigen::Matrix point((T(point_cloud_[i].position.x())), + (T(point_cloud_[i].position.y())), + T(1.)); + const Eigen::Matrix world = transform * point; + const T point_weight = interpolated_grid_.GetWeight(world[0], world[1]); + summed_weight += point_weight; + residual[i] = + T(point_cloud_.size()) * residual_scaling_factor_ * + interpolated_grid_.GetCorrespondenceCost(world[0], world[1]) * + point_weight; + } + if (summed_weight == T(0)) return false; + for (size_t i = 0; i < point_cloud_.size(); ++i) { + residual[i] /= summed_weight; + } + return true; + } + + private: + TSDFMatchCostFunction2D(const TSDFMatchCostFunction2D&) = delete; + TSDFMatchCostFunction2D& operator=(const TSDFMatchCostFunction2D&) = delete; + + const double residual_scaling_factor_; + const sensor::PointCloud& point_cloud_; + const InterpolatedTSDF2D interpolated_grid_; +}; + +ceres::CostFunction* CreateTSDFMatchCostFunction2D( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const TSDF2D& tsdf) { + return new ceres::AutoDiffCostFunction( + new TSDFMatchCostFunction2D(scaling_factor, point_cloud, tsdf), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h new file mode 100644 index 0000000..8a1ce6d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h @@ -0,0 +1,39 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ + +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/sensor/point_cloud.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Creates a cost function for matching the 'point_cloud' in the 'grid' at +// a 'pose'. The cost increases with the signed distance of the matched point +// location in the 'grid'. +ceres::CostFunction* CreateTSDFMatchCostFunction2D( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const TSDF2D& grid); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc new file mode 100644 index 0000000..8c9cf04 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -0,0 +1,166 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h" + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleNear; +using ::testing::ElementsAre; + +class TSDFSpaceCostFunction2DTest : public ::testing::Test { + protected: + TSDFSpaceCostFunction2DTest() + : tsdf_(MapLimits(0.1, Eigen::Vector2d(2.05, 2.05), CellLimits(40, 40)), + 0.3, 1.0, &conversion_tables_) { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "truncation_distance = 0.3," + "maximum_weight = 1.0," + "update_free_space = false," + "normal_estimation_options = {" + "num_normal_samples = 2," + "sample_radius = 10.," + "}," + "project_sdf_distance_to_scan_normal = true," + "update_weight_range_exponent = 0," + "update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0," + "update_weight_distance_cell_to_hit_kernel_bandwidth = 0," + "}"); + options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()); + range_data_inserter_ = absl::make_unique(options_); + } + + void InsertPointcloud() { + auto range_data = sensor::RangeData(); + for (float x = -.5; x < 0.5f; x += 0.1) { + range_data.returns.push_back({Eigen::Vector3f{x, 1.0f, 0.f}}); + } + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + } + + ValueConversionTables conversion_tables_; + proto::TSDFRangeDataInserterOptions2D options_; + TSDF2D tsdf_; + std::unique_ptr range_data_inserter_; +}; + +TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { + const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}}); + std::unique_ptr cost_function( + CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); + const std::array pose_estimate{{0., 0., 0.}}; + const std::array parameter_blocks{{pose_estimate.data()}}; + std::array residuals; + std::array, 1> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data(); + bool valid_result = cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobians_ptrs.data()); + EXPECT_FALSE(valid_result); +} + +TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { + InsertPointcloud(); + const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); + std::unique_ptr cost_function( + CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); + const std::array pose_estimate{{0., 0., 0.}}; + const std::array parameter_blocks{{pose_estimate.data()}}; + std::array residuals; + std::array, 1> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data(); + const bool valid_result = cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobians_ptrs.data()); + EXPECT_TRUE(valid_result); + EXPECT_THAT(residuals, ElementsAre(DoubleNear(0., 1e-3))); + EXPECT_THAT(jacobians[0], + ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3), + DoubleNear(0., 1e-3))); +} + +TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { + InsertPointcloud(); + sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); + std::unique_ptr cost_function( + CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); + std::array pose_estimate{{0., 0.1, 0.}}; + std::array parameter_blocks{{pose_estimate.data()}}; + std::array residuals; + std::array, 1> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data(); + + bool valid_result = cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobians_ptrs.data()); + EXPECT_TRUE(valid_result); + EXPECT_THAT(residuals, ElementsAre(DoubleNear(-0.1, 1e-3))); + EXPECT_THAT(jacobians[0], + ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3), + DoubleNear(0., 1e-3))); + + pose_estimate[1] = -0.1; + parameter_blocks = {{pose_estimate.data()}}; + valid_result = cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobians_ptrs.data()); + EXPECT_TRUE(valid_result); + EXPECT_THAT(residuals, ElementsAre(DoubleNear(0.1, 1e-3))); + EXPECT_THAT(jacobians[0], + ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3), + DoubleNear(0., 1e-3))); +} + +TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) { + InsertPointcloud(); + sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); + std::unique_ptr cost_function( + CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); + std::array pose_estimate{{0., 0.4, 0.}}; + std::array parameter_blocks{{pose_estimate.data()}}; + std::array residuals; + std::array, 1> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data(); + + bool valid_result = cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobians_ptrs.data()); + EXPECT_FALSE(valid_result); + + pose_estimate[1] = -0.4; + parameter_blocks = {{pose_estimate.data()}}; + valid_result = cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobians_ptrs.data()); + EXPECT_FALSE(valid_result); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter.cc new file mode 100644 index 0000000..5feddad --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter.cc @@ -0,0 +1,35 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" + +namespace cartographer { +namespace mapping { + +TSDValueConverter::TSDValueConverter(float max_tsd, float max_weight, + ValueConversionTables* conversion_tables) + : max_tsd_(max_tsd), + min_tsd_(-max_tsd), + max_weight_(max_weight), + tsd_resolution_(32766.f / (max_tsd_ - min_tsd_)), + weight_resolution_(32766.f / (max_weight_ - min_weight_)), + value_to_tsd_( + conversion_tables->GetConversionTable(min_tsd_, min_tsd_, max_tsd_)), + value_to_weight_(conversion_tables->GetConversionTable( + min_weight_, min_weight_, max_weight)) {} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter.h new file mode 100644 index 0000000..f01d826 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter.h @@ -0,0 +1,104 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_TSD_VALUE_CONVERTER_H_ +#define CARTOGRAPHER_MAPPING_TSD_VALUE_CONVERTER_H_ + +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/value_conversion_tables.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +// Provides conversions between float and uint16 representations for +// truncated signed distance values and weights. +class TSDValueConverter { + public: + TSDValueConverter(float max_tsd, float max_weight, + ValueConversionTables* conversion_tables); + + // Converts a tsd to a uint16 in the [1, 32767] range. + inline uint16 TSDToValue(const float tsd) const { + const int value = + common::RoundToInt((ClampTSD(tsd) - min_tsd_) * tsd_resolution_) + 1; + DCHECK_GE(value, 1); + DCHECK_LE(value, 32767); + return value; + } + + // Converts a weight to a uint16 in the [1, 32767] range. + inline uint16 WeightToValue(const float weight) const { + const int value = common::RoundToInt((ClampWeight(weight) - min_weight_) * + weight_resolution_) + + 1; + DCHECK_GE(value, 1); + DCHECK_LE(value, 32767); + return value; + } + + // Converts a uint16 (which may or may not have the update marker set) to a + // value in the range [min_tsd_, max_tsd_]. + inline float ValueToTSD(const uint16 value) const { + return (*value_to_tsd_)[value]; + } + + // Converts a uint16 (which may or may not have the update marker set) to a + // value in the range [min_weight_, max_weight_]. + inline float ValueToWeight(const uint16 value) const { + return (*value_to_weight_)[value]; + } + + static uint16 getUnknownTSDValue() { return unknown_tsd_value_; } + static uint16 getUnknownWeightValue() { return unknown_weight_value_; } + static uint16 getUpdateMarker() { return update_marker_; } + float getMaxTSD() const { return max_tsd_; } + float getMinTSD() const { return min_tsd_; } + float getMaxWeight() const { return max_weight_; } + float getMinWeight() const { return min_weight_; } + + private: + // Clamps TSD to be in the range [min_tsd_, max_tsd_]. + inline float ClampTSD(const float tsd) const { + return common::Clamp(tsd, min_tsd_, max_tsd_); + } + // Clamps weight to be in the range [min_tsd_, max_tsd_]. + inline float ClampWeight(const float weight) const { + return common::Clamp(weight, min_weight_, max_weight_); + } + + float max_tsd_; + float min_tsd_; + float max_weight_; + float tsd_resolution_; + float weight_resolution_; + static constexpr float min_weight_ = 0.f; + static constexpr uint16 unknown_tsd_value_ = 0; + static constexpr uint16 unknown_weight_value_ = 0; + static constexpr uint16 update_marker_ = 1u << 15; + + const std::vector* value_to_tsd_; + const std::vector* value_to_weight_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TSD_VALUE_CONVERTER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter_test.cc new file mode 100644 index 0000000..1fa496e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsd_value_converter_test.cc @@ -0,0 +1,124 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +class TSDValueConverterTest : public ::testing::Test { + protected: + TSDValueConverterTest() + : truncation_distance_(0.1f), + max_weight_(10.0f), + tsd_value_converter_(truncation_distance_, max_weight_, + &conversion_tables) {} + ValueConversionTables conversion_tables; + float truncation_distance_; + float max_weight_; + TSDValueConverter tsd_value_converter_; +}; + +TEST_F(TSDValueConverterTest, DefaultValues) { + EXPECT_EQ(tsd_value_converter_.getUnknownWeightValue(), 0); + EXPECT_EQ(tsd_value_converter_.getUnknownTSDValue(), 0); + EXPECT_EQ(tsd_value_converter_.getMinTSD(), -truncation_distance_); + EXPECT_EQ(tsd_value_converter_.getMaxTSD(), truncation_distance_); + EXPECT_EQ(tsd_value_converter_.getMinWeight(), 0.f); + EXPECT_EQ(tsd_value_converter_.getMaxWeight(), max_weight_); +} + +TEST_F(TSDValueConverterTest, ValueToTSDConversions) { + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_EQ( + tsd_value_converter_.TSDToValue(tsd_value_converter_.ValueToTSD(i)), i); + } +} + +TEST_F(TSDValueConverterTest, ValueToTSDConversionsWithUpdateMarker) { + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_EQ(tsd_value_converter_.TSDToValue(tsd_value_converter_.ValueToTSD( + i + tsd_value_converter_.getUpdateMarker())), + i); + } +} + +TEST_F(TSDValueConverterTest, ValueToWeightConversions) { + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_EQ(tsd_value_converter_.WeightToValue( + tsd_value_converter_.ValueToWeight(i)), + i); + } +} + +TEST_F(TSDValueConverterTest, ValueToWeightConversionsWithUpdateMarker) { + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_EQ( + tsd_value_converter_.WeightToValue(tsd_value_converter_.ValueToWeight( + i + tsd_value_converter_.getUpdateMarker())), + i); + } +} + +TEST_F(TSDValueConverterTest, TSDToValueConversions) { + uint16 num_samples = 1000; + float tolerance = truncation_distance_ * 2.f / 32767.f; + for (uint16 i = 0; i < num_samples; ++i) { + float sdf_sample = + -truncation_distance_ + i * 2.f * truncation_distance_ / num_samples; + EXPECT_NEAR(tsd_value_converter_.ValueToTSD( + tsd_value_converter_.TSDToValue(sdf_sample)), + sdf_sample, tolerance); + } +} + +TEST_F(TSDValueConverterTest, WeightToValueConversions) { + uint16 num_samples = 1000; + float tolerance = max_weight_ / 32767.f; + for (uint16 i = 0; i < num_samples; ++i) { + float weight_sample = i * max_weight_ / num_samples; + EXPECT_NEAR(tsd_value_converter_.ValueToWeight( + tsd_value_converter_.WeightToValue(weight_sample)), + weight_sample, tolerance); + } +} + +TEST_F(TSDValueConverterTest, WeightToValueOutOfRangeConversions) { + float tolerance = max_weight_ / 32767.f; + EXPECT_NEAR(tsd_value_converter_.ValueToWeight( + tsd_value_converter_.WeightToValue(2.f * max_weight_)), + max_weight_, tolerance); + EXPECT_NEAR(tsd_value_converter_.ValueToWeight( + tsd_value_converter_.WeightToValue(-max_weight_)), + 0.f, tolerance); +} + +TEST_F(TSDValueConverterTest, TSDToValueOutOfRangeConversions) { + float tolerance = truncation_distance_ * 2.f / 32767.f; + EXPECT_NEAR(tsd_value_converter_.ValueToTSD( + tsd_value_converter_.TSDToValue(2.f * truncation_distance_)), + truncation_distance_, tolerance); + EXPECT_NEAR(tsd_value_converter_.ValueToTSD( + tsd_value_converter_.TSDToValue(-2.f * truncation_distance_)), + -truncation_distance_, tolerance); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d.cc new file mode 100644 index 0000000..fa6fc9b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d.cc @@ -0,0 +1,185 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/tsdf_2d.h" + +#include "absl/memory/memory.h" + +namespace cartographer { +namespace mapping { + +TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance, + float max_weight, ValueConversionTables* conversion_tables) + : Grid2D(limits, -truncation_distance, truncation_distance, + conversion_tables), + conversion_tables_(conversion_tables), + value_converter_(absl::make_unique( + truncation_distance, max_weight, conversion_tables_)), + weight_cells_( + limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells, + value_converter_->getUnknownWeightValue()) {} + +TSDF2D::TSDF2D(const proto::Grid2D& proto, + ValueConversionTables* conversion_tables) + : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { + CHECK(proto.has_tsdf_2d()); + value_converter_ = absl::make_unique( + proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(), + conversion_tables_); + weight_cells_.reserve(proto.tsdf_2d().weight_cells_size()); + for (const auto& cell : proto.tsdf_2d().weight_cells()) { + CHECK_LE(cell, std::numeric_limits::max()); + weight_cells_.push_back(cell); + } +} + +bool TSDF2D::CellIsUpdated(const Eigen::Array2i& cell_index) const { + const int flat_index = ToFlatIndex(cell_index); + uint16 tsdf_cell = correspondence_cost_cells()[flat_index]; + return tsdf_cell >= value_converter_->getUpdateMarker(); +} + +void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd, + float weight) { + const int flat_index = ToFlatIndex(cell_index); + uint16* tsdf_cell = &(*mutable_correspondence_cost_cells())[flat_index]; + if (*tsdf_cell >= value_converter_->getUpdateMarker()) { + return; + } + mutable_update_indices()->push_back(flat_index); + mutable_known_cells_box()->extend(cell_index.matrix()); + *tsdf_cell = + value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker(); + uint16* weight_cell = &weight_cells_[flat_index]; + *weight_cell = value_converter_->WeightToValue(weight); +} + +GridType TSDF2D::GetGridType() const { return GridType::TSDF; } + +float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const { + if (limits().Contains(cell_index)) { + return value_converter_->ValueToTSD( + correspondence_cost_cells()[ToFlatIndex(cell_index)]); + } + return value_converter_->getMinTSD(); +} + +float TSDF2D::GetWeight(const Eigen::Array2i& cell_index) const { + if (limits().Contains(cell_index)) { + return value_converter_->ValueToWeight( + weight_cells_[ToFlatIndex(cell_index)]); + } + return value_converter_->getMinWeight(); +} + +std::pair TSDF2D::GetTSDAndWeight( + const Eigen::Array2i& cell_index) const { + if (limits().Contains(cell_index)) { + int flat_index = ToFlatIndex(cell_index); + return std::make_pair( + value_converter_->ValueToTSD(correspondence_cost_cells()[flat_index]), + value_converter_->ValueToWeight(weight_cells_[flat_index])); + } + return std::make_pair(value_converter_->getMinTSD(), + value_converter_->getMinWeight()); +} + +void TSDF2D::GrowLimits(const Eigen::Vector2f& point) { + Grid2D::GrowLimits(point, + {mutable_correspondence_cost_cells(), &weight_cells_}, + {value_converter_->getUnknownTSDValue(), + value_converter_->getUnknownWeightValue()}); +} + +proto::Grid2D TSDF2D::ToProto() const { + proto::Grid2D result; + result = Grid2D::ToProto(); + *result.mutable_tsdf_2d()->mutable_weight_cells() = {weight_cells_.begin(), + weight_cells_.end()}; + result.mutable_tsdf_2d()->set_truncation_distance( + value_converter_->getMaxTSD()); + result.mutable_tsdf_2d()->set_max_weight(value_converter_->getMaxWeight()); + return result; +} + +std::unique_ptr TSDF2D::ComputeCroppedGrid() const { + Eigen::Array2i offset; + CellLimits cell_limits; + ComputeCroppedLimits(&offset, &cell_limits); + const double resolution = limits().resolution(); + const Eigen::Vector2d max = + limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); + std::unique_ptr cropped_grid = absl::make_unique( + MapLimits(resolution, max, cell_limits), value_converter_->getMaxTSD(), + value_converter_->getMaxWeight(), conversion_tables_); + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) continue; + cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset), + GetWeight(xy_index + offset)); + } + cropped_grid->FinishUpdate(); + return std::move(cropped_grid); +} + +bool TSDF2D::DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const { + Eigen::Array2i offset; + CellLimits cell_limits; + ComputeCroppedLimits(&offset, &cell_limits); + + std::string cells; + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) { + cells.push_back(0); // value + cells.push_back(0); // alpha + continue; + } + // 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. + float normalized_tsdf = std::abs(GetTSD(xy_index + offset)); + normalized_tsdf = + std::pow(normalized_tsdf / value_converter_->getMaxTSD(), 0.5f); + float normalized_weight = + GetWeight(xy_index + offset) / value_converter_->getMaxWeight(); + const int delta = static_cast( + std::round(normalized_weight * (normalized_tsdf * 255. - 128.))); + 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); + } + + common::FastGzipString(cells, texture->mutable_cells()); + texture->set_width(cell_limits.num_x_cells); + texture->set_height(cell_limits.num_y_cells); + const double resolution = limits().resolution(); + texture->set_resolution(resolution); + const double max_x = limits().max().x() - resolution * offset.y(); + const double max_y = limits().max().y() - resolution * offset.x(); + *texture->mutable_slice_pose() = transform::ToProto( + local_pose.inverse() * + transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); + + return true; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d.h new file mode 100644 index 0000000..16134f6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d.h @@ -0,0 +1,64 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_ + +#include + +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" + +namespace cartographer { +namespace mapping { + +// Represents a 2D grid of truncated signed distances and weights. +class TSDF2D : public Grid2D { + public: + TSDF2D(const MapLimits& limits, float truncation_distance, float max_weight, + ValueConversionTables* conversion_tables); + explicit TSDF2D(const proto::Grid2D& proto, + ValueConversionTables* conversion_tables); + + void SetCell(const Eigen::Array2i& cell_index, const float tsd, + const float weight); + GridType GetGridType() const override; + float GetTSD(const Eigen::Array2i& cell_index) const; + float GetWeight(const Eigen::Array2i& cell_index) const; + std::pair GetTSDAndWeight( + const Eigen::Array2i& cell_index) const; + + void GrowLimits(const Eigen::Vector2f& point) override; + proto::Grid2D ToProto() const override; + std::unique_ptr ComputeCroppedGrid() const override; + bool DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const override; + bool CellIsUpdated(const Eigen::Array2i& cell_index) const; + + private: + ValueConversionTables* conversion_tables_; + std::unique_ptr value_converter_; + std::vector weight_cells_; // Highest bit is update marker. +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d_test.cc new file mode 100644 index 0000000..2ea899e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_2d_test.cc @@ -0,0 +1,169 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/tsdf_2d.h" + +#include + +#include "cartographer/mapping/value_conversion_tables.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using Eigen::Array2i; +using Eigen::Vector2f; + +TEST(TSDF2DTest, ProtoConstructor) { + proto::Grid2D proto; + const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); + *proto.mutable_limits() = ToProto(limits); + proto.mutable_tsdf_2d()->set_max_weight(10.0); + proto.mutable_tsdf_2d()->set_truncation_distance(1.0); + for (int i = 6; i < 12; ++i) { + proto.mutable_cells()->Add(static_cast(i)); + proto.mutable_tsdf_2d()->mutable_weight_cells()->Add( + static_cast(i)); + } + proto.mutable_known_cells_box()->set_max_x(19); + proto.mutable_known_cells_box()->set_max_y(20); + proto.mutable_known_cells_box()->set_min_x(21); + proto.mutable_known_cells_box()->set_min_y(22); + proto.set_max_correspondence_cost(1.0); + proto.set_min_correspondence_cost(-1.0); + + ValueConversionTables conversion_tables; + TSDF2D grid(proto, &conversion_tables); + EXPECT_EQ(grid.GetGridType(), GridType::TSDF); + EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); +} + +TEST(TSDF2DTest, ConstructorGridType) { + ValueConversionTables conversion_tables; + TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, + 10.0f, &conversion_tables); + EXPECT_EQ(tsdf.GetGridType(), GridType::TSDF); +} + +TEST(TSDF2DTest, ToProto) { + ValueConversionTables conversion_tables; + TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, + 10.0f, &conversion_tables); + + const auto proto = tsdf.ToProto(); + EXPECT_EQ(ToProto(tsdf.limits()).DebugString(), proto.limits().DebugString()); +} + +TEST(TSDF2DTest, GetCellIndex) { + ValueConversionTables conversion_tables; + TSDF2D tsdf(MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)), 1.f, + 10.f, &conversion_tables); + + const MapLimits& limits = tsdf.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( + (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all()); + EXPECT_TRUE( + (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all()); + + // Check around the origin. + EXPECT_TRUE( + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all()); +} + +TEST(TSDF2DTest, WriteRead) { + const float truncation_distance = 1.f; + const float max_weight = 10.f; + ValueConversionTables conversion_tables; + TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)), + truncation_distance, max_weight, &conversion_tables); + + const MapLimits& limits = tsdf.limits(); + EXPECT_EQ(1., limits.max().x()); + EXPECT_EQ(2., 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); + + std::mt19937 rng(42); + std::uniform_real_distribution tsd_distribution(-truncation_distance, + truncation_distance); + std::uniform_real_distribution weight_distribution(0.f, max_weight); + for (size_t i = 0; i < 1; ++i) { + const float tsd = tsd_distribution(rng); + const float weight = weight_distribution(rng); + tsdf.SetCell(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), tsd, weight); + EXPECT_NEAR( + tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).first, + tsd, 2.f * truncation_distance / 32768.f); + EXPECT_NEAR( + tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).second, + weight, max_weight / 32768.f); + EXPECT_NEAR(tsdf.GetTSD(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), tsd, + 2.f * truncation_distance / 32768.f); + EXPECT_NEAR(tsdf.GetWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), + weight, max_weight / 32768.f); + } + for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)), + limits.GetCellIndex(Vector2f(0.5f, 0.5f)), + limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) { + EXPECT_TRUE(limits.Contains(xy_index)); + EXPECT_FALSE(tsdf.IsKnown(xy_index)); + } +} + +TEST(TSDF2DTest, CorrectCropping) { + // Create a TSDF with random values. + const float truncation_distance = 1.f; + const float max_weight = 10.f; + std::mt19937 rng(42); + std::uniform_real_distribution tsdf_distribution(-truncation_distance, + truncation_distance); + std::uniform_real_distribution weight_distribution(0.f, max_weight); + ValueConversionTables conversion_tables; + TSDF2D tsdf(MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)), + truncation_distance, max_weight, &conversion_tables); + for (const Array2i& xy_index : + XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) { + tsdf.SetCell(xy_index, tsdf_distribution(rng), weight_distribution(rng)); + } + Array2i offset; + CellLimits limits; + tsdf.ComputeCroppedLimits(&offset, &limits); + EXPECT_TRUE((offset == Array2i(100, 100)).all()); + EXPECT_EQ(limits.num_x_cells, 200); + EXPECT_EQ(limits.num_y_cells, 200); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc new file mode 100644 index 0000000..7e94b13 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc @@ -0,0 +1,278 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" + +#include "cartographer/mapping/internal/2d/normal_estimation_2d.h" +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" + +namespace cartographer { +namespace mapping { +namespace { + +// Factor for subpixel accuracy of start and end point for ray casts. +constexpr int kSubpixelScale = 1000; +// Minimum distance between range observation and origin. Otherwise, range +// observations are discarded. +constexpr float kMinRangeMeters = 1e-6f; +const float kSqrtTwoPi = std::sqrt(2.0 * M_PI); + +void GrowAsNeeded(const sensor::RangeData& range_data, + const float truncation_distance, TSDF2D* const tsdf) { + Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); + for (const sensor::RangefinderPoint& hit : range_data.returns) { + const Eigen::Vector3f direction = + (hit.position - range_data.origin).normalized(); + const Eigen::Vector3f end_position = + hit.position + truncation_distance * direction; + bounding_box.extend(end_position.head<2>()); + } + // Padding around bounding box to avoid numerical issues at cell boundaries. + constexpr float kPadding = 1e-6f; + tsdf->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones()); + tsdf->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones()); +} + +float GaussianKernel(const float x, const float sigma) { + return 1.0 / (kSqrtTwoPi * sigma) * std::exp(-0.5 * x * x / (sigma * sigma)); +} + +std::pair SuperscaleRay( + const Eigen::Vector2f& begin, const Eigen::Vector2f& end, + TSDF2D* const tsdf) { + const MapLimits& limits = tsdf->limits(); + const double superscaled_resolution = limits.resolution() / kSubpixelScale; + const MapLimits superscaled_limits( + superscaled_resolution, limits.max(), + CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, + limits.cell_limits().num_y_cells * kSubpixelScale)); + + const Eigen::Array2i superscaled_begin = + superscaled_limits.GetCellIndex(begin); + const Eigen::Array2i superscaled_end = superscaled_limits.GetCellIndex(end); + return std::make_pair(superscaled_begin, superscaled_end); +} + +struct RangeDataSorter { + RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); } + bool operator()(const sensor::RangefinderPoint& lhs, + const sensor::RangefinderPoint& rhs) { + const Eigen::Vector2f delta_lhs = + (lhs.position.head<2>() - origin_).normalized(); + const Eigen::Vector2f delta_rhs = + (rhs.position.head<2>() - origin_).normalized(); + if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) { + return delta_lhs[1] < 0.f; + } else if (delta_lhs[1] < 0.f) { + return delta_lhs[0] < delta_rhs[0]; + } else { + return delta_lhs[0] > delta_rhs[0]; + } + } + + private: + Eigen::Vector2f origin_; +}; + +float ComputeRangeWeightFactor(float range, int exponent) { + float weight = 0.f; + if (std::abs(range) > kMinRangeMeters) { + weight = 1.f / (std::pow(range, exponent)); + } + return weight; +} +} // namespace + +proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::TSDFRangeDataInserterOptions2D options; + options.set_truncation_distance( + parameter_dictionary->GetDouble("truncation_distance")); + options.set_maximum_weight(parameter_dictionary->GetDouble("maximum_weight")); + options.set_update_free_space( + parameter_dictionary->GetBool("update_free_space")); + *options + .mutable_normal_estimation_options() = CreateNormalEstimationOptions2D( + parameter_dictionary->GetDictionary("normal_estimation_options").get()); + options.set_project_sdf_distance_to_scan_normal( + parameter_dictionary->GetBool("project_sdf_distance_to_scan_normal")); + options.set_update_weight_range_exponent( + parameter_dictionary->GetInt("update_weight_range_exponent")); + options.set_update_weight_angle_scan_normal_to_ray_kernel_bandwidth( + parameter_dictionary->GetDouble( + "update_weight_angle_scan_normal_to_ray_kernel_bandwidth")); + options.set_update_weight_distance_cell_to_hit_kernel_bandwidth( + parameter_dictionary->GetDouble( + "update_weight_distance_cell_to_hit_kernel_bandwidth")); + return options; +} + +TSDFRangeDataInserter2D::TSDFRangeDataInserter2D( + const proto::TSDFRangeDataInserterOptions2D& options) + : options_(options) {} + +// Casts a ray from origin towards hit for each hit in range data. +// If 'options.update_free_space' is 'true', all cells along the ray +// until 'truncation_distance' behind hit are updated. Otherwise, only the cells +// within 'truncation_distance' around hit are updated. +void TSDFRangeDataInserter2D::Insert_new(const sensor::RangeData& range_data, + GridInterface* grid, + const transform::Rigid3d local_pose_) const { + const float truncation_distance = + static_cast(options_.truncation_distance()); + TSDF2D* tsdf = static_cast(grid); + GrowAsNeeded(range_data, truncation_distance, tsdf); + + // Compute normals if needed. + bool scale_update_weight_angle_scan_normal_to_ray = + options_.update_weight_angle_scan_normal_to_ray_kernel_bandwidth() != 0.f; + sensor::RangeData sorted_range_data = range_data; + std::vector normals; + if (options_.project_sdf_distance_to_scan_normal() || + scale_update_weight_angle_scan_normal_to_ray) { + std::vector returns = + sorted_range_data.returns.points(); + std::sort(returns.begin(), returns.end(), + RangeDataSorter(sorted_range_data.origin)); + sorted_range_data.returns = sensor::PointCloud(std::move(returns)); + normals = EstimateNormals(sorted_range_data, + options_.normal_estimation_options()); + } + + const Eigen::Vector2f origin = sorted_range_data.origin.head<2>(); + for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size(); + ++hit_index) { + const Eigen::Vector2f hit = + sorted_range_data.returns[hit_index].position.head<2>(); + const float normal = normals.empty() + ? std::numeric_limits::quiet_NaN() + : normals[hit_index]; + InsertHit(options_, hit, origin, normal, tsdf); + } + tsdf->FinishUpdate(); +} +void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data, + GridInterface* grid) const { + const float truncation_distance = + static_cast(options_.truncation_distance()); + TSDF2D* tsdf = static_cast(grid); + GrowAsNeeded(range_data, truncation_distance, tsdf); + + // Compute normals if needed. + bool scale_update_weight_angle_scan_normal_to_ray = + options_.update_weight_angle_scan_normal_to_ray_kernel_bandwidth() != 0.f; + sensor::RangeData sorted_range_data = range_data; + std::vector normals; + if (options_.project_sdf_distance_to_scan_normal() || + scale_update_weight_angle_scan_normal_to_ray) { + std::vector returns = + sorted_range_data.returns.points(); + std::sort(returns.begin(), returns.end(), + RangeDataSorter(sorted_range_data.origin)); + sorted_range_data.returns = sensor::PointCloud(std::move(returns)); + normals = EstimateNormals(sorted_range_data, + options_.normal_estimation_options()); + } + + const Eigen::Vector2f origin = sorted_range_data.origin.head<2>(); + for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size(); + ++hit_index) { + const Eigen::Vector2f hit = + sorted_range_data.returns[hit_index].position.head<2>(); + const float normal = normals.empty() + ? std::numeric_limits::quiet_NaN() + : normals[hit_index]; + InsertHit(options_, hit, origin, normal, tsdf); + } + tsdf->FinishUpdate(); +} + +void TSDFRangeDataInserter2D::InsertHit( + const proto::TSDFRangeDataInserterOptions2D& options, + const Eigen::Vector2f& hit, const Eigen::Vector2f& origin, float normal, + TSDF2D* tsdf) const { + const Eigen::Vector2f ray = hit - origin; + const float range = ray.norm(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + if (range < truncation_distance) return; + const float truncation_ratio = truncation_distance / range; + const Eigen::Vector2f ray_begin = + options_.update_free_space() ? origin + : origin + (1.0f - truncation_ratio) * ray; + const Eigen::Vector2f ray_end = origin + (1.0f + truncation_ratio) * ray; + std::pair superscaled_ray = + SuperscaleRay(ray_begin, ray_end, tsdf); + std::vector ray_mask = RayToPixelMask( + superscaled_ray.first, superscaled_ray.second, kSubpixelScale); + + // Precompute weight factors. + float weight_factor_angle_ray_normal = 1.f; + if (options_.update_weight_angle_scan_normal_to_ray_kernel_bandwidth() != + 0.f) { + const Eigen::Vector2f negative_ray = -ray; + float angle_ray_normal = + common::NormalizeAngleDifference(normal - common::atan2(negative_ray)); + weight_factor_angle_ray_normal = GaussianKernel( + angle_ray_normal, + options_.update_weight_angle_scan_normal_to_ray_kernel_bandwidth()); + } + float weight_factor_range = 1.f; + if (options_.update_weight_range_exponent() != 0) { + weight_factor_range = ComputeRangeWeightFactor( + range, options_.update_weight_range_exponent()); + } + + // Update Cells. + for (const Eigen::Array2i& cell_index : ray_mask) { + if (tsdf->CellIsUpdated(cell_index)) continue; + Eigen::Vector2f cell_center = tsdf->limits().GetCellCenter(cell_index); + float distance_cell_to_origin = (cell_center - origin).norm(); + float update_tsd = range - distance_cell_to_origin; + if (options_.project_sdf_distance_to_scan_normal()) { + float normal_orientation = normal; + update_tsd = (cell_center - hit) + .dot(Eigen::Vector2f{std::cos(normal_orientation), + std::sin(normal_orientation)}); + } + update_tsd = + common::Clamp(update_tsd, -truncation_distance, truncation_distance); + float update_weight = weight_factor_range * weight_factor_angle_ray_normal; + if (options_.update_weight_distance_cell_to_hit_kernel_bandwidth() != 0.f) { + update_weight *= GaussianKernel( + update_tsd, + options_.update_weight_distance_cell_to_hit_kernel_bandwidth()); + } + UpdateCell(cell_index, update_tsd, update_weight, tsdf); + } +} + +void TSDFRangeDataInserter2D::UpdateCell(const Eigen::Array2i& cell, + float update_sdf, float update_weight, + TSDF2D* tsdf) const { + if (update_weight == 0.f) return; + const std::pair tsd_and_weight = tsdf->GetTSDAndWeight(cell); + float updated_weight = tsd_and_weight.second + update_weight; + float updated_sdf = (tsd_and_weight.first * tsd_and_weight.second + + update_sdf * update_weight) / + updated_weight; + updated_weight = + std::min(updated_weight, static_cast(options_.maximum_weight())); + tsdf->SetCell(cell, updated_sdf, updated_weight); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h new file mode 100644 index 0000000..cb132c0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h @@ -0,0 +1,63 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/range_data_inserter_interface.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +class TSDFRangeDataInserter2D : public RangeDataInserterInterface { + public: + explicit TSDFRangeDataInserter2D( + const proto::TSDFRangeDataInserterOptions2D& options); + + TSDFRangeDataInserter2D(const TSDFRangeDataInserter2D&) = delete; + TSDFRangeDataInserter2D& operator=(const TSDFRangeDataInserter2D&) = delete; + + // Casts a ray from origin towards hit for each hit in range data. + // If 'options.update_free_space' is 'true', all cells along the ray + // until 'truncation_distance' behind hit are updated. Otherwise, only the + // cells within 'truncation_distance' around hit are updated. + virtual void Insert(const sensor::RangeData& range_data, + GridInterface* grid) const override; + virtual void Insert_new(const sensor::RangeData& range_data, + GridInterface* grid, + const transform::Rigid3d local_pose_) const override; + + private: + void InsertHit(const proto::TSDFRangeDataInserterOptions2D& options, + const Eigen::Vector2f& hit, const Eigen::Vector2f& origin, + float normal, TSDF2D* tsdf) const; + void UpdateCell(const Eigen::Array2i& cell, float update_sdf, + float update_weight, TSDF2D* tsdf) const; + const proto::TSDFRangeDataInserterOptions2D options_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc new file mode 100644 index 0000000..0850e6e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc @@ -0,0 +1,349 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +class RangeDataInserterTest2DTSDF : public ::testing::Test { + protected: + RangeDataInserterTest2DTSDF() + : tsdf_(MapLimits(1., Eigen::Vector2d(1., 7.), CellLimits(8, 1)), 2.0, + 10.0, &conversion_tables_) { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "truncation_distance = 2.0," + "maximum_weight = 10.," + "update_free_space = false," + "normal_estimation_options = {" + "num_normal_samples = 2," + "sample_radius = 10.," + "}," + "project_sdf_distance_to_scan_normal = false," + "update_weight_range_exponent = 0," + "update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0," + "update_weight_distance_cell_to_hit_kernel_bandwidth = 0," + "}"); + options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()); + range_data_inserter_ = absl::make_unique(options_); + } + + void InsertPoint() { + auto range_data = sensor::RangeData(); + range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + } + + ValueConversionTables conversion_tables_; + proto::TSDFRangeDataInserterOptions2D options_; + TSDF2D tsdf_; + std::unique_ptr range_data_inserter_; +}; + +class MockCellProperties { + public: + MockCellProperties(const Eigen::Array2i& cell_index, const TSDF2D& tsdf) + : is_known_(tsdf.IsKnown(cell_index)), + tsd_(tsdf.GetTSD(cell_index)), + weight_(tsdf.GetWeight(cell_index)){}; + + bool is_known_; + float tsd_; + float weight_; +}; + +::std::ostream& operator<<(::std::ostream& os, const MockCellProperties& bar) { + return os << std::to_string(bar.is_known_) + "\t" + std::to_string(bar.tsd_) + + "\t" + std::to_string(bar.weight_); +} + +MATCHER_P3(EqualCellProperties, expected_is_known, expected_tsd, + expected_weight, + std::string("Expected ") + std::to_string(expected_is_known) + "\t" + + std::to_string(expected_tsd) + "\t" + + std::to_string(expected_weight)) { + bool result = expected_is_known == arg.is_known_; + result = result && (std::abs(expected_tsd - arg.tsd_) < 1e-4); + result = result && (std::abs(expected_weight - arg.weight_) < 1e-2); + return result; +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPoint) { + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + const float maximum_weight = static_cast(options_.maximum_weight()); + + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + // Cells don't intersect with ray. + x = 0.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + expected_tsdf = -truncation_distance; + expected_weight = 0.; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + x = 1.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + } + + // Cells don't intersect with ray. + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.5, 6.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + for (int i = 0; i < 1000; ++i) { + InsertPoint(); + } + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, maximum_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointWithFreeSpaceUpdate) { + options_.set_update_free_space(true); + range_data_inserter_ = absl::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + const float maximum_weight = static_cast(options_.maximum_weight()); + + for (float y = -0.5; y < 6.; ++y) { + // Cells intersect with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + // Cells don't intersect with ray. + x = 0.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + expected_tsdf = -truncation_distance; + expected_weight = 0.; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + x = 1.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + } + + // Cells don't intersect with the ray. + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, 6.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + for (int i = 0; i < 1000; ++i) { + InsertPoint(); + } + for (float y = -0.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, maximum_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointLinearWeight) { + options_.set_update_weight_range_exponent(1); + range_data_inserter_ = absl::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f / 4.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) { + options_.set_update_weight_range_exponent(2); + range_data_inserter_ = absl::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f / std::pow(4.f, 2); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, + InsertSmallAnglePointWithoutNormalProjection) { + auto range_data = sensor::RangeData(); + range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{10.5f, 3.5f, 0.f}}); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + float x = 4.5f; + float y = 2.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + Eigen::Vector2f ray = + Eigen::Vector2f(-0.5f, -0.5f) - Eigen::Vector2f(5.5f, 3.5f); + float ray_length = ray.norm(); + Eigen::Vector2f origin_to_cell = + Eigen::Vector2f(x, y) - Eigen::Vector2f(-0.5f, -0.5f); + float distance_origin_to_cell = origin_to_cell.norm(); + float expected_tsdf = ray_length - distance_origin_to_cell; + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) { + options_.set_project_sdf_distance_to_scan_normal(true); + range_data_inserter_ = absl::make_unique(options_); + auto range_data = sensor::RangeData(); + range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}}); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + float x = 4.5f; + float y = 2.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = 1.f; + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + x = 6.5f; + y = 4.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + expected_tsdf = -1.f; + expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); +} + +TEST_F(RangeDataInserterTest2DTSDF, + InsertPointsWithAngleScanNormalToRayWeight) { + float bandwidth = 10.f; + options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwidth( + bandwidth); + range_data_inserter_ = absl::make_unique(options_); + auto range_data = sensor::RangeData(); + range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}}); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + float x = -0.5f; + float y = 3.5f; + // Ray is perpendicular to surface. + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwidth); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); + x = 6.5f; + y = 4.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); + // Ray is inclined relative to surface. + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float angle = std::atan(7.f / 5.f); + expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwidth) * + std::exp(angle * angle / (2 * std::pow(bandwidth, 2))); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); + x = 6.5f; + y = 4.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) { + float bandwidth = 10.f; + options_.set_update_weight_distance_cell_to_hit_kernel_bandwidth(bandwidth); + range_data_inserter_ = absl::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + for (float y = 1.5; y < 6.; ++y) { + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = + 1.f / (std::sqrt(2 * M_PI) * bandwidth) * + std::exp(std::pow(expected_tsdf, 2) / (2 * std::pow(bandwidth, 2))); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + } +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/imu_integration.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/imu_integration.h new file mode 100644 index 0000000..b89875c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/imu_integration.h @@ -0,0 +1,163 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_IMU_INTEGRATION_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_IMU_INTEGRATION_H_ + +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/time.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +template +struct IntegrateImuResult { + Eigen::Matrix delta_velocity; + Eigen::Matrix delta_translation; + Eigen::Quaternion delta_rotation; +}; + +template +IntegrateImuResult IntegrateImu( + const RangeType& imu_data, + const Eigen::Transform& + linear_acceleration_calibration, + const Eigen::Transform& angular_velocity_calibration, + const common::Time start_time, const common::Time end_time, + IteratorType* const it) { + CHECK_LE(start_time, end_time); + CHECK(*it != imu_data.end()); + CHECK_LE((*it)->time, start_time); + if (std::next(*it) != imu_data.end()) { + CHECK_GT(std::next(*it)->time, start_time); + } + + common::Time current_time = start_time; + + IntegrateImuResult result = {Eigen::Matrix::Zero(), + Eigen::Matrix::Zero(), + Eigen::Quaterniond::Identity().cast()}; + while (current_time < end_time) { + common::Time next_imu_data = common::Time::max(); + if (std::next(*it) != imu_data.end()) { + next_imu_data = std::next(*it)->time; + } + common::Time next_time = std::min(next_imu_data, end_time); + const T delta_t(common::ToSeconds(next_time - current_time)); + + const Eigen::Matrix delta_angle = + (angular_velocity_calibration * + (*it)->angular_velocity.template cast()) * + delta_t; + result.delta_rotation *= + transform::AngleAxisVectorToRotationQuaternion(delta_angle); + result.delta_velocity += result.delta_rotation * + ((linear_acceleration_calibration * + (*it)->linear_acceleration.template cast()) * + delta_t); + result.delta_translation += delta_t * result.delta_velocity; + current_time = next_time; + if (current_time == next_imu_data) { + ++(*it); + } + } + return result; +} + +// Returns velocity delta in map frame. +template +IntegrateImuResult IntegrateImu(const RangeType& imu_data, + const common::Time start_time, + const common::Time end_time, + IteratorType* const it) { + return IntegrateImu( + imu_data, Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(), + start_time, end_time, it); +} + +template +struct ExtrapolatePoseResult { + transform::Rigid3 pose; + Eigen::Matrix velocity; +}; + +// Returns pose and linear velocity at 'time' which is equal to +// 'prev_from_tracking' extrapolated using IMU data. +template +ExtrapolatePoseResult ExtrapolatePoseWithImu( + const transform::Rigid3& prev_from_tracking, + const Eigen::Matrix& prev_velocity_in_tracking, + const common::Time prev_time, const Eigen::Matrix& gravity, + const common::Time time, const RangeType& imu_data, + IteratorType* const imu_it) { + const IntegrateImuResult result = + IntegrateImu(imu_data, Eigen::Transform::Identity(), + Eigen::Transform::Identity(), prev_time, + time, imu_it); + + const T delta_t = static_cast(common::ToSeconds(time - prev_time)); + const Eigen::Matrix translation = + prev_from_tracking.translation() + + prev_from_tracking.rotation() * + (delta_t * prev_velocity_in_tracking + result.delta_translation) - + static_cast(.5) * delta_t * delta_t * gravity; + const Eigen::Quaternion rotation = + prev_from_tracking.rotation() * result.delta_rotation; + + const Eigen::Matrix velocity = + prev_from_tracking.rotation() * + (prev_velocity_in_tracking + result.delta_velocity) - + delta_t * gravity; + return ExtrapolatePoseResult{transform::Rigid3(translation, rotation), + velocity}; +} + +// Same as above but given the last two poses. +template +ExtrapolatePoseResult ExtrapolatePoseWithImu( + const transform::Rigid3& prev_from_tracking, + const common::Time prev_time, + const transform::Rigid3& prev_prev_from_tracking, + const common::Time prev_prev_time, const Eigen::Matrix& gravity, + const common::Time time, const RangeType& imu_data, + IteratorType* const imu_it) { + // TODO(danielsievers): Really we should integrate velocities starting from + // the midpoint in between two poses, since this is how we fit them to poses + // in the optimization. + const T prev_delta_t = + static_cast(common::ToSeconds(prev_time - prev_prev_time)); + const Eigen::Matrix prev_velocity_in_tracking = + prev_from_tracking.inverse().rotation() * + (prev_from_tracking.translation() - + prev_prev_from_tracking.translation()) / + prev_delta_t; + + return ExtrapolatePoseWithImu(prev_from_tracking, prev_velocity_in_tracking, + prev_time, gravity, time, imu_data, imu_it); +} + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_IMU_INTEGRATION_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_slam_result_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_slam_result_3d.cc new file mode 100644 index 0000000..6f8e703 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_slam_result_3d.cc @@ -0,0 +1,46 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/local_slam_result_3d.h" + +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" + +namespace cartographer { +namespace mapping { + +void LocalSlamResult3D::AddToTrajectoryBuilder( + TrajectoryBuilderInterface* const trajectory_builder) { + trajectory_builder->AddLocalSlamResultData( + absl::make_unique(*this)); +} + +void LocalSlamResult3D::AddToPoseGraph(int trajectory_id, + PoseGraph* pose_graph) const { + DCHECK(dynamic_cast(pose_graph)); + CHECK_GE(local_slam_result_data_.submaps().size(), 1); + CHECK(local_slam_result_data_.submaps(0).has_submap_3d()); + std::vector> submaps; + for (const auto& submap_proto : local_slam_result_data_.submaps()) { + submaps.push_back(submap_controller_->UpdateSubmap(submap_proto)); + } + static_cast(pose_graph) + ->AddNode(std::make_shared( + mapping::FromProto(local_slam_result_data_.node_data())), + trajectory_id, submaps); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_slam_result_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_slam_result_3d.h new file mode 100644 index 0000000..88e94ff --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_slam_result_3d.h @@ -0,0 +1,52 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ + +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/internal/submap_controller.h" +#include "cartographer/mapping/trajectory_builder_interface.h" + +namespace cartographer { +namespace mapping { + +class LocalSlamResult3D : public LocalSlamResultData { + public: + LocalSlamResult3D( + const std::string& sensor_id, + const mapping::proto::LocalSlamResultData local_slam_result_data, + SubmapController* submap_controller) + : LocalSlamResultData(sensor_id, common::FromUniversal( + local_slam_result_data.timestamp())), + sensor_id_(sensor_id), + local_slam_result_data_(local_slam_result_data), + submap_controller_(submap_controller) {} + + void AddToTrajectoryBuilder( + TrajectoryBuilderInterface* const trajectory_builder) override; + void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override; + + private: + const std::string sensor_id_; + const mapping::proto::LocalSlamResultData local_slam_result_data_; + SubmapController* submap_controller_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc new file mode 100644 index 0000000..b2a4f86 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -0,0 +1,479 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/local_trajectory_builder_3d.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/proto/submaps_options_3d.pb.h" +#include "cartographer/transform/timestamped_transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +// TODO(spielawa): Adjust metrics for multi-trajectory. So far we assume a +// single trajectory. +static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null(); +static auto* kLocalSlamVoxelFilterFraction = metrics::Gauge::Null(); +static auto* kLocalSlamScanMatcherFraction = metrics::Gauge::Null(); +static auto* kLocalSlamInsertIntoSubmapFraction = metrics::Gauge::Null(); +static auto* kLocalSlamRealTimeRatio = metrics::Gauge::Null(); +static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null(); +static auto* kRealTimeCorrelativeScanMatcherScoreMetric = + metrics::Histogram::Null(); +static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null(); + +LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D( + const mapping::proto::LocalTrajectoryBuilderOptions3D& options, + const std::vector& expected_range_sensor_ids) + : options_(options), + active_submaps_(options.submaps_options()), + motion_filter_(options.motion_filter_options()), + real_time_correlative_scan_matcher_( + absl::make_unique( + options_.real_time_correlative_scan_matcher_options())), + ceres_scan_matcher_(absl::make_unique( + options_.ceres_scan_matcher_options())), + range_data_collator_(expected_range_sensor_ids) {} + +LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {} + +std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( + const transform::Rigid3d& pose_prediction, + const sensor::PointCloud& low_resolution_point_cloud_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud_in_tracking) { + if (active_submaps_.submaps().empty()) { + return absl::make_unique(pose_prediction); + } + std::shared_ptr matching_submap = + active_submaps_.submaps().front(); + transform::Rigid3d initial_ceres_pose = + matching_submap->local_pose().inverse() * pose_prediction; + if (options_.use_online_correlative_scan_matching()) { + // We take a copy since we use 'initial_ceres_pose' as an output argument. + const transform::Rigid3d initial_pose = initial_ceres_pose; + const double score = real_time_correlative_scan_matcher_->Match( + initial_pose, high_resolution_point_cloud_in_tracking, + matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose); + kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); + } + + transform::Rigid3d pose_observation_in_submap; + ceres::Solver::Summary summary; + const auto* high_resolution_intensity_hybrid_grid = + options_.use_intensities() + ? &matching_submap->high_resolution_intensity_hybrid_grid() + : nullptr; + ceres_scan_matcher_->Match( + (matching_submap->local_pose().inverse() * pose_prediction).translation(), + initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking, + &matching_submap->high_resolution_hybrid_grid(), + high_resolution_intensity_hybrid_grid}, + {&low_resolution_point_cloud_in_tracking, + &matching_submap->low_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}}, + &pose_observation_in_submap, &summary); + kCeresScanMatcherCostMetric->Observe(summary.final_cost); + const double residual_distance = (pose_observation_in_submap.translation() - + initial_ceres_pose.translation()) + .norm(); + kScanMatcherResidualDistanceMetric->Observe(residual_distance); + const double residual_angle = + pose_observation_in_submap.rotation().angularDistance( + initial_ceres_pose.rotation()); + kScanMatcherResidualAngleMetric->Observe(residual_angle); + return absl::make_unique(matching_submap->local_pose() * + pose_observation_in_submap); +} + +void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { + if (extrapolator_ != nullptr) { + extrapolator_->AddImuData(imu_data); + return; + } + std::vector initial_poses; + for (const auto& pose_proto : options_.initial_poses()) { + initial_poses.push_back(transform::FromProto(pose_proto)); + } + std::vector initial_imu_data; + for (const auto& imu : options_.initial_imu_data()) { + initial_imu_data.push_back(sensor::FromProto(imu)); + } + initial_imu_data.push_back(imu_data); + extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( + options_.pose_extrapolator_options(), initial_imu_data, initial_poses); +} + +std::unique_ptr +LocalTrajectoryBuilder3D::AddRangeData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& unsynchronized_data) { + if (options_.use_intensities()) { + CHECK_EQ(unsynchronized_data.ranges.size(), + unsynchronized_data.intensities.size()) + << "Passed point cloud has inconsistent number of intensities and " + "ranges."; + } + auto synchronized_data = + range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); + if (synchronized_data.ranges.empty()) { + LOG(INFO) << "Range data collator filling buffer."; + return nullptr; + } + + if (extrapolator_ == nullptr) { + // Until we've initialized the extrapolator with our first IMU message, we + // cannot compute the orientation of the rangefinder. + LOG(INFO) << "IMU not yet initialized."; + return nullptr; + } + + CHECK(!synchronized_data.ranges.empty()); + CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f); + const common::Time time_first_point = + synchronized_data.time + + common::FromSeconds(synchronized_data.ranges.front().point_time.time); + if (time_first_point < extrapolator_->GetLastPoseTime()) { + LOG(INFO) << "Extrapolator is still initializing."; + return nullptr; + } + + if (num_accumulated_ == 0) { + accumulated_point_cloud_origin_data_.clear(); + } + + synchronized_data.ranges = sensor::VoxelFilter( + synchronized_data.ranges, 0.5f * options_.voxel_filter_size()); + accumulated_point_cloud_origin_data_.emplace_back( + std::move(synchronized_data)); + ++num_accumulated_; + + if (num_accumulated_ < options_.num_accumulated_range_data()) { + return nullptr; + } + num_accumulated_ = 0; + + bool warned = false; + std::vector hit_times; + common::Time prev_time_point = extrapolator_->GetLastExtrapolatedTime(); + for (const auto& point_cloud_origin_data : + accumulated_point_cloud_origin_data_) { + for (const auto& hit : point_cloud_origin_data.ranges) { + common::Time time_point = point_cloud_origin_data.time + + common::FromSeconds(hit.point_time.time); + if (time_point < prev_time_point) { + if (!warned) { + LOG(ERROR) << "Timestamp of individual range data point jumps " + "backwards from " + << prev_time_point << " to " << time_point; + warned = true; + } + time_point = prev_time_point; + } + + hit_times.push_back(time_point); + prev_time_point = time_point; + } + } + hit_times.push_back(accumulated_point_cloud_origin_data_.back().time); + + const PoseExtrapolatorInterface::ExtrapolationResult extrapolation_result = + extrapolator_->ExtrapolatePosesWithGravity(hit_times); + std::vector hits_poses( + std::move(extrapolation_result.previous_poses)); + hits_poses.push_back(extrapolation_result.current_pose.cast()); + CHECK_EQ(hits_poses.size(), hit_times.size()); + + const size_t max_possible_number_of_accumulated_points = hit_times.size(); + std::vector accumulated_points; + std::vector accumulated_intensities; + accumulated_points.reserve(max_possible_number_of_accumulated_points); + if (options_.use_intensities()) { + accumulated_intensities.reserve(max_possible_number_of_accumulated_points); + } + sensor::PointCloud misses; + std::vector::const_iterator hits_poses_it = + hits_poses.begin(); + for (const auto& point_cloud_origin_data : + accumulated_point_cloud_origin_data_) { + for (const auto& hit : point_cloud_origin_data.ranges) { + const Eigen::Vector3f hit_in_local = + *hits_poses_it * hit.point_time.position; + const Eigen::Vector3f origin_in_local = + *hits_poses_it * point_cloud_origin_data.origins.at(hit.origin_index); + const Eigen::Vector3f delta = hit_in_local - origin_in_local; + const float range = delta.norm(); + if (range >= options_.min_range()) { + if (range <= options_.max_range()) { + accumulated_points.push_back(sensor::RangefinderPoint{hit_in_local}); + if (options_.use_intensities()) { + accumulated_intensities.push_back(hit.intensity); + } + } else { + // We insert a ray cropped to 'max_range' as a miss for hits beyond + // the maximum range. This way the free space up to the maximum range + // will be updated. + // TODO(wohe): since `misses` are not used anywhere in 3D, consider + // removing `misses` from `range_data` and/or everywhere in 3D. + misses.push_back(sensor::RangefinderPoint{ + origin_in_local + options_.max_range() / range * delta}); + } + } + ++hits_poses_it; + } + } + CHECK(std::next(hits_poses_it) == hits_poses.end()); + const sensor::PointCloud returns(std::move(accumulated_points), + std::move(accumulated_intensities)); + + const common::Time current_sensor_time = synchronized_data.time; + absl::optional sensor_duration; + if (last_sensor_time_.has_value()) { + sensor_duration = current_sensor_time - last_sensor_time_.value(); + } + last_sensor_time_ = current_sensor_time; + + const common::Time current_time = hit_times.back(); + const auto voxel_filter_start = std::chrono::steady_clock::now(); + const sensor::RangeData filtered_range_data = { + extrapolation_result.current_pose.translation().cast(), + sensor::VoxelFilter(returns, options_.voxel_filter_size()), + sensor::VoxelFilter(misses, options_.voxel_filter_size())}; + const auto voxel_filter_stop = std::chrono::steady_clock::now(); + const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; + + if (sensor_duration.has_value()) { + const double voxel_filter_fraction = + common::ToSeconds(voxel_filter_duration) / + common::ToSeconds(sensor_duration.value()); + kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction); + } + + return AddAccumulatedRangeData( + current_time, + sensor::TransformRangeData( + filtered_range_data, + extrapolation_result.current_pose.inverse().cast()), + sensor_duration, extrapolation_result.current_pose, + extrapolation_result.gravity_from_tracking); +} + +std::unique_ptr +LocalTrajectoryBuilder3D::AddAccumulatedRangeData( + const common::Time time, + const sensor::RangeData& filtered_range_data_in_tracking, + const absl::optional& sensor_duration, + const transform::Rigid3d& pose_prediction, + const Eigen::Quaterniond& gravity_alignment) { + if (filtered_range_data_in_tracking.returns.empty()) { + LOG(WARNING) << "Dropped empty range data."; + return nullptr; + } + + const auto scan_matcher_start = std::chrono::steady_clock::now(); + + const sensor::PointCloud high_resolution_point_cloud_in_tracking = + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.high_resolution_adaptive_voxel_filter_options()); + if (high_resolution_point_cloud_in_tracking.empty()) { + LOG(WARNING) << "Dropped empty high resolution point cloud data."; + return nullptr; + } + const sensor::PointCloud low_resolution_point_cloud_in_tracking = + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.low_resolution_adaptive_voxel_filter_options()); + if (low_resolution_point_cloud_in_tracking.empty()) { + LOG(WARNING) << "Dropped empty low resolution point cloud data."; + return nullptr; + } + + std::unique_ptr pose_estimate = + ScanMatch(pose_prediction, low_resolution_point_cloud_in_tracking, + high_resolution_point_cloud_in_tracking); + if (pose_estimate == nullptr) { + LOG(WARNING) << "Scan matching failed."; + return nullptr; + } + extrapolator_->AddPose(time, *pose_estimate); + + const auto scan_matcher_stop = std::chrono::steady_clock::now(); + const auto scan_matcher_duration = scan_matcher_stop - scan_matcher_start; + if (sensor_duration.has_value()) { + const double scan_matcher_fraction = + common::ToSeconds(scan_matcher_duration) / + common::ToSeconds(sensor_duration.value()); + kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction); + } + + sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData( + filtered_range_data_in_tracking, pose_estimate->cast()); + + const auto insert_into_submap_start = std::chrono::steady_clock::now(); + std::unique_ptr insertion_result = InsertIntoSubmap( + time, filtered_range_data_in_local, filtered_range_data_in_tracking, + high_resolution_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, *pose_estimate, + gravity_alignment); + const auto insert_into_submap_stop = std::chrono::steady_clock::now(); + + const auto insert_into_submap_duration = + insert_into_submap_stop - insert_into_submap_start; + if (sensor_duration.has_value()) { + const double insert_into_submap_fraction = + common::ToSeconds(insert_into_submap_duration) / + common::ToSeconds(sensor_duration.value()); + kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction); + } + const auto wall_time = std::chrono::steady_clock::now(); + if (last_wall_time_.has_value()) { + const auto wall_time_duration = wall_time - last_wall_time_.value(); + kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration)); + if (sensor_duration.has_value()) { + kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) / + common::ToSeconds(wall_time_duration)); + } + } + const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds(); + if (last_thread_cpu_time_seconds_.has_value()) { + const double thread_cpu_duration_seconds = + thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value(); + if (sensor_duration.has_value()) { + kLocalSlamCpuRealTimeRatio->Set( + common::ToSeconds(sensor_duration.value()) / + thread_cpu_duration_seconds); + } + } + last_wall_time_ = wall_time; + last_thread_cpu_time_seconds_ = thread_cpu_time_seconds; + return absl::make_unique(MatchingResult{ + time, *pose_estimate, std::move(filtered_range_data_in_local), + std::move(insertion_result)}); +} + +void LocalTrajectoryBuilder3D::AddOdometryData( + const sensor::OdometryData& odometry_data) { + if (extrapolator_ == nullptr) { + // Until we've initialized the extrapolator we cannot add odometry data. + LOG(INFO) << "Extrapolator not yet initialized."; + return; + } + extrapolator_->AddOdometryData(odometry_data); +} + +std::unique_ptr +LocalTrajectoryBuilder3D::InsertIntoSubmap( + const common::Time time, + const sensor::RangeData& filtered_range_data_in_local, + const sensor::RangeData& filtered_range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud_in_tracking, + const sensor::PointCloud& low_resolution_point_cloud_in_tracking, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment) { + if (motion_filter_.IsSimilar(time, pose_estimate)) { + return nullptr; + } + const Eigen::VectorXf rotational_scan_matcher_histogram_in_gravity = + scan_matching::RotationalScanMatcher::ComputeHistogram( + sensor::TransformPointCloud( + filtered_range_data_in_tracking.returns, + transform::Rigid3f::Rotation(gravity_alignment.cast())), + options_.rotational_histogram_size()); + + const Eigen::Quaterniond local_from_gravity_aligned = + pose_estimate.rotation() * gravity_alignment.inverse(); + std::vector> insertion_submaps = + active_submaps_.InsertData(filtered_range_data_in_local, + local_from_gravity_aligned, + rotational_scan_matcher_histogram_in_gravity); + return absl::make_unique( + InsertionResult{std::make_shared( + mapping::TrajectoryNode::Data{ + time, + gravity_alignment, + {}, // 'filtered_point_cloud' is only used in 2D. + high_resolution_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, + rotational_scan_matcher_histogram_in_gravity, + pose_estimate}), + std::move(insertion_submaps)}); +} + +void LocalTrajectoryBuilder3D::RegisterMetrics( + metrics::FamilyFactory* family_factory) { + auto* latency = family_factory->NewGaugeFamily( + "mapping_3d_local_trajectory_builder_latency", + "Duration from first incoming point cloud in accumulation to local slam " + "result"); + kLocalSlamLatencyMetric = latency->Add({}); + + auto* voxel_filter_fraction = family_factory->NewGaugeFamily( + "mapping_3d_local_trajectory_builder_voxel_filter_fraction", + "Fraction of total sensor time taken up by voxel filter."); + kLocalSlamVoxelFilterFraction = voxel_filter_fraction->Add({}); + + auto* scan_matcher_fraction = family_factory->NewGaugeFamily( + "mapping_3d_local_trajectory_builder_scan_matcher_fraction", + "Fraction of total sensor time taken up by scan matcher."); + kLocalSlamScanMatcherFraction = scan_matcher_fraction->Add({}); + + auto* insert_into_submap_fraction = family_factory->NewGaugeFamily( + "mapping_3d_local_trajectory_builder_insert_into_submap_fraction", + "Fraction of total sensor time taken up by inserting into submap."); + kLocalSlamInsertIntoSubmapFraction = insert_into_submap_fraction->Add({}); + + auto* real_time_ratio = family_factory->NewGaugeFamily( + "mapping_3d_local_trajectory_builder_real_time_ratio", + "sensor duration / wall clock duration."); + kLocalSlamRealTimeRatio = real_time_ratio->Add({}); + + auto* cpu_real_time_ratio = family_factory->NewGaugeFamily( + "mapping_3d_local_trajectory_builder_cpu_real_time_ratio", + "sensor duration / cpu duration."); + kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({}); + + auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = family_factory->NewHistogramFamily( + "mapping_3d_local_trajectory_builder_scores", "Local scan matcher scores", + score_boundaries); + kRealTimeCorrelativeScanMatcherScoreMetric = + scores->Add({{"scan_matcher", "real_time_correlative"}}); + auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100); + auto* costs = family_factory->NewHistogramFamily( + "mapping_3d_local_trajectory_builder_costs", "Local scan matcher costs", + cost_boundaries); + kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}}); + auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10); + auto* residuals = family_factory->NewHistogramFamily( + "mapping_3d_local_trajectory_builder_residuals", + "Local scan matcher residuals", distance_boundaries); + kScanMatcherResidualDistanceMetric = + residuals->Add({{"component", "distance"}}); + kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}}); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h new file mode 100644 index 0000000..f242c38 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -0,0 +1,126 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h" +#include "cartographer/mapping/internal/motion_filter.h" +#include "cartographer/mapping/internal/range_data_collator.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { + +// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) +// without loop closure. +class LocalTrajectoryBuilder3D { + public: + struct InsertionResult { + std::shared_ptr constant_data; + std::vector> insertion_submaps; + }; + struct MatchingResult { + common::Time time; + transform::Rigid3d local_pose; + sensor::RangeData range_data_in_local; + // 'nullptr' if dropped by the motion filter. + std::unique_ptr insertion_result; + }; + + explicit LocalTrajectoryBuilder3D( + const mapping::proto::LocalTrajectoryBuilderOptions3D& options, + const std::vector& expected_range_sensor_ids); + ~LocalTrajectoryBuilder3D(); + + LocalTrajectoryBuilder3D(const LocalTrajectoryBuilder3D&) = delete; + LocalTrajectoryBuilder3D& operator=(const LocalTrajectoryBuilder3D&) = delete; + + void AddImuData(const sensor::ImuData& imu_data); + // Returns 'MatchingResult' when range data accumulation completed, + // otherwise 'nullptr'. `TimedPointCloudData::time` is when the last point in + // `range_data` was acquired, `TimedPointCloudData::ranges` contains the + // relative time of point with respect to `TimedPointCloudData::time`. + std::unique_ptr AddRangeData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& range_data); + void AddOdometryData(const sensor::OdometryData& odometry_data); + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + std::unique_ptr AddAccumulatedRangeData( + common::Time time, + const sensor::RangeData& filtered_range_data_in_tracking, + const absl::optional& sensor_duration, + const transform::Rigid3d& pose_prediction, + const Eigen::Quaterniond& gravity_alignment); + + std::unique_ptr InsertIntoSubmap( + common::Time time, const sensor::RangeData& filtered_range_data_in_local, + const sensor::RangeData& filtered_range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud_in_tracking, + const sensor::PointCloud& low_resolution_point_cloud_in_tracking, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment); + + // Scan matches using the two point clouds and returns the observed pose, or + // nullptr on failure. + std::unique_ptr ScanMatch( + const transform::Rigid3d& pose_prediction, + const sensor::PointCloud& low_resolution_point_cloud_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud_in_tracking); + + const mapping::proto::LocalTrajectoryBuilderOptions3D options_; + mapping::ActiveSubmaps3D active_submaps_; + + mapping::MotionFilter motion_filter_; + std::unique_ptr + real_time_correlative_scan_matcher_; + std::unique_ptr ceres_scan_matcher_; + + std::unique_ptr extrapolator_; + + int num_accumulated_ = 0; + std::vector + accumulated_point_cloud_origin_data_; + absl::optional last_wall_time_; + + absl::optional last_thread_cpu_time_seconds_; + + RangeDataCollator range_data_collator_; + + absl::optional last_sensor_time_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc new file mode 100644 index 0000000..e8f1269 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -0,0 +1,309 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +constexpr char kSensorId[] = "sensor_id"; + +class LocalTrajectoryBuilderTest : public ::testing::Test { + protected: + struct TrajectoryNode { + common::Time time; + transform::Rigid3d pose; + }; + + void SetUp() override { GenerateBubbles(); } + + mapping::proto::LocalTrajectoryBuilderOptions3D + CreateTrajectoryBuilderOptions3D() { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + min_range = 0.5, + max_range = 50., + num_accumulated_range_data = 1, + voxel_filter_size = 0.2, + + high_resolution_adaptive_voxel_filter = { + max_length = 0.7, + min_num_points = 200, + max_range = 50., + }, + + low_resolution_adaptive_voxel_filter = { + max_length = 0.7, + min_num_points = 200, + max_range = 50., + }, + + use_online_correlative_scan_matching = false, + real_time_correlative_scan_matcher = { + linear_search_window = 0.2, + angular_search_window = math.rad(1.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1., + }, + + ceres_scan_matcher = { + occupied_space_weight_0 = 5., + occupied_space_weight_1 = 20., + translation_weight = 0.1, + rotation_weight = 0.3, + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 20, + num_threads = 1, + }, + }, + + motion_filter = { + max_time_seconds = 0.2, + max_distance_meters = 0.02, + max_angle_radians = 0.001, + }, + + imu_gravity_time_constant = 1., + rotational_histogram_size = 120, + + pose_extrapolator = { + use_imu_based = false, + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, + }, + + submaps = { + high_resolution = 0.2, + high_resolution_max_range = 50., + low_resolution = 0.5, + num_range_data = 45000, + range_data_inserter = { + hit_probability = 0.7, + miss_probability = 0.4, + num_free_space_voxels = 0, + intensity_threshold = 100.0, + }, + }, + + use_intensities = false, + } + )text"); + return mapping::CreateLocalTrajectoryBuilderOptions3D( + parameter_dictionary.get()); + } + + void GenerateBubbles() { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + + for (int i = 0; i != 100; ++i) { + const float x = distribution(prng); + const float y = distribution(prng); + const float z = distribution(prng); + bubbles_.push_back(10. * Eigen::Vector3f(x, y, z).normalized()); + } + } + + // Computes the earliest intersection of the ray 'from' to 'to' with the + // axis-aligned cube with edge length 30 and center at the origin. Assumes + // that 'from' is inside the cube. + Eigen::Vector3f CollideWithBox(const Eigen::Vector3f& from, + const Eigen::Vector3f& to) { + float first = 100.f; + if (to.x() > from.x()) { + first = std::min(first, (15.f - from.x()) / (to.x() - from.x())); + } else if (to.x() < from.x()) { + first = std::min(first, (-15.f - from.x()) / (to.x() - from.x())); + } + if (to.y() > from.y()) { + first = std::min(first, (15.f - from.y()) / (to.y() - from.y())); + } else if (to.y() < from.y()) { + first = std::min(first, (-15.f - from.y()) / (to.y() - from.y())); + } + if (to.z() > from.z()) { + first = std::min(first, (15.f - from.z()) / (to.z() - from.z())); + } else if (to.z() < from.z()) { + first = std::min(first, (-15.f - from.z()) / (to.z() - from.z())); + } + return first * (to - from) + from; + } + + // Computes the earliest intersection of the ray 'from' to 'to' with all + // bubbles. Returns 'to' if no intersection exists. + Eigen::Vector3f CollideWithBubbles(const Eigen::Vector3f& from, + const Eigen::Vector3f& to) { + float first = 1.f; + constexpr float kBubbleRadius = 0.5f; + for (const Eigen::Vector3f& center : bubbles_) { + const float a = (to - from).squaredNorm(); + const float beta = (to - from).dot(from - center); + const float c = + (from - center).squaredNorm() - kBubbleRadius * kBubbleRadius; + const float discriminant = beta * beta - a * c; + if (discriminant < 0.f) { + continue; + } + const float solution = (-beta - std::sqrt(discriminant)) / a; + if (solution < 0.f) { + continue; + } + first = std::min(first, solution); + } + return first * (to - from) + from; + } + + sensor::TimedPointCloudData GeneratePointCloudData( + const transform::Rigid3d& pose, const common::Time time) { + // 360 degree rays at 16 angles. + sensor::TimedPointCloud directions_in_rangefinder_frame; + for (int r = -8; r != 8; ++r) { + for (int s = -250; s != 250; ++s) { + const sensor::TimedRangefinderPoint first_point{ + Eigen::Vector3f{ + Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., + Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX()}, + 0.}; + directions_in_rangefinder_frame.push_back(first_point); + // Second orthogonal rangefinder. + const sensor::TimedRangefinderPoint second_point{ + Eigen::Vector3f{ + Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., + Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX()}, + 0.}; + directions_in_rangefinder_frame.push_back(second_point); + } + } + // We simulate a 30 m edge length box around the origin, also containing + // 50 cm radius spheres. + sensor::TimedPointCloud returns_in_world_frame; + for (const auto& direction_in_world_frame : + sensor::TransformTimedPointCloud(directions_in_rangefinder_frame, + pose.cast())) { + const Eigen::Vector3f origin = + pose.cast() * Eigen::Vector3f::Zero(); + const sensor::TimedRangefinderPoint return_point{ + CollideWithBubbles( + origin, + CollideWithBox(origin, direction_in_world_frame.position)), + 0.}; + returns_in_world_frame.push_back(return_point); + } + return {time, Eigen::Vector3f::Zero(), + sensor::TransformTimedPointCloud(returns_in_world_frame, + pose.inverse().cast())}; + } + + void AddLinearOnlyImuObservation(const common::Time time, + const transform::Rigid3d& expected_pose) { + const Eigen::Vector3d gravity = + expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81); + local_trajectory_builder_->AddImuData( + sensor::ImuData{time, gravity, Eigen::Vector3d::Zero()}); + } + + std::vector GenerateCorkscrewTrajectory() { + std::vector trajectory; + common::Time current_time = common::FromUniversal(12345678); + // One second at zero velocity. + for (int i = 0; i != 5; ++i) { + current_time += common::FromSeconds(0.3); + trajectory.push_back( + TrajectoryNode{current_time, transform::Rigid3d::Identity()}); + } + // Corkscrew translation and constant velocity rotation. + for (double t = 0.; t <= 0.6; t += 0.05) { + current_time += common::FromSeconds(0.3); + trajectory.push_back(TrajectoryNode{ + current_time, + transform::Rigid3d::Translation(Eigen::Vector3d( + std::sin(4. * t), 1. - std::cos(4. * t), 1. * t)) * + transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 0.3 * t, Eigen::Vector3d(1., -1., 2.).normalized()))}); + } + return trajectory; + } + + void VerifyAccuracy(const std::vector& expected_trajectory, + double expected_accuracy) { + int num_poses = 0; + for (const TrajectoryNode& node : expected_trajectory) { + AddLinearOnlyImuObservation(node.time, node.pose); + const auto point_cloud = GeneratePointCloudData(node.pose, node.time); + const std::unique_ptr + matching_result = + local_trajectory_builder_->AddRangeData(kSensorId, point_cloud); + if (matching_result != nullptr) { + EXPECT_THAT(matching_result->local_pose, + transform::IsNearly(node.pose, 1e-1)); + ++num_poses; + LOG(INFO) << "num_poses: " << num_poses; + } + } + } + + std::unique_ptr local_trajectory_builder_; + std::vector bubbles_; +}; + +TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) { + local_trajectory_builder_.reset(new LocalTrajectoryBuilder3D( + CreateTrajectoryBuilderOptions3D(), {kSensorId})); + VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc new file mode 100644 index 0000000..860b7d6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc @@ -0,0 +1,74 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" + +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping/internal/motion_filter.h" +#include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::LocalTrajectoryBuilderOptions3D options; + options.set_min_range(parameter_dictionary->GetDouble("min_range")); + options.set_max_range(parameter_dictionary->GetDouble("max_range")); + options.set_num_accumulated_range_data( + parameter_dictionary->GetInt("num_accumulated_range_data")); + options.set_voxel_filter_size( + parameter_dictionary->GetDouble("voxel_filter_size")); + *options.mutable_high_resolution_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("high_resolution_adaptive_voxel_filter") + .get()); + *options.mutable_low_resolution_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("low_resolution_adaptive_voxel_filter") + .get()); + options.set_use_online_correlative_scan_matching( + parameter_dictionary->GetBool("use_online_correlative_scan_matching")); + *options.mutable_real_time_correlative_scan_matcher_options() = + mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary + ->GetDictionary("real_time_correlative_scan_matcher") + .get()); + *options.mutable_ceres_scan_matcher_options() = + mapping::scan_matching::CreateCeresScanMatcherOptions3D( + parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); + *options.mutable_motion_filter_options() = CreateMotionFilterOptions( + parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + options.set_rotational_histogram_size( + parameter_dictionary->GetInt("rotational_histogram_size")); + *options.mutable_submaps_options() = CreateSubmapsOptions3D( + parameter_dictionary->GetDictionary("submaps").get()); + options.set_use_intensities(parameter_dictionary->GetBool("use_intensities")); + return options; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h new file mode 100644 index 0000000..e26a6ec --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h @@ -0,0 +1,32 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" + +namespace cartographer { +namespace mapping { + +proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d.cc new file mode 100644 index 0000000..8a91e59 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -0,0 +1,1320 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/pose_graph_3d.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); +static auto* kWorkQueueSizeMetric = metrics::Gauge::Null(); +static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); +static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null(); +static auto* kActiveSubmapsMetric = metrics::Gauge::Null(); +static auto* kFrozenSubmapsMetric = metrics::Gauge::Null(); +static auto* kDeletedSubmapsMetric = metrics::Gauge::Null(); + +PoseGraph3D::PoseGraph3D( + const proto::PoseGraphOptions& options, + std::unique_ptr optimization_problem, + common::ThreadPool* thread_pool) + : options_(options), + optimization_problem_(std::move(optimization_problem)), + constraint_builder_(options_.constraint_builder_options(), thread_pool), + thread_pool_(thread_pool) {} + +PoseGraph3D::~PoseGraph3D() { + WaitForAllComputations(); + absl::MutexLock locker(&work_queue_mutex_); + CHECK(work_queue_ == nullptr); +} + +std::vector PoseGraph3D::InitializeGlobalSubmapPoses( + const int trajectory_id, const common::Time time, + const std::vector>& insertion_submaps) { + CHECK(!insertion_submaps.empty()); + const auto& submap_data = optimization_problem_->submap_data(); + if (insertion_submaps.size() == 1) { + // If we don't already have an entry for the first submap, add one. + if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + if (data_.initial_trajectory_poses.count(trajectory_id) > 0) { + data_.trajectory_connectivity_state.Connect( + trajectory_id, + data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id, + time); + } + optimization_problem_->AddSubmap( + trajectory_id, ComputeLocalToGlobalTransform( + data_.global_submap_poses_3d, trajectory_id) * + insertion_submaps[0]->local_pose()); + } + CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); + const SubmapId submap_id{trajectory_id, 0}; + CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front()); + return {submap_id}; + } + CHECK_EQ(2, insertion_submaps.size()); + const auto end_it = submap_data.EndOfTrajectory(trajectory_id); + CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); + const SubmapId last_submap_id = std::prev(end_it)->id; + if (data_.submap_data.at(last_submap_id).submap == + insertion_submaps.front()) { + // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' + // and 'insertions_submaps.back()' is new. + const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; + optimization_problem_->AddSubmap( + trajectory_id, first_submap_pose * + insertion_submaps[0]->local_pose().inverse() * + insertion_submaps[1]->local_pose()); + return {last_submap_id, + SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; + } + CHECK(data_.submap_data.at(last_submap_id).submap == + insertion_submaps.back()); + const SubmapId front_submap_id{trajectory_id, + last_submap_id.submap_index - 1}; + CHECK(data_.submap_data.at(front_submap_id).submap == + insertion_submaps.front()); + return {front_submap_id, last_submap_id}; +} + +NodeId PoseGraph3D::AppendNode( + std::shared_ptr constant_data, + const int trajectory_id, + const std::vector>& insertion_submaps, + const transform::Rigid3d& optimized_pose) { + absl::MutexLock locker(&mutex_); + AddTrajectoryIfNeeded(trajectory_id); + if (!CanAddWorkItemModifying(trajectory_id)) { + LOG(WARNING) << "AddNode was called for finished or deleted trajectory."; + } + const NodeId node_id = data_.trajectory_nodes.Append( + trajectory_id, TrajectoryNode{constant_data, optimized_pose}); + ++data_.num_trajectory_nodes; + // Test if the 'insertion_submap.back()' is one we never saw before. + if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + std::prev(data_.submap_data.EndOfTrajectory(trajectory_id)) + ->data.submap != insertion_submaps.back()) { + // We grow 'data_.submap_data' as needed. This code assumes that the first + // time we see a new submap is as 'insertion_submaps.back()'. + const SubmapId submap_id = + data_.submap_data.Append(trajectory_id, InternalSubmapData()); + data_.submap_data.at(submap_id).submap = insertion_submaps.back(); + LOG(INFO) << "Inserted submap " << submap_id << "."; + kActiveSubmapsMetric->Increment(); + } + return node_id; +} + +NodeId PoseGraph3D::AddNode( + std::shared_ptr constant_data, + const int trajectory_id, + const std::vector>& insertion_submaps) { + const transform::Rigid3d optimized_pose( + GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); + + const NodeId node_id = AppendNode(constant_data, trajectory_id, + insertion_submaps, optimized_pose); + // We have to check this here, because it might have changed by the time we + // execute the lambda. + const bool newly_finished_submap = + insertion_submaps.front()->insertion_finished(); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + return ComputeConstraintsForNode(node_id, insertion_submaps, + newly_finished_submap); + }); + return node_id; +} + +void PoseGraph3D::AddWorkItem( + const std::function& work_item) { + absl::MutexLock locker(&work_queue_mutex_); + if (work_queue_ == nullptr) { + work_queue_ = absl::make_unique(); + auto task = absl::make_unique(); + task->SetWorkItem([this]() { DrainWorkQueue(); }); + thread_pool_->Schedule(std::move(task)); + } + const auto now = std::chrono::steady_clock::now(); + work_queue_->push_back({now, work_item}); + kWorkQueueSizeMetric->Set(work_queue_->size()); + kWorkQueueDelayMetric->Set( + std::chrono::duration_cast>( + now - work_queue_->front().time) + .count()); +} + +void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) { + data_.trajectories_state[trajectory_id]; + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::FINISHED); + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::DELETED); + CHECK(data_.trajectories_state.at(trajectory_id).deletion_state == + InternalTrajectoryState::DeletionState::NORMAL); + data_.trajectory_connectivity_state.Add(trajectory_id); + // Make sure we have a sampler for this trajectory. + if (!global_localization_samplers_[trajectory_id]) { + global_localization_samplers_[trajectory_id] = + absl::make_unique( + options_.global_sampling_ratio()); + } +} + +void PoseGraph3D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddImuData(trajectory_id, imu_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddOdometryData(const int trajectory_id, + const sensor::OdometryData& odometry_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddOdometryData(trajectory_id, odometry_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddFixedFramePoseData(trajectory_id, + fixed_frame_pose_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + for (const auto& observation : landmark_data.landmark_observations) { + data_.landmark_nodes[observation.id].landmark_observations.emplace_back( + PoseGraphInterface::LandmarkNode::LandmarkObservation{ + trajectory_id, landmark_data.time, + observation.landmark_to_tracking_transform, + observation.translation_weight, observation.rotation_weight}); + } + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::ComputeConstraint(const NodeId& node_id, + const SubmapId& submap_id) { + const transform::Rigid3d global_node_pose = + optimization_problem_->node_data().at(node_id).global_pose; + + const transform::Rigid3d global_submap_pose = + optimization_problem_->submap_data().at(submap_id).global_pose; + + bool maybe_add_local_constraint = false; + bool maybe_add_global_constraint = false; + const TrajectoryNode::Data* constant_data; + const Submap3D* submap; + { + absl::MutexLock locker(&mutex_); + CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); + if (!data_.submap_data.at(submap_id).submap->insertion_finished()) { + // Uplink server only receives grids when they are finished, so skip + // constraint search before that. + return; + } + + const common::Time node_time = GetLatestNodeTime(node_id, submap_id); + const common::Time last_connection_time = + data_.trajectory_connectivity_state.LastConnectionTime( + node_id.trajectory_id, submap_id.trajectory_id); + if (node_id.trajectory_id == submap_id.trajectory_id || + node_time < + last_connection_time + + common::FromSeconds( + options_.global_constraint_search_after_n_seconds())) { + // If the node and the submap belong to the same trajectory or if there + // has been a recent global constraint that ties that node's trajectory to + // the submap's trajectory, it suffices to do a match constrained to a + // local search window. + maybe_add_local_constraint = true; + } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { + // In this situation, 'global_node_pose' and 'global_submap_pose' have + // orientations agreeing on gravity. Their relationship regarding yaw is + // arbitrary. Finding the correct yaw component will be handled by the + // matching procedure in the FastCorrelativeScanMatcher, and the given yaw + // is essentially ignored. + maybe_add_global_constraint = true; + } + constant_data = data_.trajectory_nodes.at(node_id).constant_data.get(); + submap = static_cast( + data_.submap_data.at(submap_id).submap.get()); + } + + if (maybe_add_local_constraint) { + constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, + constant_data, global_node_pose, + global_submap_pose); + } else if (maybe_add_global_constraint) { + constraint_builder_.MaybeAddGlobalConstraint( + submap_id, submap, node_id, constant_data, global_node_pose.rotation(), + global_submap_pose.rotation()); + } +} + +WorkItem::Result PoseGraph3D::ComputeConstraintsForNode( + const NodeId& node_id, + std::vector> insertion_submaps, + const bool newly_finished_submap) { + std::vector submap_ids; + std::vector finished_submap_ids; + std::set newly_finished_submap_node_ids; + { + absl::MutexLock locker(&mutex_); + const auto& constant_data = + data_.trajectory_nodes.at(node_id).constant_data; + submap_ids = InitializeGlobalSubmapPoses( + node_id.trajectory_id, constant_data->time, insertion_submaps); + CHECK_EQ(submap_ids.size(), insertion_submaps.size()); + const SubmapId matching_id = submap_ids.front(); + const transform::Rigid3d& local_pose = constant_data->local_pose; + const transform::Rigid3d global_pose = + optimization_problem_->submap_data().at(matching_id).global_pose * + insertion_submaps.front()->local_pose().inverse() * local_pose; + optimization_problem_->AddTrajectoryNode( + matching_id.trajectory_id, + optimization::NodeSpec3D{constant_data->time, local_pose, global_pose}); + for (size_t i = 0; i < insertion_submaps.size(); ++i) { + const SubmapId submap_id = submap_ids[i]; + // Even if this was the last node added to 'submap_id', the submap will + // only be marked as finished in 'data_.submap_data' further below. + CHECK(data_.submap_data.at(submap_id).state == + SubmapState::kNoConstraintSearch); + data_.submap_data.at(submap_id).node_ids.emplace(node_id); + const transform::Rigid3d constraint_transform = + insertion_submaps[i]->local_pose().inverse() * local_pose; + data_.constraints.push_back(Constraint{ + submap_id, + node_id, + {constraint_transform, options_.matcher_translation_weight(), + options_.matcher_rotation_weight()}, + Constraint::INTRA_SUBMAP}); + } + // TODO(gaschler): Consider not searching for constraints against + // trajectories scheduled for deletion. + // TODO(danielsievers): Add a member variable and avoid having to copy + // them out here. + for (const auto& submap_id_data : data_.submap_data) { + if (submap_id_data.data.state == SubmapState::kFinished) { + CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); + finished_submap_ids.emplace_back(submap_id_data.id); + } + } + if (newly_finished_submap) { + const SubmapId newly_finished_submap_id = submap_ids.front(); + InternalSubmapData& finished_submap_data = + data_.submap_data.at(newly_finished_submap_id); + CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch); + finished_submap_data.state = SubmapState::kFinished; + newly_finished_submap_node_ids = finished_submap_data.node_ids; + } + } + + for (const auto& submap_id : finished_submap_ids) { + ComputeConstraint(node_id, submap_id); + } + + if (newly_finished_submap) { + const SubmapId newly_finished_submap_id = submap_ids.front(); + // We have a new completed submap, so we look into adding constraints for + // old nodes. + for (const auto& node_id_data : optimization_problem_->node_data()) { + const NodeId& node_id = node_id_data.id; + if (newly_finished_submap_node_ids.count(node_id) == 0) { + ComputeConstraint(node_id, newly_finished_submap_id); + } + } + } + constraint_builder_.NotifyEndOfNode(); + absl::MutexLock locker(&mutex_); + ++num_nodes_since_last_loop_closure_; + if (options_.optimize_every_n_nodes() > 0 && + num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) { + return WorkItem::Result::kRunOptimization; + } + return WorkItem::Result::kDoNotRunOptimization; +} + +common::Time PoseGraph3D::GetLatestNodeTime(const NodeId& node_id, + const SubmapId& submap_id) const { + common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time; + const InternalSubmapData& submap_data = data_.submap_data.at(submap_id); + if (!submap_data.node_ids.empty()) { + const NodeId last_submap_node_id = + *data_.submap_data.at(submap_id).node_ids.rbegin(); + time = std::max( + time, + data_.trajectory_nodes.at(last_submap_node_id).constant_data->time); + } + return time; +} + +void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) { + CHECK_EQ(constraint.tag, PoseGraphInterface::Constraint::INTER_SUBMAP); + const common::Time time = + GetLatestNodeTime(constraint.node_id, constraint.submap_id); + data_.trajectory_connectivity_state.Connect( + constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, + time); +} + +void PoseGraph3D::DeleteTrajectoriesIfNeeded() { + TrimmingHandle trimming_handle(this); + for (auto& it : data_.trajectories_state) { + if (it.second.deletion_state == + InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION) { + // TODO(gaschler): Consider directly deleting from data_, which may be + // more complete. + auto submap_ids = trimming_handle.GetSubmapIds(it.first); + for (auto& submap_id : submap_ids) { + trimming_handle.TrimSubmap(submap_id); + } + it.second.state = TrajectoryState::DELETED; + it.second.deletion_state = InternalTrajectoryState::DeletionState::NORMAL; + } + } +} + +void PoseGraph3D::HandleWorkQueue( + const constraints::ConstraintBuilder3D::Result& result) { + { + absl::MutexLock locker(&mutex_); + data_.constraints.insert(data_.constraints.end(), result.begin(), + result.end()); + } + RunOptimization(); + + if (global_slam_optimization_callback_) { + std::map trajectory_id_to_last_optimized_node_id; + std::map trajectory_id_to_last_optimized_submap_id; + { + absl::MutexLock locker(&mutex_); + const auto& submap_data = optimization_problem_->submap_data(); + const auto& node_data = optimization_problem_->node_data(); + for (const int trajectory_id : node_data.trajectory_ids()) { + if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + continue; + } + trajectory_id_to_last_optimized_node_id.emplace( + trajectory_id, + std::prev(node_data.EndOfTrajectory(trajectory_id))->id); + trajectory_id_to_last_optimized_submap_id.emplace( + trajectory_id, + std::prev(submap_data.EndOfTrajectory(trajectory_id))->id); + } + } + global_slam_optimization_callback_( + trajectory_id_to_last_optimized_submap_id, + trajectory_id_to_last_optimized_node_id); + } + + { + absl::MutexLock locker(&mutex_); + for (const Constraint& constraint : result) { + UpdateTrajectoryConnectivity(constraint); + } + DeleteTrajectoriesIfNeeded(); + TrimmingHandle trimming_handle(this); + for (auto& trimmer : trimmers_) { + trimmer->Trim(&trimming_handle); + } + trimmers_.erase( + std::remove_if(trimmers_.begin(), trimmers_.end(), + [](std::unique_ptr& trimmer) { + return trimmer->IsFinished(); + }), + trimmers_.end()); + + num_nodes_since_last_loop_closure_ = 0; + + // Update the gauges that count the current number of constraints. + double inter_constraints_same_trajectory = 0; + double inter_constraints_different_trajectory = 0; + for (const auto& constraint : data_.constraints) { + if (constraint.tag == + cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + if (constraint.node_id.trajectory_id == + constraint.submap_id.trajectory_id) { + ++inter_constraints_same_trajectory; + } else { + ++inter_constraints_different_trajectory; + } + } + kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory); + kConstraintsDifferentTrajectoryMetric->Set( + inter_constraints_different_trajectory); + } + + DrainWorkQueue(); +} + +void PoseGraph3D::DrainWorkQueue() { + bool process_work_queue = true; + size_t work_queue_size; + while (process_work_queue) { + std::function work_item; + { + absl::MutexLock locker(&work_queue_mutex_); + if (work_queue_->empty()) { + work_queue_.reset(); + return; + } + work_item = work_queue_->front().task; + work_queue_->pop_front(); + work_queue_size = work_queue_->size(); + kWorkQueueSizeMetric->Set(work_queue_size); + } + process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; + } + LOG(INFO) << "Remaining work items in queue: " << work_queue_size; + // We have to optimize again. + constraint_builder_.WhenDone( + [this](const constraints::ConstraintBuilder3D::Result& result) { + HandleWorkQueue(result); + }); +} + +void PoseGraph3D::WaitForAllComputations() { + int num_trajectory_nodes; + { + absl::MutexLock locker(&mutex_); + num_trajectory_nodes = data_.num_trajectory_nodes; + } + + const int num_finished_nodes_at_start = + constraint_builder_.GetNumFinishedNodes(); + + auto report_progress = [this, num_trajectory_nodes, + num_finished_nodes_at_start]() { + // Log progress on nodes only when we are actually processing nodes. + if (num_trajectory_nodes != num_finished_nodes_at_start) { + std::ostringstream progress_info; + progress_info << "Optimizing: " << std::fixed << std::setprecision(1) + << 100. * + (constraint_builder_.GetNumFinishedNodes() - + num_finished_nodes_at_start) / + (num_trajectory_nodes - num_finished_nodes_at_start) + << "%..."; + std::cout << "\r\x1b[K" << progress_info.str() << std::flush; + } + }; + + // First wait for the work queue to drain so that it's safe to schedule + // a WhenDone() callback. + { + const auto predicate = [this]() + EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) { + return work_queue_ == nullptr; + }; + absl::MutexLock locker(&work_queue_mutex_); + while (!work_queue_mutex_.AwaitWithTimeout( + absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { + report_progress(); + } + } + + // Now wait for any pending constraint computations to finish. + absl::MutexLock locker(&mutex_); + bool notification = false; + constraint_builder_.WhenDone( + [this, + ¬ification](const constraints::ConstraintBuilder3D::Result& result) + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.constraints.insert(data_.constraints.end(), result.begin(), + result.end()); + notification = true; + }); + const auto predicate = [¬ification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return notification; + }; + while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { + report_progress(); + } + CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes); + std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; +} + +void PoseGraph3D::DeleteTrajectory(const int trajectory_id) { + { + absl::MutexLock locker(&mutex_); + auto it = data_.trajectories_state.find(trajectory_id); + if (it == data_.trajectories_state.end()) { + LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " + << trajectory_id; + return; + } + it->second.deletion_state = + InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; + } + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::ACTIVE); + CHECK(data_.trajectories_state.at(trajectory_id).state != + TrajectoryState::DELETED); + CHECK(data_.trajectories_state.at(trajectory_id).deletion_state == + InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION); + data_.trajectories_state.at(trajectory_id).deletion_state = + InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::FinishTrajectory(const int trajectory_id) { + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + CHECK(!IsTrajectoryFinished(trajectory_id)); + data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; + + for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) { + data_.submap_data.at(submap.id).state = SubmapState::kFinished; + } + return WorkItem::Result::kRunOptimization; + }); +} + +bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const { + return data_.trajectories_state.count(trajectory_id) != 0 && + data_.trajectories_state.at(trajectory_id).state == + TrajectoryState::FINISHED; +} + +void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { + { + absl::MutexLock locker(&mutex_); + data_.trajectory_connectivity_state.Add(trajectory_id); + } + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + CHECK(!IsTrajectoryFrozen(trajectory_id)); + // Connect multiple frozen trajectories among each other. + // This is required for localization against multiple frozen trajectories + // because we lose inter-trajectory constraints when freezing. + for (const auto& entry : data_.trajectories_state) { + const int other_trajectory_id = entry.first; + if (!IsTrajectoryFrozen(other_trajectory_id)) { + continue; + } + if (data_.trajectory_connectivity_state.TransitivelyConnected( + trajectory_id, other_trajectory_id)) { + // Already connected, nothing to do. + continue; + } + data_.trajectory_connectivity_state.Connect( + trajectory_id, other_trajectory_id, common::FromUniversal(0)); + } + data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const { + return data_.trajectories_state.count(trajectory_id) != 0 && + data_.trajectories_state.at(trajectory_id).state == + TrajectoryState::FROZEN; +} + +void PoseGraph3D::AddSubmapFromProto( + const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { + if (!submap.has_submap_3d()) { + return; + } + + const SubmapId submap_id = {submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; + std::shared_ptr submap_ptr = + std::make_shared(submap.submap_3d()); + + { + absl::MutexLock locker(&mutex_); + AddTrajectoryIfNeeded(submap_id.trajectory_id); + if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; + data_.submap_data.Insert(submap_id, InternalSubmapData()); + data_.submap_data.at(submap_id).submap = submap_ptr; + // Immediately show the submap at the 'global_submap_pose'. + data_.global_submap_poses_3d.Insert( + submap_id, optimization::SubmapSpec3D{global_submap_pose}); + } + + // TODO(MichaelGrupp): MapBuilder does freezing before deserializing submaps, + // so this should be fine. + if (IsTrajectoryFrozen(submap_id.trajectory_id)) { + kFrozenSubmapsMetric->Increment(); + } else { + kActiveSubmapsMetric->Increment(); + } + + AddWorkItem([this, submap_id, global_submap_pose]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.submap_data.at(submap_id).state = SubmapState::kFinished; + optimization_problem_->InsertSubmap(submap_id, global_submap_pose); + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, + const proto::Node& node) { + const NodeId node_id = {node.node_id().trajectory_id(), + node.node_id().node_index()}; + std::shared_ptr constant_data = + std::make_shared(FromProto(node.node_data())); + + { + absl::MutexLock locker(&mutex_); + AddTrajectoryIfNeeded(node_id.trajectory_id); + if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; + data_.trajectory_nodes.Insert(node_id, + TrajectoryNode{constant_data, global_pose}); + } + + AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + const auto& constant_data = + data_.trajectory_nodes.at(node_id).constant_data; + optimization_problem_->InsertTrajectoryNode( + node_id, + optimization::NodeSpec3D{constant_data->time, constant_data->local_pose, + global_pose}); + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::SetTrajectoryDataFromProto( + const proto::TrajectoryData& data) { + TrajectoryData trajectory_data; + trajectory_data.gravity_constant = data.gravity_constant(); + trajectory_data.imu_calibration = { + {data.imu_calibration().w(), data.imu_calibration().x(), + data.imu_calibration().y(), data.imu_calibration().z()}}; + if (data.has_fixed_frame_origin_in_map()) { + trajectory_data.fixed_frame_origin_in_map = + transform::ToRigid3(data.fixed_frame_origin_in_map()); + } + + const int trajectory_id = data.trajectory_id(); + AddWorkItem([this, trajectory_id, trajectory_data]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) { + AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(submap_id.trajectory_id)) { + data_.submap_data.at(submap_id).node_ids.insert(node_id); + } + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddSerializedConstraints( + const std::vector& constraints) { + AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + for (const auto& constraint : constraints) { + CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); + CHECK(data_.submap_data.Contains(constraint.submap_id)); + CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data != + nullptr); + CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr); + switch (constraint.tag) { + case Constraint::Tag::INTRA_SUBMAP: + CHECK(data_.submap_data.at(constraint.submap_id) + .node_ids.emplace(constraint.node_id) + .second); + break; + case Constraint::Tag::INTER_SUBMAP: + UpdateTrajectoryConnectivity(constraint); + break; + } + data_.constraints.push_back(constraint); + } + LOG(INFO) << "Loaded " << constraints.size() << " constraints."; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::AddTrimmer(std::unique_ptr trimmer) { + // C++11 does not allow us to move a unique_ptr into a lambda. + PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); + AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + trimmers_.emplace_back(trimmer_ptr); + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +void PoseGraph3D::RunFinalOptimization() { + { + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + optimization_problem_->SetMaxNumIterations( + options_.max_num_final_iterations()); + return WorkItem::Result::kRunOptimization; + }); + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + optimization_problem_->SetMaxNumIterations( + options_.optimization_problem_options() + .ceres_solver_options() + .max_num_iterations()); + return WorkItem::Result::kDoNotRunOptimization; + }); + } + WaitForAllComputations(); +} + +void PoseGraph3D::LogResidualHistograms() const { + common::Histogram rotational_residual; + common::Histogram translational_residual; + for (const Constraint& constraint : data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + const cartographer::transform::Rigid3d optimized_node_to_map = + data_.trajectory_nodes.at(constraint.node_id).global_pose; + const cartographer::transform::Rigid3d node_to_submap_constraint = + constraint.pose.zbar_ij; + const cartographer::transform::Rigid3d optimized_submap_to_map = + data_.global_submap_poses_3d.at(constraint.submap_id).global_pose; + const cartographer::transform::Rigid3d optimized_node_to_submap = + optimized_submap_to_map.inverse() * optimized_node_to_map; + const cartographer::transform::Rigid3d residual = + node_to_submap_constraint.inverse() * optimized_node_to_submap; + rotational_residual.Add( + common::NormalizeAngleDifference(transform::GetAngle(residual))); + translational_residual.Add(residual.translation().norm()); + } + } + LOG(INFO) << "Translational residuals histogram:\n" + << translational_residual.ToString(10); + LOG(INFO) << "Rotational residuals histogram:\n" + << rotational_residual.ToString(10); +} + +void PoseGraph3D::RunOptimization() { + if (optimization_problem_->submap_data().empty()) { + return; + } + + // No other thread is accessing the optimization_problem_, data_.constraints, + // data_.frozen_trajectories and data_.landmark_nodes when executing the + // Solve. Solve is time consuming, so not taking the mutex before Solve to + // avoid blocking foreground processing. + optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), + data_.landmark_nodes); + absl::MutexLock locker(&mutex_); + + const auto& submap_data = optimization_problem_->submap_data(); + const auto& node_data = optimization_problem_->node_data(); + for (const int trajectory_id : node_data.trajectory_ids()) { + for (const auto& node : node_data.trajectory(trajectory_id)) { + data_.trajectory_nodes.at(node.id).global_pose = node.data.global_pose; + } + + // Extrapolate all point cloud poses that were not included in the + // 'optimization_problem_' yet. + const auto local_to_new_global = + ComputeLocalToGlobalTransform(submap_data, trajectory_id); + const auto local_to_old_global = ComputeLocalToGlobalTransform( + data_.global_submap_poses_3d, trajectory_id); + const transform::Rigid3d old_global_to_new_global = + local_to_new_global * local_to_old_global.inverse(); + + const NodeId last_optimized_node_id = + std::prev(node_data.EndOfTrajectory(trajectory_id))->id; + auto node_it = + std::next(data_.trajectory_nodes.find(last_optimized_node_id)); + for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id); + ++node_it) { + auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id); + mutable_trajectory_node.global_pose = + old_global_to_new_global * mutable_trajectory_node.global_pose; + } + } + for (const auto& landmark : optimization_problem_->landmark_data()) { + data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second; + } + data_.global_submap_poses_3d = submap_data; + + // Log the histograms for the pose residuals. + if (options_.log_residual_histograms()) { + LogResidualHistograms(); + } +} + +bool PoseGraph3D::CanAddWorkItemModifying(int trajectory_id) { + auto it = data_.trajectories_state.find(trajectory_id); + if (it == data_.trajectories_state.end()) { + return true; + } + if (it->second.state == TrajectoryState::FINISHED) { + // TODO(gaschler): Replace all FATAL to WARNING after some testing. + LOG(FATAL) << "trajectory_id " << trajectory_id + << " has finished " + "but modification is requested, skipping."; + return false; + } + if (it->second.deletion_state != + InternalTrajectoryState::DeletionState::NORMAL) { + LOG(FATAL) << "trajectory_id " << trajectory_id + << " has been scheduled for deletion " + "but modification is requested, skipping."; + return false; + } + if (it->second.state == TrajectoryState::DELETED) { + LOG(FATAL) << "trajectory_id " << trajectory_id + << " has been deleted " + "but modification is requested, skipping."; + return false; + } + return true; +} + +MapById PoseGraph3D::GetTrajectoryNodes() const { + absl::MutexLock locker(&mutex_); + return data_.trajectory_nodes; +} + +MapById PoseGraph3D::GetTrajectoryNodePoses() + const { + MapById node_poses; + absl::MutexLock locker(&mutex_); + for (const auto& node_id_data : data_.trajectory_nodes) { + absl::optional constant_pose_data; + if (node_id_data.data.constant_data != nullptr) { + constant_pose_data = TrajectoryNodePose::ConstantPoseData{ + node_id_data.data.constant_data->time, + node_id_data.data.constant_data->local_pose}; + } + node_poses.Insert( + node_id_data.id, + TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data}); + } + return node_poses; +} + +std::map +PoseGraph3D::GetTrajectoryStates() const { + std::map trajectories_state; + absl::MutexLock locker(&mutex_); + for (const auto& it : data_.trajectories_state) { + trajectories_state[it.first] = it.second.state; + } + return trajectories_state; +} + +std::map PoseGraph3D::GetLandmarkPoses() + const { + std::map landmark_poses; + absl::MutexLock locker(&mutex_); + for (const auto& landmark : data_.landmark_nodes) { + // Landmark without value has not been optimized yet. + if (!landmark.second.global_landmark_pose.has_value()) continue; + landmark_poses[landmark.first] = + landmark.second.global_landmark_pose.value(); + } + return landmark_poses; +} + +void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; + data_.landmark_nodes[landmark_id].frozen = frozen; + return WorkItem::Result::kDoNotRunOptimization; + }); +} + +sensor::MapByTime PoseGraph3D::GetImuData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->imu_data(); +} + +sensor::MapByTime PoseGraph3D::GetOdometryData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->odometry_data(); +} + +sensor::MapByTime +PoseGraph3D::GetFixedFramePoseData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->fixed_frame_pose_data(); +} + +std::map +PoseGraph3D::GetLandmarkNodes() const { + absl::MutexLock locker(&mutex_); + return data_.landmark_nodes; +} + +std::map +PoseGraph3D::GetTrajectoryData() const { + absl::MutexLock locker(&mutex_); + return optimization_problem_->trajectory_data(); +} + +std::vector PoseGraph3D::constraints() const { + absl::MutexLock locker(&mutex_); + return data_.constraints; +} + +void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id, + const int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) { + absl::MutexLock locker(&mutex_); + data_.initial_trajectory_poses[from_trajectory_id] = + InitialTrajectoryPose{to_trajectory_id, pose, time}; +} + +transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose( + const int trajectory_id, const common::Time time) const { + CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0); + const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time); + if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) { + return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id) + ->data.global_pose; + } + if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) { + return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) + ->data.global_pose; + } + return transform::Interpolate( + transform::TimestampedTransform{std::prev(it)->data.time(), + std::prev(it)->data.global_pose}, + transform::TimestampedTransform{it->data.time(), + it->data.global_pose}, + time) + .transform; +} + +transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform( + const int trajectory_id) const { + absl::MutexLock locker(&mutex_); + return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d, + trajectory_id); +} + +std::vector> PoseGraph3D::GetConnectedTrajectories() const { + absl::MutexLock locker(&mutex_); + return data_.trajectory_connectivity_state.Components(); +} + +PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData( + const SubmapId& submap_id) const { + absl::MutexLock locker(&mutex_); + return GetSubmapDataUnderLock(submap_id); +} + +MapById +PoseGraph3D::GetAllSubmapData() const { + absl::MutexLock locker(&mutex_); + return GetSubmapDataUnderLock(); +} + +MapById +PoseGraph3D::GetAllSubmapPoses() const { + absl::MutexLock locker(&mutex_); + MapById submap_poses; + for (const auto& submap_id_data : data_.submap_data) { + auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); + submap_poses.Insert( + submap_id_data.id, + PoseGraphInterface::SubmapPose{submap_data.submap->num_range_data(), + submap_data.pose}); + } + return submap_poses; +} + +transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( + const MapById& global_submap_poses, + const int trajectory_id) const { + auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); + auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); + if (begin_it == end_it) { + const auto it = data_.initial_trajectory_poses.find(trajectory_id); + if (it != data_.initial_trajectory_poses.end()) { + return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, + it->second.time) * + it->second.relative_pose; + } else { + return transform::Rigid3d::Identity(); + } + } + const SubmapId last_optimized_submap_id = std::prev(end_it)->id; + // Accessing 'local_pose' in Submap is okay, since the member is const. + return global_submap_poses.at(last_optimized_submap_id).global_pose * + data_.submap_data.at(last_optimized_submap_id) + .submap->local_pose() + .inverse(); +} + +PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock( + const SubmapId& submap_id) const { + const auto it = data_.submap_data.find(submap_id); + if (it == data_.submap_data.end()) { + return {}; + } + auto submap = it->data.submap; + if (data_.global_submap_poses_3d.Contains(submap_id)) { + // We already have an optimized pose. + return {submap, data_.global_submap_poses_3d.at(submap_id).global_pose}; + } + // We have to extrapolate. + return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_3d, + submap_id.trajectory_id) * + submap->local_pose()}; +} + +PoseGraph3D::TrimmingHandle::TrimmingHandle(PoseGraph3D* const parent) + : parent_(parent) {} + +int PoseGraph3D::TrimmingHandle::num_submaps(const int trajectory_id) const { + const auto& submap_data = parent_->optimization_problem_->submap_data(); + return submap_data.SizeOfTrajectoryOrZero(trajectory_id); +} + +std::vector PoseGraph3D::TrimmingHandle::GetSubmapIds( + int trajectory_id) const { + std::vector submap_ids; + const auto& submap_data = parent_->optimization_problem_->submap_data(); + for (const auto& it : submap_data.trajectory(trajectory_id)) { + submap_ids.push_back(it.id); + } + return submap_ids; +} +MapById +PoseGraph3D::TrimmingHandle::GetOptimizedSubmapData() const { + MapById submaps; + for (const auto& submap_id_data : parent_->data_.submap_data) { + if (submap_id_data.data.state != SubmapState::kFinished || + !parent_->data_.global_submap_poses_3d.Contains(submap_id_data.id)) { + continue; + } + submaps.Insert( + submap_id_data.id, + SubmapData{submap_id_data.data.submap, + parent_->data_.global_submap_poses_3d.at(submap_id_data.id) + .global_pose}); + } + return submaps; +} + +const MapById& +PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const { + return parent_->data_.trajectory_nodes; +} + +const std::vector& +PoseGraph3D::TrimmingHandle::GetConstraints() const { + return parent_->data_.constraints; +} + +bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const { + return parent_->IsTrajectoryFinished(trajectory_id); +} + +void PoseGraph3D::TrimmingHandle::SetTrajectoryState(int trajectory_id, + TrajectoryState state) { + parent_->data_.trajectories_state[trajectory_id].state = state; +} + +void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { + // TODO(hrapp): We have to make sure that the trajectory has been finished + // if we want to delete the last submaps. + CHECK(parent_->data_.submap_data.at(submap_id).state == + SubmapState::kFinished); + + // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps + // once the submap with 'submap_id' is gone. + // We need to use node_ids instead of constraints here to be also compatible + // with frozen trajectories that don't have intra-constraints. + std::set nodes_to_retain; + for (const auto& submap_data : parent_->data_.submap_data) { + if (submap_data.id != submap_id) { + nodes_to_retain.insert(submap_data.data.node_ids.begin(), + submap_data.data.node_ids.end()); + } + } + + // Remove all nodes that are exlusively associated to 'submap_id'. + std::set nodes_to_remove; + std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(), + parent_->data_.submap_data.at(submap_id).node_ids.end(), + nodes_to_retain.begin(), nodes_to_retain.end(), + std::inserter(nodes_to_remove, nodes_to_remove.begin())); + + // Remove all 'data_.constraints' related to 'submap_id'. + { + std::vector constraints; + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.submap_id != submap_id) { + constraints.push_back(constraint); + } + } + parent_->data_.constraints = std::move(constraints); + } + + // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. + { + std::vector constraints; + std::set other_submap_ids_losing_constraints; + for (const Constraint& constraint : parent_->data_.constraints) { + if (nodes_to_remove.count(constraint.node_id) == 0) { + constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); + } + } + parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } + } + + // Mark the submap with 'submap_id' as trimmed and remove its data. + CHECK(parent_->data_.submap_data.at(submap_id).state == + SubmapState::kFinished); + parent_->data_.submap_data.Trim(submap_id); + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + parent_->optimization_problem_->TrimSubmap(submap_id); + + // We have one submap less, update the gauge metrics. + kDeletedSubmapsMetric->Increment(); + if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) { + kFrozenSubmapsMetric->Decrement(); + } else { + kActiveSubmapsMetric->Decrement(); + } + + // Remove the 'nodes_to_remove' from the pose graph and the optimization + // problem. + for (const NodeId& node_id : nodes_to_remove) { + parent_->data_.trajectory_nodes.Trim(node_id); + parent_->optimization_problem_->TrimTrajectoryNode(node_id); + } +} + +MapById +PoseGraph3D::GetSubmapDataUnderLock() const { + MapById submaps; + for (const auto& submap_id_data : data_.submap_data) { + submaps.Insert(submap_id_data.id, + GetSubmapDataUnderLock(submap_id_data.id)); + } + return submaps; +} + +void PoseGraph3D::SetGlobalSlamOptimizationCallback( + PoseGraphInterface::GlobalSlamOptimizationCallback callback) { + global_slam_optimization_callback_ = callback; +} + +void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) { + auto* latency = family_factory->NewGaugeFamily( + "mapping_3d_pose_graph_work_queue_delay", + "Age of the oldest entry in the work queue in seconds"); + kWorkQueueDelayMetric = latency->Add({}); + auto* queue_size = + family_factory->NewGaugeFamily("mapping_3d_pose_graph_work_queue_size", + "Number of items in the work queue"); + kWorkQueueSizeMetric = queue_size->Add({}); + auto* constraints = family_factory->NewGaugeFamily( + "mapping_3d_pose_graph_constraints", + "Current number of constraints in the pose graph"); + kConstraintsDifferentTrajectoryMetric = + constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}}); + kConstraintsSameTrajectoryMetric = + constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}}); + auto* submaps = family_factory->NewGaugeFamily( + "mapping_3d_pose_graph_submaps", "Number of submaps in the pose graph."); + kActiveSubmapsMetric = submaps->Add({{"state", "active"}}); + kFrozenSubmapsMetric = submaps->Add({{"state", "frozen"}}); + kDeletedSubmapsMetric = submaps->Add({{"state", "deleted"}}); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d.h new file mode 100644 index 0000000..12a3469 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -0,0 +1,302 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_3D_POSE_GRAPH_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_3D_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/container/flat_hash_map.h" +#include "absl/synchronization/mutex.h" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" +#include "cartographer/mapping/internal/trajectory_connectivity_state.h" +#include "cartographer/mapping/internal/pose_graph_data.h" +#include "cartographer/mapping/internal/work_queue.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph_trimmer.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +// Implements the loop closure method called Sparse Pose Adjustment (SPA) from +// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." +// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference +// on (pp. 22--29). IEEE, 2010. +// +// It is extended for submapping in 3D: +// Each node has been matched against one or more submaps (adding a constraint +// for each match), both poses of nodes and of submaps are to be optimized. +// All constraints are between a submap i and a node j. +class PoseGraph3D : public PoseGraph { + public: + PoseGraph3D( + const proto::PoseGraphOptions& options, + std::unique_ptr optimization_problem, + common::ThreadPool* thread_pool); + ~PoseGraph3D() override; + + PoseGraph3D(const PoseGraph3D&) = delete; + PoseGraph3D& operator=(const PoseGraph3D&) = delete; + + // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was + // determined by scan matching against 'insertion_submaps.front()' and the + // node data was inserted into the 'insertion_submaps'. If + // 'insertion_submaps.front().finished()' is 'true', data was inserted into + // this submap for the last time. + NodeId AddNode( + std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& insertion_submaps) + LOCKS_EXCLUDED(mutex_); + + void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override + LOCKS_EXCLUDED(mutex_); + void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) override + LOCKS_EXCLUDED(mutex_); + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) override + LOCKS_EXCLUDED(mutex_); + void AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) override + LOCKS_EXCLUDED(mutex_); + + void DeleteTrajectory(int trajectory_id) override; + void FinishTrajectory(int trajectory_id) override; + bool IsTrajectoryFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void FreezeTrajectory(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, + const proto::Submap& submap) override; + void AddNodeFromProto(const transform::Rigid3d& global_pose, + const proto::Node& node) override; + void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override; + void AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) override; + void AddSerializedConstraints( + const std::vector& constraints) override; + void AddTrimmer(std::unique_ptr trimmer) override; + void RunFinalOptimization() override; + std::vector> GetConnectedTrajectories() const override + LOCKS_EXCLUDED(mutex_); + PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const + LOCKS_EXCLUDED(mutex_) override; + MapById GetAllSubmapData() const + LOCKS_EXCLUDED(mutex_) override; + MapById GetAllSubmapPoses() const + LOCKS_EXCLUDED(mutex_) override; + transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const + LOCKS_EXCLUDED(mutex_) override; + MapById GetTrajectoryNodes() const override + LOCKS_EXCLUDED(mutex_); + MapById GetTrajectoryNodePoses() const override + LOCKS_EXCLUDED(mutex_); + std::map GetTrajectoryStates() const override + LOCKS_EXCLUDED(mutex_); + std::map GetLandmarkPoses() const override + LOCKS_EXCLUDED(mutex_); + void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen = false) override + LOCKS_EXCLUDED(mutex_); + sensor::MapByTime GetImuData() const override + LOCKS_EXCLUDED(mutex_); + sensor::MapByTime GetOdometryData() const override + LOCKS_EXCLUDED(mutex_); + sensor::MapByTime GetFixedFramePoseData() + const override LOCKS_EXCLUDED(mutex_); + std::map + GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_); + std::map GetTrajectoryData() const override; + + std::vector constraints() const override LOCKS_EXCLUDED(mutex_); + void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) override + LOCKS_EXCLUDED(mutex_); + void SetGlobalSlamOptimizationCallback( + PoseGraphInterface::GlobalSlamOptimizationCallback callback) override; + transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( + int trajectory_id, const common::Time time) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + protected: + // Waits until we caught up (i.e. nothing is waiting to be scheduled), and + // all computations have finished. + void WaitForAllComputations() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(work_queue_mutex_); + + private: + MapById GetSubmapDataUnderLock() const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Handles a new work item. + void AddWorkItem(const std::function& work_item) + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_); + + // Adds connectivity and sampler for a trajectory if it does not exist. + void AddTrajectoryIfNeeded(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Appends the new node and submap (if needed) to the internal data stuctures. + NodeId AppendNode( + std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& insertion_submaps, + const transform::Rigid3d& optimized_pose) LOCKS_EXCLUDED(mutex_); + + // Grows the optimization problem to have an entry for every element of + // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. + std::vector InitializeGlobalSubmapPoses( + int trajectory_id, const common::Time time, + const std::vector>& insertion_submaps) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Adds constraints for a node, and starts scan matching in the background. + WorkItem::Result ComputeConstraintsForNode( + const NodeId& node_id, + std::vector> insertion_submaps, + bool newly_finished_submap) LOCKS_EXCLUDED(mutex_); + + // Computes constraints for a node and submap pair. + void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) + LOCKS_EXCLUDED(mutex_); + + // Deletes trajectories waiting for deletion. Must not be called during + // constraint search. + void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Runs the optimization, executes the trimmers and processes the work queue. + void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result) + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_); + + // Process pending tasks in the work queue on the calling thread, until the + // queue is either empty or an optimization is required. + void DrainWorkQueue() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(work_queue_mutex_); + + // Runs the optimization. Callers have to make sure, that there is only one + // optimization being run at a time. + void RunOptimization() LOCKS_EXCLUDED(mutex_); + + bool CanAddWorkItemModifying(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Computes the local to global map frame transform based on the given + // 'global_submap_poses'. + transform::Rigid3d ComputeLocalToGlobalTransform( + const MapById& global_submap_poses, + int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + common::Time GetLatestNodeTime(const NodeId& node_id, + const SubmapId& submap_id) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Logs histograms for the translational and rotational residual of node + // poses. + void LogResidualHistograms() const EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Updates the trajectory connectivity structure with a new constraint. + void UpdateTrajectoryConnectivity(const Constraint& constraint) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + const proto::PoseGraphOptions options_; + GlobalSlamOptimizationCallback global_slam_optimization_callback_; + mutable absl::Mutex mutex_; + absl::Mutex work_queue_mutex_; + + // If it exists, further work items must be added to this queue, and will be + // considered later. + std::unique_ptr work_queue_ GUARDED_BY(work_queue_mutex_); + + // We globally localize a fraction of the nodes from each trajectory. + absl::flat_hash_map> + global_localization_samplers_ GUARDED_BY(mutex_); + + // Number of nodes added since last loop closure. + int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; + + // Current optimization problem. + std::unique_ptr optimization_problem_; + constraints::ConstraintBuilder3D constraint_builder_; + + // Thread pool used for handling the work queue. + common::ThreadPool* const thread_pool_; + + // List of all trimmers to consult when optimizations finish. + std::vector> trimmers_ GUARDED_BY(mutex_); + + PoseGraphData data_ GUARDED_BY(mutex_); + + // Allows querying and manipulating the pose graph by the 'trimmers_'. The + // 'mutex_' of the pose graph is held while this class is used. + class TrimmingHandle : public Trimmable { + public: + TrimmingHandle(PoseGraph3D* parent); + ~TrimmingHandle() override {} + + int num_submaps(int trajectory_id) const override; + std::vector GetSubmapIds(int trajectory_id) const override; + MapById GetOptimizedSubmapData() const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + const MapById& GetTrajectoryNodes() const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + const std::vector& GetConstraints() const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + void TrimSubmap(const SubmapId& submap_id) + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override; + bool IsFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + + void SetTrajectoryState(int trajectory_id, TrajectoryState state) override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); + + private: + PoseGraph3D* const parent_; + }; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d_test.cc new file mode 100644 index 0000000..0ecc220 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -0,0 +1,381 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" + +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" +#include "google/protobuf/util/message_differencer.h" + +namespace cartographer { +namespace mapping { +namespace { + +using ::cartographer::mapping::optimization::OptimizationProblem3D; +using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions; +using ::cartographer::transform::Rigid3d; + +class MockOptimizationProblem3D : public OptimizationProblem3D { + public: + MockOptimizationProblem3D() + : OptimizationProblem3D(OptimizationProblemOptions{}) {} + ~MockOptimizationProblem3D() override = default; + + MOCK_METHOD3(Solve, + void(const std::vector &, + const std::map &, + const std::map &)); +}; + +class PoseGraph3DForTesting : public PoseGraph3D { + public: + PoseGraph3DForTesting( + const proto::PoseGraphOptions &options, + std::unique_ptr optimization_problem, + common::ThreadPool *thread_pool) + : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {} + + void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); } +}; + +class PoseGraph3DTest : public ::testing::Test { + protected: + PoseGraph3DTest() : thread_pool_(absl::make_unique(1)) {} + + void SetUp() override { + const std::string kPoseGraphLua = R"text( + include "pose_graph.lua" + return POSE_GRAPH)text"; + auto pose_graph_parameters = testing::ResolveLuaParameters(kPoseGraphLua); + pose_graph_options_ = CreatePoseGraphOptions(pose_graph_parameters.get()); + } + + void BuildPoseGraph() { + auto optimization_problem = + absl::make_unique( + pose_graph_options_.optimization_problem_options()); + pose_graph_ = absl::make_unique( + pose_graph_options_, std::move(optimization_problem), + thread_pool_.get()); + } + + void BuildPoseGraphWithFakeOptimization() { + auto optimization_problem = absl::make_unique(); + pose_graph_ = absl::make_unique( + pose_graph_options_, std::move(optimization_problem), + thread_pool_.get()); + } + + proto::PoseGraphOptions pose_graph_options_; + std::unique_ptr thread_pool_; + std::unique_ptr pose_graph_; +}; + +TEST_F(PoseGraph3DTest, Empty) { + BuildPoseGraph(); + pose_graph_->WaitForAllComputations(); + EXPECT_TRUE(pose_graph_->GetAllSubmapData().empty()); + EXPECT_TRUE(pose_graph_->constraints().empty()); + EXPECT_TRUE(pose_graph_->GetConnectedTrajectories().empty()); + EXPECT_TRUE(pose_graph_->GetAllSubmapPoses().empty()); + EXPECT_TRUE(pose_graph_->GetTrajectoryNodes().empty()); + EXPECT_TRUE(pose_graph_->GetTrajectoryNodePoses().empty()); + EXPECT_TRUE(pose_graph_->GetTrajectoryData().empty()); + proto::PoseGraph empty_proto; + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + pose_graph_->ToProto(/*include_unfinished_submaps=*/true), empty_proto)); +} + +TEST_F(PoseGraph3DTest, BasicSerialization) { + BuildPoseGraph(); + proto::PoseGraph proto; + auto fake_node = testing::CreateFakeNode(); + testing::AddToProtoGraph(fake_node, &proto); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node); + auto fake_submap = testing::CreateFakeSubmap3D(); + testing::AddToProtoGraph(fake_submap, &proto); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap); + testing::AddToProtoGraph( + testing::CreateFakeConstraint(fake_node, fake_submap), &proto); + pose_graph_->AddSerializedConstraints(FromProto(proto.constraint())); + testing::AddToProtoGraph( + testing::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto); + pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity()); + pose_graph_->WaitForAllComputations(); + proto::PoseGraph actual_proto = + pose_graph_->ToProto(/*include_unfinished_submaps=*/true); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.constraint(0), actual_proto.constraint(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.trajectory(0), actual_proto.trajectory(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.landmark_poses(0), actual_proto.landmark_poses(0))); + EXPECT_TRUE( + google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto)); +} + +TEST_F(PoseGraph3DTest, SerializationWithUnfinishedSubmaps) { + BuildPoseGraph(); + proto::PoseGraph proto; + + // Create three nodes. + auto fake_node_1 = testing::CreateFakeNode(1, 1); + testing::AddToProtoGraph(fake_node_1, &proto); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_1); + auto fake_node_2 = testing::CreateFakeNode(1, 2); + testing::AddToProtoGraph(fake_node_2, &proto); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_2); + auto fake_node_3 = testing::CreateFakeNode(1, 3); + testing::AddToProtoGraph(fake_node_3, &proto); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_3); + + // Create two submaps: one finished, the other not. + auto fake_submap_1 = testing::CreateFakeSubmap3D(1, 1, true); + testing::AddToProtoGraph(fake_submap_1, &proto); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_1); + auto fake_submap_2 = testing::CreateFakeSubmap3D(1, 2, false); + testing::AddToProtoGraph(fake_submap_2, &proto); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_2); + + // Connect node 1 to submap 1, node 2 to submaps 1 and 2, node 3 to submap 2. + testing::AddToProtoGraph( + testing::CreateFakeConstraint(fake_node_1, fake_submap_1), &proto); + testing::AddToProtoGraph( + testing::CreateFakeConstraint(fake_node_2, fake_submap_1), &proto); + testing::AddToProtoGraph( + testing::CreateFakeConstraint(fake_node_2, fake_submap_2), &proto); + testing::AddToProtoGraph( + testing::CreateFakeConstraint(fake_node_3, fake_submap_2), &proto); + pose_graph_->AddSerializedConstraints(FromProto(proto.constraint())); + + pose_graph_->WaitForAllComputations(); + proto::PoseGraph actual_proto = + pose_graph_->ToProto(/*include_unfinished_submaps=*/false); + EXPECT_EQ(actual_proto.constraint_size(), 2); + EXPECT_EQ(actual_proto.trajectory_size(), 1); + EXPECT_EQ(actual_proto.trajectory(0).node_size(), 3); + EXPECT_EQ(actual_proto.trajectory(0).submap_size(), 1); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.constraint(0), actual_proto.constraint(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.constraint(1), actual_proto.constraint(1))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.trajectory(0).node(1), actual_proto.trajectory(0).node(1))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0))); +} + +TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) { + BuildPoseGraphWithFakeOptimization(); + const int trajectory_id = 2; + const int num_submaps_to_create = 5; + const int num_submaps_to_keep = 3; + const int num_nodes_per_submap = 2; + for (int i = 0; i < num_submaps_to_create; ++i) { + int submap_index = (i < 3) ? 42 + i : 100 + i; + auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); + for (int j = 0; j < num_nodes_per_submap; ++j) { + int node_index = 7 + num_nodes_per_submap * submap_index + j; + auto node = testing::CreateFakeNode(trajectory_id, node_index); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); + proto::PoseGraph proto; + auto constraint = testing::CreateFakeConstraint(node, submap); + // TODO(gaschler): Also remove inter constraints when all references are + // gone. + constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP); + testing::AddToProtoGraph(constraint, &proto); + pose_graph_->AddSerializedConstraints(FromProto(proto.constraint())); + } + } + pose_graph_->AddTrimmer(absl::make_unique( + trajectory_id, num_submaps_to_keep)); + pose_graph_->WaitForAllComputations(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->constraints().size(), + num_nodes_per_submap * num_submaps_to_create); + for (int i = 0; i < 2; ++i) { + pose_graph_->RunFinalOptimization(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->constraints().size(), + num_nodes_per_submap * num_submaps_to_keep); + } +} + +class EvenSubmapTrimmer : public PoseGraphTrimmer { + public: + explicit EvenSubmapTrimmer(int trajectory_id) + : trajectory_id_(trajectory_id) {} + + void Trim(Trimmable *pose_graph) override { + auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); + for (const auto submap_id : submap_ids) { + if (submap_id.submap_index % 2 == 0) { + pose_graph->TrimSubmap(submap_id); + } + } + } + + bool IsFinished() override { return false; } + + private: + int trajectory_id_; +}; + +TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) { + BuildPoseGraphWithFakeOptimization(); + const int trajectory_id = 2; + const int num_submaps_to_keep = 10; + const int num_submaps_to_create = 2 * num_submaps_to_keep; + const int num_nodes_per_submap = 3; + for (int i = 0; i < num_submaps_to_create; ++i) { + int submap_index = 42 + i; + auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); + for (int j = 0; j < num_nodes_per_submap; ++j) { + int node_index = 7 + num_nodes_per_submap * i + j; + auto node = testing::CreateFakeNode(trajectory_id, node_index); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); + proto::PoseGraph proto; + auto constraint = testing::CreateFakeConstraint(node, submap); + constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP); + testing::AddToProtoGraph(constraint, &proto); + pose_graph_->AddSerializedConstraints(FromProto(proto.constraint())); + } + } + pose_graph_->AddTrimmer(absl::make_unique(trajectory_id)); + pose_graph_->WaitForAllComputations(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->constraints().size(), + num_nodes_per_submap * num_submaps_to_create); + for (int i = 0; i < 2; ++i) { + pose_graph_->RunFinalOptimization(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->constraints().size(), + num_nodes_per_submap * num_submaps_to_keep); + } +} + +TEST_F(PoseGraph3DTest, EvenSubmapTrimmerOnFrozenTrajectory) { + BuildPoseGraphWithFakeOptimization(); + const int trajectory_id = 2; + const int num_submaps_to_keep = 10; + const int num_submaps_to_create = 2 * num_submaps_to_keep; + const int num_nodes_per_submap = 3; + for (int i = 0; i < num_submaps_to_create; ++i) { + int submap_index = 42 + i; + auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); + for (int j = 0; j < num_nodes_per_submap; ++j) { + int node_index = 7 + num_nodes_per_submap * i + j; + auto node = testing::CreateFakeNode(trajectory_id, node_index); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); + // This step is normally done by a MapBuilder when loading frozen state. + pose_graph_->AddNodeToSubmap(NodeId{trajectory_id, node_index}, + SubmapId{trajectory_id, submap_index}); + } + } + pose_graph_->FreezeTrajectory(trajectory_id); + pose_graph_->AddTrimmer(absl::make_unique(trajectory_id)); + pose_graph_->WaitForAllComputations(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->constraints().size(), 0); + for (int i = 0; i < 2; ++i) { + pose_graph_->RunFinalOptimization(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->constraints().size(), 0); + } +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/rotation_parameterization.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/rotation_parameterization.h new file mode 100644 index 0000000..4ce0055 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/rotation_parameterization.h @@ -0,0 +1,67 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_PARAMETERIZATION_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_PARAMETERIZATION_H_ + +#include "cartographer/common/math.h" +#include "ceres/jet.h" +#include "ceres/rotation.h" + +namespace cartographer { +namespace mapping { + +struct YawOnlyQuaternionPlus { + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + const T clamped_delta = common::Clamp(delta[0], T(-0.5), T(0.5)); + T q_delta[4]; + q_delta[0] = ceres::sqrt(1. - clamped_delta * clamped_delta); + q_delta[1] = T(0.); + q_delta[2] = T(0.); + q_delta[3] = clamped_delta; + ceres::QuaternionProduct(q_delta, x, x_plus_delta); + return true; + } +}; + +struct ConstantYawQuaternionPlus { + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + const T delta_norm = + ceres::sqrt(common::Pow2(delta[0]) + common::Pow2(delta[1])); + const T sin_delta_over_delta = + delta_norm < 1e-6 ? T(1.) : ceres::sin(delta_norm) / delta_norm; + T q_delta[4]; + q_delta[0] = delta_norm < 1e-6 ? T(1.) : ceres::cos(delta_norm); + q_delta[1] = sin_delta_over_delta * delta[0]; + q_delta[2] = sin_delta_over_delta * delta[1]; + q_delta[3] = T(0.); + // We apply the 'delta' which is interpreted as an angle-axis rotation + // vector in the xy-plane of the submap frame. This way we can align to + // gravity because rotations around the z-axis in the submap frame do not + // change gravity alignment, while disallowing random rotations of the map + // that have nothing to do with gravity alignment (i.e. we disallow steps + // just changing "yaw" of the complete map). + ceres::QuaternionProduct(x, q_delta, x_plus_delta); + return true; + } +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_PARAMETERIZATION_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc new file mode 100644 index 0000000..5aab1fd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc @@ -0,0 +1,160 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/ceres_scan_matcher_3d.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::CeresScanMatcherOptions3D options; + for (int i = 0;; ++i) { + const std::string lua_identifier = + "occupied_space_weight_" + std::to_string(i); + if (!parameter_dictionary->HasKey(lua_identifier)) { + break; + } + options.add_occupied_space_weight( + parameter_dictionary->GetDouble(lua_identifier)); + } + for (int i = 0;; ++i) { + const std::string lua_identifier = + "intensity_cost_function_options_" + std::to_string(i); + if (!parameter_dictionary->HasKey(lua_identifier)) { + break; + } + const auto intensity_cost_function_options_dictionary = + parameter_dictionary->GetDictionary(lua_identifier); + auto* intensity_cost_function_options = + options.add_intensity_cost_function_options(); + intensity_cost_function_options->set_weight( + intensity_cost_function_options_dictionary->GetDouble("weight")); + intensity_cost_function_options->set_huber_scale( + intensity_cost_function_options_dictionary->GetDouble("huber_scale")); + intensity_cost_function_options->set_intensity_threshold( + intensity_cost_function_options_dictionary->GetDouble( + "intensity_threshold")); + } + options.set_translation_weight( + parameter_dictionary->GetDouble("translation_weight")); + options.set_rotation_weight( + parameter_dictionary->GetDouble("rotation_weight")); + options.set_only_optimize_yaw( + parameter_dictionary->GetBool("only_optimize_yaw")); + *options.mutable_ceres_solver_options() = + common::CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("ceres_solver_options").get()); + return options; +} + +CeresScanMatcher3D::CeresScanMatcher3D( + const proto::CeresScanMatcherOptions3D& options) + : options_(options), + ceres_solver_options_( + common::CreateCeresSolverOptions(options.ceres_solver_options())) { + ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; +} + +void CeresScanMatcher3D::Match( + const Eigen::Vector3d& target_translation, + const transform::Rigid3d& initial_pose_estimate, + const std::vector& + point_clouds_and_hybrid_grids, + transform::Rigid3d* const pose_estimate, + ceres::Solver::Summary* const summary) const { + ceres::Problem problem; + optimization::CeresPose ceres_pose( + initial_pose_estimate, nullptr /* translation_parameterization */, + options_.only_optimize_yaw() + ? std::unique_ptr( + absl::make_unique>()) + : std::unique_ptr( + absl::make_unique()), + &problem); + + CHECK_EQ(options_.occupied_space_weight_size(), + point_clouds_and_hybrid_grids.size()); + for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { + CHECK_GT(options_.occupied_space_weight(i), 0.); + const sensor::PointCloud& point_cloud = + *point_clouds_and_hybrid_grids[i].point_cloud; + const HybridGrid& hybrid_grid = + *point_clouds_and_hybrid_grids[i].hybrid_grid; + problem.AddResidualBlock( + OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction( + options_.occupied_space_weight(i) / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, hybrid_grid), + nullptr /* loss function */, ceres_pose.translation(), + ceres_pose.rotation()); + if (point_clouds_and_hybrid_grids[i].intensity_hybrid_grid) { + CHECK_GT(options_.intensity_cost_function_options(i).huber_scale(), 0.); + CHECK_GT(options_.intensity_cost_function_options(i).weight(), 0.); + CHECK_GT( + options_.intensity_cost_function_options(i).intensity_threshold(), 0); + const IntensityHybridGrid& intensity_hybrid_grid = + *point_clouds_and_hybrid_grids[i].intensity_hybrid_grid; + problem.AddResidualBlock( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + options_.intensity_cost_function_options(i).weight() / + std::sqrt(static_cast(point_cloud.size())), + options_.intensity_cost_function_options(i).intensity_threshold(), + point_cloud, intensity_hybrid_grid), + new ceres::HuberLoss( + options_.intensity_cost_function_options(i).huber_scale()), + ceres_pose.translation(), ceres_pose.rotation()); + } + } + + CHECK_GT(options_.translation_weight(), 0.); + problem.AddResidualBlock( + TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction( + options_.translation_weight(), target_translation), + nullptr /* loss function */, ceres_pose.translation()); + CHECK_GT(options_.rotation_weight(), 0.); + problem.AddResidualBlock( + RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction( + options_.rotation_weight(), initial_pose_estimate.rotation()), + nullptr /* loss function */, ceres_pose.rotation()); + + ceres::Solve(ceres_solver_options_, &problem, summary); + + *pose_estimate = ceres_pose.ToRigid(); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h new file mode 100644 index 0000000..17c6066 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h @@ -0,0 +1,70 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( + common::LuaParameterDictionary* parameter_dictionary); + +struct PointCloudAndHybridGridsPointers { + const sensor::PointCloud* point_cloud; + const HybridGrid* hybrid_grid; + const IntensityHybridGrid* intensity_hybrid_grid; // optional +}; + +// This scan matcher uses Ceres to align scans with an existing map. +class CeresScanMatcher3D { + public: + explicit CeresScanMatcher3D(const proto::CeresScanMatcherOptions3D& options); + + CeresScanMatcher3D(const CeresScanMatcher3D&) = delete; + CeresScanMatcher3D& operator=(const CeresScanMatcher3D&) = delete; + + // Aligns 'point_clouds' within the 'hybrid_grids' given an + // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver + // 'summary'. + void Match(const Eigen::Vector3d& target_translation, + const transform::Rigid3d& initial_pose_estimate, + const std::vector& + point_clouds_and_hybrid_grids, + transform::Rigid3d* pose_estimate, + ceres::Solver::Summary* summary) const; + + private: + const proto::CeresScanMatcherOptions3D options_; + ceres::Solver::Options ceres_solver_options_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc new file mode 100644 index 0000000..bc0489f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -0,0 +1,141 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/ceres_scan_matcher_3d.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class CeresScanMatcher3DTest : public ::testing::Test { + protected: + CeresScanMatcher3DTest() + : hybrid_grid_(1.f), + intensity_hybrid_grid_(1.f), + expected_pose_( + transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { + std::vector points; + std::vector intensities; + for (const Eigen::Vector3f& point : + {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), + Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), + Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), + Eigen::Vector3f(-7.f, 3.f, 1.f)}) { + points.push_back({point}); + intensities.push_back(50); + hybrid_grid_.SetProbability( + hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + intensity_hybrid_grid_.AddIntensity( + intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast() * + point), + 50); + } + point_cloud_ = sensor::PointCloud(points, intensities); + + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + occupied_space_weight_0 = 1., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 55, + intensity_threshold = 100, + }, + translation_weight = 0.01, + rotation_weight = 0.1, + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 10, + num_threads = 1, + }, + })text"); + options_ = CreateCeresScanMatcherOptions3D(parameter_dictionary.get()); + ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_)); + } + + void TestFromInitialPose(const transform::Rigid3d& initial_pose) { + transform::Rigid3d pose; + + ceres::Solver::Summary summary; + + IntensityHybridGrid* intensity_hybrid_grid_ptr = + point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_; + + ceres_scan_matcher_->Match( + initial_pose.translation(), initial_pose, + {{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose, + &summary); + EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); + EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); + } + + HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; + transform::Rigid3d expected_pose_; + sensor::PointCloud point_cloud_; + proto::CeresScanMatcherOptions3D options_; + std::unique_ptr ceres_scan_matcher_; +}; + +TEST_F(CeresScanMatcher3DTest, PerfectEstimate) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))); +} + +TEST_F(CeresScanMatcher3DTest, AlongX) { + ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_)); + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.))); +} + +TEST_F(CeresScanMatcher3DTest, AlongZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2))); +} + +TEST_F(CeresScanMatcher3DTest, AlongXYZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2))); +} + +TEST_F(CeresScanMatcher3DTest, FullPoseCorrection) { + // We try to find the rotation around z... + const auto additional_transform = transform::Rigid3d::Rotation( + Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.))); + point_cloud_ = sensor::TransformPointCloud( + point_cloud_, additional_transform.cast()); + expected_pose_ = expected_pose_ * additional_transform.inverse(); + // ...starting initially with rotation around x. + TestFromInitialPose( + transform::Rigid3d(Eigen::Vector3d(-0.95, -0.05, 0.05), + Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.)))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc new file mode 100644 index 0000000..0a0a895 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc @@ -0,0 +1,444 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h" + +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::FastCorrelativeScanMatcherOptions3D +CreateFastCorrelativeScanMatcherOptions3D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::FastCorrelativeScanMatcherOptions3D options; + options.set_branch_and_bound_depth( + parameter_dictionary->GetInt("branch_and_bound_depth")); + options.set_full_resolution_depth( + parameter_dictionary->GetInt("full_resolution_depth")); + options.set_min_rotational_score( + parameter_dictionary->GetDouble("min_rotational_score")); + options.set_min_low_resolution_score( + parameter_dictionary->GetDouble("min_low_resolution_score")); + options.set_linear_xy_search_window( + parameter_dictionary->GetDouble("linear_xy_search_window")); + options.set_linear_z_search_window( + parameter_dictionary->GetDouble("linear_z_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + return options; +} + +PrecomputationGridStack3D::PrecomputationGridStack3D( + const HybridGrid& hybrid_grid, + const proto::FastCorrelativeScanMatcherOptions3D& options) { + CHECK_GE(options.branch_and_bound_depth(), 1); + CHECK_GE(options.full_resolution_depth(), 1); + precomputation_grids_.reserve(options.branch_and_bound_depth()); + precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid)); + Eigen::Array3i last_width = Eigen::Array3i::Ones(); + for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) { + const bool half_resolution = depth >= options.full_resolution_depth(); + const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones()); + const int full_voxels_per_high_resolution_voxel = + 1 << std::max(0, depth - options.full_resolution_depth()); + const Eigen::Array3i shift = (next_width - last_width + + (full_voxels_per_high_resolution_voxel - 1)) / + full_voxels_per_high_resolution_voxel; + precomputation_grids_.push_back( + PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift)); + last_width = next_width; + } +} + +struct DiscreteScan3D { + transform::Rigid3f pose; + // Contains a vector of discretized scans for each 'depth'. + std::vector> cell_indices_per_depth; + float rotational_score; +}; + +struct Candidate3D { + Candidate3D(const int scan_index, const Eigen::Array3i& offset) + : scan_index(scan_index), offset(offset) {} + + static Candidate3D Unsuccessful() { + return Candidate3D(0, Eigen::Array3i::Zero()); + } + + // Index into the discrete scans vectors. + int scan_index; + + // Linear offset from the initial pose in cell indices. For lower resolution + // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth + // block of possibilities. + Eigen::Array3i offset; + + // Score, higher is better. + float score = -std::numeric_limits::infinity(); + + // Score of the low resolution matcher. + float low_resolution_score = 0.f; + + bool operator<(const Candidate3D& other) const { return score < other.score; } + bool operator>(const Candidate3D& other) const { return score > other.score; } +}; + +FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D( + const HybridGrid& hybrid_grid, + const HybridGrid* const low_resolution_hybrid_grid, + const Eigen::VectorXf* rotational_scan_matcher_histogram, + const proto::FastCorrelativeScanMatcherOptions3D& options) + : options_(options), + resolution_(hybrid_grid.resolution()), + width_in_voxels_(hybrid_grid.grid_size()), + precomputation_grid_stack_( + absl::make_unique(hybrid_grid, options)), + low_resolution_hybrid_grid_(low_resolution_hybrid_grid), + rotational_scan_matcher_(rotational_scan_matcher_histogram) {} + +FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {} + +std::unique_ptr +FastCorrelativeScanMatcher3D::Match( + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, + const TrajectoryNode::Data& constant_data, const float min_score) const { + const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( + low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud); + const SearchParameters search_parameters{ + common::RoundToInt(options_.linear_xy_search_window() / resolution_), + common::RoundToInt(options_.linear_z_search_window() / resolution_), + options_.angular_search_window(), &low_resolution_matcher}; + return MatchWithSearchParameters( + search_parameters, global_node_pose.cast(), + global_submap_pose.cast(), + constant_data.high_resolution_point_cloud, + constant_data.rotational_scan_matcher_histogram, + constant_data.gravity_alignment, min_score); +} + +std::unique_ptr +FastCorrelativeScanMatcher3D::MatchFullSubmap( + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation, + const TrajectoryNode::Data& constant_data, const float min_score) const { + float max_point_distance = 0.f; + for (const sensor::RangefinderPoint& point : + constant_data.high_resolution_point_cloud) { + max_point_distance = std::max(max_point_distance, point.position.norm()); + } + const int linear_window_size = + (width_in_voxels_ + 1) / 2 + + common::RoundToInt(max_point_distance / resolution_ + 0.5f); + const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( + low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud); + const SearchParameters search_parameters{ + linear_window_size, linear_window_size, M_PI, &low_resolution_matcher}; + return MatchWithSearchParameters( + search_parameters, + transform::Rigid3f::Rotation(global_node_rotation.cast()), + transform::Rigid3f::Rotation(global_submap_rotation.cast()), + constant_data.high_resolution_point_cloud, + constant_data.rotational_scan_matcher_histogram, + constant_data.gravity_alignment, min_score); +} + +std::unique_ptr +FastCorrelativeScanMatcher3D::MatchWithSearchParameters( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, const float min_score) const { + const std::vector discrete_scans = GenerateDiscreteScans( + search_parameters, point_cloud, rotational_scan_matcher_histogram, + gravity_alignment, global_node_pose, global_submap_pose); + + const std::vector lowest_resolution_candidates = + ComputeLowestResolutionCandidates(search_parameters, discrete_scans); + + const Candidate3D best_candidate = BranchAndBound( + search_parameters, discrete_scans, lowest_resolution_candidates, + precomputation_grid_stack_->max_depth(), min_score); + if (best_candidate.score > min_score) { + return absl::make_unique(Result{ + best_candidate.score, + GetPoseFromCandidate(discrete_scans, best_candidate).cast(), + discrete_scans[best_candidate.scan_index].rotational_score, + best_candidate.low_resolution_score}); + } + return nullptr; +} + +DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose, + const float rotational_score) const { + std::vector> cell_indices_per_depth; + const PrecomputationGrid3D& original_grid = + precomputation_grid_stack_->Get(0); + std::vector full_resolution_cell_indices; + for (const sensor::RangefinderPoint& point : + sensor::TransformPointCloud(point_cloud, pose)) { + full_resolution_cell_indices.push_back( + original_grid.GetCellIndex(point.position)); + } + const int full_resolution_depth = std::min(options_.full_resolution_depth(), + options_.branch_and_bound_depth()); + CHECK_GE(full_resolution_depth, 1); + for (int i = 0; i != full_resolution_depth; ++i) { + cell_indices_per_depth.push_back(full_resolution_cell_indices); + } + const int low_resolution_depth = + options_.branch_and_bound_depth() - full_resolution_depth; + CHECK_GE(low_resolution_depth, 0); + const Eigen::Array3i search_window_start( + -search_parameters.linear_xy_window_size, + -search_parameters.linear_xy_window_size, + -search_parameters.linear_z_window_size); + for (int i = 0; i != low_resolution_depth; ++i) { + const int reduction_exponent = i + 1; + const Eigen::Array3i low_resolution_search_window_start( + search_window_start[0] >> reduction_exponent, + search_window_start[1] >> reduction_exponent, + search_window_start[2] >> reduction_exponent); + cell_indices_per_depth.emplace_back(); + for (const Eigen::Array3i& cell_index : full_resolution_cell_indices) { + const Eigen::Array3i cell_at_start = cell_index + search_window_start; + const Eigen::Array3i low_resolution_cell_at_start( + cell_at_start[0] >> reduction_exponent, + cell_at_start[1] >> reduction_exponent, + cell_at_start[2] >> reduction_exponent); + cell_indices_per_depth.back().push_back( + low_resolution_cell_at_start - low_resolution_search_window_start); + } + } + return DiscreteScan3D{pose, cell_indices_per_depth, rotational_score}; +} + +std::vector FastCorrelativeScanMatcher3D::GenerateDiscreteScans( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose) const { + std::vector result; + // We set this value to something on the order of resolution to make sure that + // the std::acos() below is defined. + float max_scan_range = 3.f * resolution_; + for (const sensor::RangefinderPoint& point : point_cloud) { + const float range = point.position.norm(); + max_scan_range = std::max(range, max_scan_range); + } + const float kSafetyMargin = 1.f - 1e-2f; + const float angular_step_size = + kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) / + (2.f * common::Pow2(max_scan_range))); + const int angular_window_size = common::RoundToInt( + search_parameters.angular_search_window / angular_step_size); + std::vector angles; + for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { + angles.push_back(rz * angular_step_size); + } + const transform::Rigid3f node_to_submap = + global_submap_pose.inverse() * global_node_pose; + const std::vector scores = rotational_scan_matcher_.Match( + rotational_scan_matcher_histogram, + transform::GetYaw(node_to_submap.rotation() * + gravity_alignment.inverse().cast()), + angles); + for (size_t i = 0; i != angles.size(); ++i) { + if (scores[i] < options_.min_rotational_score()) { + continue; + } + const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]); + // It's important to apply the 'angle_axis' rotation between the translation + // and rotation of the 'initial_pose', so that the rotation is around the + // origin of the range data, and yaw is in map frame. + const transform::Rigid3f pose( + node_to_submap.translation(), + global_submap_pose.rotation().inverse() * + transform::AngleAxisVectorToRotationQuaternion(angle_axis) * + global_node_pose.rotation()); + result.push_back( + DiscretizeScan(search_parameters, point_cloud, pose, scores[i])); + } + return result; +} + +std::vector +FastCorrelativeScanMatcher3D::GenerateLowestResolutionCandidates( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const int num_discrete_scans) const { + const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); + const int num_lowest_resolution_linear_xy_candidates = + (2 * search_parameters.linear_xy_window_size + linear_step_size) / + linear_step_size; + const int num_lowest_resolution_linear_z_candidates = + (2 * search_parameters.linear_z_window_size + linear_step_size) / + linear_step_size; + const int num_candidates = + num_discrete_scans * + common::Power(num_lowest_resolution_linear_xy_candidates, 2) * + num_lowest_resolution_linear_z_candidates; + std::vector candidates; + candidates.reserve(num_candidates); + for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) { + for (int z = -search_parameters.linear_z_window_size; + z <= search_parameters.linear_z_window_size; z += linear_step_size) { + for (int y = -search_parameters.linear_xy_window_size; + y <= search_parameters.linear_xy_window_size; + y += linear_step_size) { + for (int x = -search_parameters.linear_xy_window_size; + x <= search_parameters.linear_xy_window_size; + x += linear_step_size) { + candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z)); + } + } + } + } + CHECK_EQ(candidates.size(), num_candidates); + return candidates; +} + +void FastCorrelativeScanMatcher3D::ScoreCandidates( + const int depth, const std::vector& discrete_scans, + std::vector* const candidates) const { + const int reduction_exponent = + std::max(0, depth - options_.full_resolution_depth() + 1); + for (Candidate3D& candidate : *candidates) { + int sum = 0; + const DiscreteScan3D& discrete_scan = discrete_scans[candidate.scan_index]; + const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent, + candidate.offset[1] >> reduction_exponent, + candidate.offset[2] >> reduction_exponent); + CHECK_LT(depth, discrete_scan.cell_indices_per_depth.size()); + for (const Eigen::Array3i& cell_index : + discrete_scan.cell_indices_per_depth[depth]) { + const Eigen::Array3i proposed_cell_index = cell_index + offset; + sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index); + } + candidate.score = PrecomputationGrid3D::ToProbability( + sum / + static_cast(discrete_scan.cell_indices_per_depth[depth].size())); + } + std::sort(candidates->begin(), candidates->end(), + std::greater()); +} + +std::vector +FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const std::vector& discrete_scans) const { + std::vector lowest_resolution_candidates = + GenerateLowestResolutionCandidates(search_parameters, + discrete_scans.size()); + ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans, + &lowest_resolution_candidates); + return lowest_resolution_candidates; +} + +transform::Rigid3f FastCorrelativeScanMatcher3D::GetPoseFromCandidate( + const std::vector& discrete_scans, + const Candidate3D& candidate) const { + return transform::Rigid3f::Translation( + resolution_ * candidate.offset.matrix().cast()) * + discrete_scans[candidate.scan_index].pose; +} + +Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const std::vector& discrete_scans, + const std::vector& candidates, const int candidate_depth, + float min_score) const { + if (candidate_depth == 0) { + for (const Candidate3D& candidate : candidates) { + if (candidate.score <= min_score) { + // Return if the candidate is bad because the following candidate will + // not have better score. + return Candidate3D::Unsuccessful(); + } + const float low_resolution_score = + (*search_parameters.low_resolution_matcher)( + GetPoseFromCandidate(discrete_scans, candidate)); + if (low_resolution_score >= options_.min_low_resolution_score()) { + // We found the best candidate that passes the matching function. + Candidate3D best_candidate = candidate; + best_candidate.low_resolution_score = low_resolution_score; + return best_candidate; + } + } + + // All candidates have good scores but none passes the matching function. + return Candidate3D::Unsuccessful(); + } + + Candidate3D best_high_resolution_candidate = Candidate3D::Unsuccessful(); + best_high_resolution_candidate.score = min_score; + for (const Candidate3D& candidate : candidates) { + if (candidate.score <= min_score) { + break; + } + std::vector higher_resolution_candidates; + const int half_width = 1 << (candidate_depth - 1); + for (int z : {0, half_width}) { + if (candidate.offset.z() + z > search_parameters.linear_z_window_size) { + break; + } + for (int y : {0, half_width}) { + if (candidate.offset.y() + y > + search_parameters.linear_xy_window_size) { + break; + } + for (int x : {0, half_width}) { + if (candidate.offset.x() + x > + search_parameters.linear_xy_window_size) { + break; + } + higher_resolution_candidates.emplace_back( + candidate.scan_index, candidate.offset + Eigen::Array3i(x, y, z)); + } + } + } + ScoreCandidates(candidate_depth - 1, discrete_scans, + &higher_resolution_candidates); + best_high_resolution_candidate = std::max( + best_high_resolution_candidate, + BranchAndBound(search_parameters, discrete_scans, + higher_resolution_candidates, candidate_depth - 1, + best_high_resolution_candidate.score)); + } + return best_high_resolution_candidate; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h new file mode 100644 index 0000000..9af9578 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h @@ -0,0 +1,157 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// This is an implementation of a 3D branch-and-bound algorithm similar to +// FastCorrelativeScanMatcher2D. + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" +#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::FastCorrelativeScanMatcherOptions3D +CreateFastCorrelativeScanMatcherOptions3D( + common::LuaParameterDictionary* parameter_dictionary); + +class PrecomputationGridStack3D { + public: + PrecomputationGridStack3D( + const HybridGrid& hybrid_grid, + const proto::FastCorrelativeScanMatcherOptions3D& options); + + const PrecomputationGrid3D& Get(int depth) const { + return precomputation_grids_.at(depth); + } + + int max_depth() const { return precomputation_grids_.size() - 1; } + + private: + std::vector precomputation_grids_; +}; + +struct DiscreteScan3D; +struct Candidate3D; + +// Used to compute scores between 0 and 1 how well the given pose matches. +using MatchingFunction = std::function; + +class FastCorrelativeScanMatcher3D { + public: + struct Result { + float score; + transform::Rigid3d pose_estimate; + float rotational_score; + float low_resolution_score; + }; + + FastCorrelativeScanMatcher3D( + const HybridGrid& hybrid_grid, + const HybridGrid* low_resolution_hybrid_grid, + const Eigen::VectorXf* rotational_scan_matcher_histogram, + const proto::FastCorrelativeScanMatcherOptions3D& options); + ~FastCorrelativeScanMatcher3D(); + + FastCorrelativeScanMatcher3D(const FastCorrelativeScanMatcher3D&) = delete; + FastCorrelativeScanMatcher3D& operator=(const FastCorrelativeScanMatcher3D&) = + delete; + + // Aligns the node with the given 'constant_data' within the 'hybrid_grid' + // given 'global_node_pose' and 'global_submap_pose'. 'Result' is only + // returned if a score above 'min_score' (excluding equality) is possible. + std::unique_ptr Match(const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, + const TrajectoryNode::Data& constant_data, + float min_score) const; + + // Aligns the node with the given 'constant_data' within the 'hybrid_grid' + // given rotations which are expected to be approximately gravity aligned. + // 'Result' is only returned if a score above 'min_score' (excluding equality) + // is possible. + std::unique_ptr MatchFullSubmap( + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation, + const TrajectoryNode::Data& constant_data, float min_score) const; + + private: + struct SearchParameters { + const int linear_xy_window_size; // voxels + const int linear_z_window_size; // voxels + const double angular_search_window; // radians + const MatchingFunction* const low_resolution_matcher; + }; + + std::unique_ptr MatchWithSearchParameters( + const SearchParameters& search_parameters, + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, float min_score) const; + DiscreteScan3D DiscretizeScan(const SearchParameters& search_parameters, + const sensor::PointCloud& point_cloud, + const transform::Rigid3f& pose, + float rotational_score) const; + std::vector GenerateDiscreteScans( + const SearchParameters& search_parameters, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose) const; + std::vector GenerateLowestResolutionCandidates( + const SearchParameters& search_parameters, int num_discrete_scans) const; + void ScoreCandidates(int depth, + const std::vector& discrete_scans, + std::vector* const candidates) const; + std::vector ComputeLowestResolutionCandidates( + const SearchParameters& search_parameters, + const std::vector& discrete_scans) const; + Candidate3D BranchAndBound(const SearchParameters& search_parameters, + const std::vector& discrete_scans, + const std::vector& candidates, + int candidate_depth, float min_score) const; + transform::Rigid3f GetPoseFromCandidate( + const std::vector& discrete_scans, + const Candidate3D& candidate) const; + + const proto::FastCorrelativeScanMatcherOptions3D options_; + const float resolution_; + const int width_in_voxels_; + std::unique_ptr precomputation_grid_stack_; + const HybridGrid* const low_resolution_hybrid_grid_; + RotationalScanMatcher rotational_scan_matcher_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc new file mode 100644 index 0000000..df610e3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -0,0 +1,209 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h" + +#include +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping/3d/range_data_inserter_3d.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class FastCorrelativeScanMatcher3DTest : public ::testing::Test { + protected: + FastCorrelativeScanMatcher3DTest() + : range_data_inserter_(CreateRangeDataInserterTestOptions3D()), + options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {} + + void SetUp() override { + point_cloud_ = sensor::PointCloud({{Eigen::Vector3f(4.f, 0.f, 0.f)}, + {Eigen::Vector3f(4.5f, 0.f, 0.f)}, + {Eigen::Vector3f(5.f, 0.f, 0.f)}, + {Eigen::Vector3f(5.5f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.5f, 0.f)}, + {Eigen::Vector3f(0.f, 5.f, 0.f)}, + {Eigen::Vector3f(0.f, 5.5f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.5f)}, + {Eigen::Vector3f(0.f, 0.f, 5.f)}, + {Eigen::Vector3f(0.f, 0.f, 5.5f)}}); + } + + transform::Rigid3f GetRandomPose() { + const float x = 0.7f * distribution_(prng_); + const float y = 0.7f * distribution_(prng_); + const float z = 0.7f * distribution_(prng_); + const float theta = 0.2f * distribution_(prng_); + return transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) * + transform::Rigid3f::Rotation( + Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); + } + + static proto::FastCorrelativeScanMatcherOptions3D + CreateFastCorrelativeScanMatcher3DTestOptions3D( + const int branch_and_bound_depth) { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "branch_and_bound_depth = " + + std::to_string(branch_and_bound_depth) + + ", " + "full_resolution_depth = " + + std::to_string(branch_and_bound_depth) + + ", " + "min_rotational_score = 0.1, " + // Unknown space has kMinProbability = 0.1, so we need to make sure here + // to pick a larger number otherwise we always find matches. + "min_low_resolution_score = 0.15, " + "linear_xy_search_window = 0.8, " + "linear_z_search_window = 0.8, " + "angular_search_window = 0.3, " + "}"); + return CreateFastCorrelativeScanMatcherOptions3D( + parameter_dictionary.get()); + } + + static mapping::proto::RangeDataInserterOptions3D + CreateRangeDataInserterTestOptions3D() { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "num_free_space_voxels = 5, " + "intensity_threshold = 100.0, " + "}"); + return CreateRangeDataInserterOptions3D(parameter_dictionary.get()); + } + + std::unique_ptr GetFastCorrelativeScanMatcher( + const proto::FastCorrelativeScanMatcherOptions3D& options, + const transform::Rigid3f& pose) { + hybrid_grid_ = absl::make_unique(0.05f); + range_data_inserter_.Insert( + sensor::RangeData{pose.translation(), + sensor::TransformPointCloud(point_cloud_, pose), + {}}, + hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); + hybrid_grid_->FinishUpdate(); + + return absl::make_unique( + *hybrid_grid_, hybrid_grid_.get(), &GetRotationalScanMatcherHistogram(), + options); + } + + TrajectoryNode::Data CreateConstantData( + const sensor::PointCloud& low_resolution_point_cloud) { + return TrajectoryNode::Data{common::FromUniversal(0), + Eigen::Quaterniond::Identity(), + {}, + point_cloud_, + low_resolution_point_cloud, + GetRotationalScanMatcherHistogram()}; + } + + const Eigen::VectorXf& GetRotationalScanMatcherHistogram() { + return rotational_scan_matcher_histogram_; + } + + std::mt19937 prng_ = std::mt19937(42); + std::uniform_real_distribution distribution_ = + std::uniform_real_distribution(-1.f, 1.f); + RangeDataInserter3D range_data_inserter_; + const proto::FastCorrelativeScanMatcherOptions3D options_; + sensor::PointCloud point_cloud_; + std::unique_ptr hybrid_grid_; + const Eigen::VectorXf rotational_scan_matcher_histogram_ = + Eigen::VectorXf::Zero(10); +}; + +constexpr float kMinScore = 0.1f; + +TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) { + for (int i = 0; i != 20; ++i) { + const auto expected_pose = GetRandomPose(); + + std::unique_ptr fast_correlative_scan_matcher( + GetFastCorrelativeScanMatcher(options_, expected_pose)); + + const std::unique_ptr result = + fast_correlative_scan_matcher->Match( + transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), + CreateConstantData(point_cloud_), kMinScore); + EXPECT_THAT(result, testing::NotNull()); + EXPECT_LT(kMinScore, result->score); + EXPECT_LT(0.09f, result->rotational_score); + EXPECT_LT(0.14f, result->low_resolution_score); + EXPECT_THAT(expected_pose, + transform::IsNearly(result->pose_estimate.cast(), 0.05f)) + << "Actual: " << transform::ToProto(result->pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + + const std::unique_ptr + low_resolution_result = fast_correlative_scan_matcher->Match( + transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), + CreateConstantData( + sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})), + kMinScore); + EXPECT_THAT(low_resolution_result, testing::IsNull()) + << low_resolution_result->low_resolution_score; + } +} + +TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) { + const auto expected_pose = GetRandomPose(); + + std::unique_ptr fast_correlative_scan_matcher( + GetFastCorrelativeScanMatcher(options_, expected_pose)); + + const std::unique_ptr result = + fast_correlative_scan_matcher->MatchFullSubmap( + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), + CreateConstantData(point_cloud_), kMinScore); + EXPECT_THAT(result, testing::NotNull()); + EXPECT_LT(kMinScore, result->score); + EXPECT_LT(0.09f, result->rotational_score); + EXPECT_LT(0.14f, result->low_resolution_score); + EXPECT_THAT(expected_pose, + transform::IsNearly(result->pose_estimate.cast(), 0.05f)) + << "Actual: " << transform::ToProto(result->pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + + const std::unique_ptr + low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), + CreateConstantData( + sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})), + kMinScore); + EXPECT_THAT(low_resolution_result, testing::IsNull()) + << low_resolution_result->low_resolution_score; +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc new file mode 100644 index 0000000..fe16ff5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc @@ -0,0 +1,25 @@ +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// This method is defined here instead of the header file as it was observed +// that defining it in the header file has a negative impact on the runtime +// performance. +ceres::CostFunction* IntensityCostFunction3D::CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) { + CHECK(!point_cloud.intensities().empty()); + return new ceres::AutoDiffCostFunction< + IntensityCostFunction3D, ceres::DYNAMIC /* residuals */, + 3 /* translation variables */, 4 /* rotation variables */>( + new IntensityCostFunction3D(scaling_factor, intensity_threshold, + point_cloud, hybrid_grid), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h new file mode 100644 index 0000000..59f1c29 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h @@ -0,0 +1,98 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a +// 'translation' and 'rotation'. The cost increases when points fall into space +// for which different intensity has been observed, i.e. at voxels with different +// values. Only points up to a certain threshold are evaluated which is intended +// to ignore data from retroreflections. +class IntensityCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid); + + template + bool operator()(const T* const translation, const T* const rotation, + T* const residual) const { + const transform::Rigid3 transform( + Eigen::Map>(translation), + Eigen::Quaternion(rotation[0], rotation[1], rotation[2], + rotation[3])); + return Evaluate(transform, residual); + } + + private: + IntensityCostFunction3D(const double scaling_factor, + const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) + : scaling_factor_(scaling_factor), + intensity_threshold_(intensity_threshold), + point_cloud_(point_cloud), + interpolated_grid_(hybrid_grid) {} + + IntensityCostFunction3D(const IntensityCostFunction3D&) = delete; + IntensityCostFunction3D& operator=(const IntensityCostFunction3D&) = delete; + + template + bool Evaluate(const transform::Rigid3& transform, + T* const residual) const { + for (size_t i = 0; i < point_cloud_.size(); ++i) { + if (point_cloud_.intensities()[i] > intensity_threshold_) { + residual[i] = T(0.f); + } else { + const Eigen::Matrix point = point_cloud_[i].position.cast(); + const T intensity = T(point_cloud_.intensities()[i]); + + const Eigen::Matrix world = transform * point; + const T interpolated_intensity = + interpolated_grid_.GetInterpolatedValue(world[0], world[1], + world[2]); + residual[i] = scaling_factor_ * (interpolated_intensity - intensity); + } + } + return true; + } + + const double scaling_factor_; + // We will ignore returns with intensity above this threshold. + const float intensity_threshold_; + const sensor::PointCloud& point_cloud_; + const InterpolatedIntensityGrid interpolated_grid_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc new file mode 100644 index 0000000..90fc8c4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/intensity_cost_function_3d.h" + +#include +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleNear; +using ::testing::ElementsAre; + +TEST(IntensityCostFunction3DTest, SmokeTest) { + const sensor::PointCloud point_cloud( + {{{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}, + {50.f, 100.f, 150.f}); + IntensityHybridGrid hybrid_grid(0.3f); + hybrid_grid.AddIntensity(hybrid_grid.GetCellIndex({0.f, 0.f, 0.f}), 50.f); + + std::unique_ptr cost_function( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + /*scaling_factor=*/1.0f, /*intensity_threshold=*/100.f, point_cloud, + hybrid_grid)); + + const std::array translation{{0., 0., 0.}}; + const std::array rotation{{1., 0., 0., 0.}}; + + const std::array parameter_blocks{ + {translation.data(), rotation.data()}}; + std::array residuals; + + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + /*jacobians=*/nullptr); + EXPECT_THAT(residuals, + ElementsAre(DoubleNear(0., 1e-9), DoubleNear(-100., 1e-9), + DoubleNear(0., 1e-9))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h new file mode 100644 index 0000000..b38fdbe --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h @@ -0,0 +1,169 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ + +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Interpolates between HybridGrid voxels. We use the tricubic +// interpolation which interpolates the values and has vanishing derivative at +// these points. +// +// This class is templated to work with the autodiff that Ceres provides. +// For this reason, it is also important that the interpolation scheme be +// continuously differentiable. +template +class InterpolatedGrid { + public: + explicit InterpolatedGrid(const HybridGridType& hybrid_grid) + : hybrid_grid_(hybrid_grid) {} + + InterpolatedGrid(const InterpolatedGrid&) = delete; + InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; + + // Returns the interpolated value at (x, y, z) of the HybridGrid + // used to perform the interpolation. + // + // This is a piecewise, continuously differentiable function. We use the + // scalar part of Jet parameters to select our interval below. It is the + // tensor product volume of piecewise cubic polynomials that interpolate + // the values, and have vanishing derivative at the interval boundaries. + template + T GetInterpolatedValue(const T& x, const T& y, const T& z) const { + double x1, y1, z1, x2, y2, z2; + ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2); + + const Eigen::Array3i index1 = + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1)); + const double q111 = GetValue(hybrid_grid_, index1); + const double q112 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1)); + const double q121 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0)); + const double q122 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1)); + const double q211 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0)); + const double q212 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1)); + const double q221 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0)); + const double q222 = + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1)); + + const T normalized_x = (x - x1) / (x2 - x1); + const T normalized_y = (y - y1) / (y2 - y1); + const T normalized_z = (z - z1) / (z2 - z1); + + // Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive. + const T normalized_xx = normalized_x * normalized_x; + const T normalized_xxx = normalized_x * normalized_xx; + const T normalized_yy = normalized_y * normalized_y; + const T normalized_yyy = normalized_y * normalized_yy; + const T normalized_zz = normalized_z * normalized_z; + const T normalized_zzz = normalized_z * normalized_zz; + + // We first interpolate in z, then y, then x. All 7 times this uses the same + // scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2). + // The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0 + // at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1. + const T q11 = (q111 - q112) * normalized_zzz * 2. + + (q112 - q111) * normalized_zz * 3. + q111; + const T q12 = (q121 - q122) * normalized_zzz * 2. + + (q122 - q121) * normalized_zz * 3. + q121; + const T q21 = (q211 - q212) * normalized_zzz * 2. + + (q212 - q211) * normalized_zz * 3. + q211; + const T q22 = (q221 - q222) * normalized_zzz * 2. + + (q222 - q221) * normalized_zz * 3. + q221; + const T q1 = (q11 - q12) * normalized_yyy * 2. + + (q12 - q11) * normalized_yy * 3. + q11; + const T q2 = (q21 - q22) * normalized_yyy * 2. + + (q22 - q21) * normalized_yy * 3. + q21; + return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. + + q1; + } + + private: + template + void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z, + double* x1, double* y1, double* z1, + double* x2, double* y2, + double* z2) const { + const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z); + *x1 = lower.x(); + *y1 = lower.y(); + *z1 = lower.z(); + *x2 = lower.x() + hybrid_grid_.resolution(); + *y2 = lower.y() + hybrid_grid_.resolution(); + *z2 = lower.z() + hybrid_grid_.resolution(); + } + + // Center of the next lower voxel, i.e., not necessarily the voxel containing + // (x, y, z). For each dimension, the largest voxel index so that the + // corresponding center is at most the given coordinate. + Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y, + const double z) const { + // Center of the cell containing (x, y, z). + Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + // Move to the next lower voxel center. + if (center.x() > x) { + center.x() -= hybrid_grid_.resolution(); + } + if (center.y() > y) { + center.y() -= hybrid_grid_.resolution(); + } + if (center.z() > z) { + center.z() -= hybrid_grid_.resolution(); + } + return center; + } + + // Uses the scalar part of a Ceres Jet. + template + Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y, + const T& jet_z) const { + return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); + } + + static float GetValue(const HybridGrid& probability_grid, + const Eigen::Array3i& index) { + return probability_grid.GetProbability(index); + } + + static float GetValue(const IntensityHybridGrid& intensity_grid, + const Eigen::Array3i& index) { + return intensity_grid.GetIntensity(index); + } + + const HybridGridType& hybrid_grid_; +}; + +using InterpolatedIntensityGrid = InterpolatedGrid; +using InterpolatedProbabilityGrid = InterpolatedGrid; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc new file mode 100644 index 0000000..503b2ec --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc @@ -0,0 +1,90 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/interpolated_grid.h" + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class InterpolatedGridTest : public ::testing::Test { + protected: + InterpolatedGridTest() + : hybrid_grid_(0.1f), interpolated_grid_(hybrid_grid_) { + for (const Eigen::Vector3f& point : + {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), + Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), + Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), + Eigen::Vector3f(-7.f, 3.f, 1.f)}) { + hybrid_grid_.SetProbability(hybrid_grid_.GetCellIndex(point), 1.); + } + } + + float GetHybridGridProbability(float x, float y, float z) const { + return hybrid_grid_.GetProbability( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + HybridGrid hybrid_grid_; + InterpolatedProbabilityGrid interpolated_grid_; +}; + +TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { + for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) { + for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { + for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { + EXPECT_NEAR(GetHybridGridProbability(x, y, z), + interpolated_grid_.GetInterpolatedValue(x, y, z), 1e-6); + } + } + } +} + +TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { + const double kSampleStep = hybrid_grid_.resolution() / 10.; + for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) { + for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { + for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { + const float start_probability = GetHybridGridProbability(x, y, z); + const float next_probability = + GetHybridGridProbability(x + hybrid_grid_.resolution(), y, z); + const float grid_difference = next_probability - start_probability; + if (std::abs(grid_difference) < 1e-6f) { + continue; + } + for (double sample = kSampleStep; + sample < hybrid_grid_.resolution() - 2 * kSampleStep; + sample += kSampleStep) { + EXPECT_LT(0., + grid_difference * (interpolated_grid_.GetInterpolatedValue( + x + sample + kSampleStep, y, z) - + interpolated_grid_.GetInterpolatedValue( + x + sample, y, z))); + } + } + } + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc new file mode 100644 index 0000000..754e9ed --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc @@ -0,0 +1,39 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/low_resolution_matcher.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +std::function CreateLowResolutionMatcher( + const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { + return [=](const transform::Rigid3f& pose) { + float score = 0.f; + for (const sensor::RangefinderPoint& point : + sensor::TransformPointCloud(*points, pose)) { + // TODO(zhengj, whess): Interpolate the Grid to get better score. + score += low_resolution_grid->GetProbability( + low_resolution_grid->GetCellIndex(point.position)); + } + return score / points->size(); + }; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h new file mode 100644 index 0000000..4c2fe48 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ + +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +std::function CreateLowResolutionMatcher( + const HybridGrid* low_resolution_grid, const sensor::PointCloud* points); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h new file mode 100644 index 0000000..ff5b653 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h @@ -0,0 +1,91 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a +// 'translation' and 'rotation'. The cost increases when points fall into less +// occupied space, i.e. at voxels with lower values. +class OccupiedSpaceCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const mapping::HybridGrid& hybrid_grid) { + return new ceres::AutoDiffCostFunction< + OccupiedSpaceCostFunction3D, ceres::DYNAMIC /* residuals */, + 3 /* translation variables */, 4 /* rotation variables */>( + new OccupiedSpaceCostFunction3D(scaling_factor, point_cloud, + hybrid_grid), + point_cloud.size()); + } + + template + bool operator()(const T* const translation, const T* const rotation, + T* const residual) const { + const transform::Rigid3 transform( + Eigen::Map>(translation), + Eigen::Quaternion(rotation[0], rotation[1], rotation[2], + rotation[3])); + return Evaluate(transform, residual); + } + + private: + OccupiedSpaceCostFunction3D(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const mapping::HybridGrid& hybrid_grid) + : scaling_factor_(scaling_factor), + point_cloud_(point_cloud), + interpolated_grid_(hybrid_grid) {} + + OccupiedSpaceCostFunction3D(const OccupiedSpaceCostFunction3D&) = delete; + OccupiedSpaceCostFunction3D& operator=(const OccupiedSpaceCostFunction3D&) = + delete; + + template + bool Evaluate(const transform::Rigid3& transform, + T* const residual) const { + for (size_t i = 0; i < point_cloud_.size(); ++i) { + const Eigen::Matrix world = + transform * point_cloud_[i].position.cast(); + const T probability = + interpolated_grid_.GetInterpolatedValue(world[0], world[1], world[2]); + residual[i] = scaling_factor_ * (1. - probability); + } + return true; + } + + const double scaling_factor_; + const sensor::PointCloud& point_cloud_; + const InterpolatedProbabilityGrid interpolated_grid_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.cc new file mode 100644 index 0000000..2dec03d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.cc @@ -0,0 +1,85 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/precomputation_grid_3d.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/math.h" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +// C++11 defines that integer division rounds towards zero. For index math, we +// actually need it to round towards negative infinity. Luckily bit shifts have +// that property. +inline int DivideByTwoRoundingTowardsNegativeInfinity(const int value) { + return value >> 1; +} + +// Computes the half resolution index corresponding to the full resolution +// 'cell_index'. +Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) { + return Eigen::Array3i( + DivideByTwoRoundingTowardsNegativeInfinity(cell_index[0]), + DivideByTwoRoundingTowardsNegativeInfinity(cell_index[1]), + DivideByTwoRoundingTowardsNegativeInfinity(cell_index[2])); +} + +} // namespace + +PrecomputationGrid3D ConvertToPrecomputationGrid( + const HybridGrid& hybrid_grid) { + PrecomputationGrid3D result(hybrid_grid.resolution()); + for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { + const int cell_value = common::RoundToInt( + (ValueToProbability(it.GetValue()) - kMinProbability) * + (255.f / (kMaxProbability - kMinProbability))); + CHECK_GE(cell_value, 0); + CHECK_LE(cell_value, 255); + *result.mutable_value(it.GetCellIndex()) = cell_value; + } + return result; +} + +PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid, + const bool half_resolution, + const Eigen::Array3i& shift) { + PrecomputationGrid3D result(grid.resolution()); + for (auto it = PrecomputationGrid3D::Iterator(grid); !it.Done(); it.Next()) { + for (int i = 0; i != 8; ++i) { + // We use this value to update 8 values in the resulting grid, at + // position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}). + // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, + // this results in precomputation grids analogous to the 2D case. + const Eigen::Array3i cell_index = + it.GetCellIndex() - shift * PrecomputationGrid3D::GetOctant(i); + auto* const cell_value = result.mutable_value( + half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index); + *cell_value = std::max(it.GetValue(), *cell_value); + } + } + return result; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h new file mode 100644 index 0000000..c9b3caf --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h @@ -0,0 +1,56 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_ + +#include "cartographer/mapping/3d/hybrid_grid.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +class PrecomputationGrid3D : public HybridGridBase { + public: + explicit PrecomputationGrid3D(const float resolution) + : HybridGridBase(resolution) {} + + // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. + static float ToProbability(float value) { + return kMinProbability + + value * ((kMaxProbability - kMinProbability) / 255.f); + } +}; + +// Converts a HybridGrid to a PrecomputationGrid3D representing the same data, +// but only using 8 bit instead of 2 x 16 bit. +PrecomputationGrid3D ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); + +// Returns a grid of the same resolution containing the maximum value of +// original voxels in 'grid'. This maximum is over the 8 voxels that have +// any combination of index components optionally increased by 'shift'. +// If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this +// is using the precomputed grid of one depth before, this results in +// precomputation grids analogous to the 2D case. +PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid, + bool half_resolution, + const Eigen::Array3i& shift); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d_test.cc new file mode 100644 index 0000000..85e6f29 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d_test.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h" + +#include +#include +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +TEST(PrecomputedGridGenerator3DTest, TestAgainstNaiveAlgorithm) { + HybridGrid hybrid_grid(2.f); + + std::mt19937 rng(23847); + std::uniform_int_distribution coordinate_distribution(-50, 49); + std::uniform_real_distribution value_distribution(kMinProbability, + kMaxProbability); + for (int i = 0; i < 1000; ++i) { + const auto x = coordinate_distribution(rng); + const auto y = coordinate_distribution(rng); + const auto z = coordinate_distribution(rng); + const Eigen::Array3i cell_index(x, y, z); + hybrid_grid.SetProbability(cell_index, value_distribution(rng)); + } + + std::vector precomputed_grids; + for (int depth = 0; depth <= 3; ++depth) { + if (depth == 0) { + precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid)); + } else { + precomputed_grids.push_back( + PrecomputeGrid(precomputed_grids.back(), false, + (1 << (depth - 1)) * Eigen::Array3i::Ones())); + } + const int width = 1 << depth; + for (int i = 0; i < 100; ++i) { + const auto x = coordinate_distribution(rng); + const auto y = coordinate_distribution(rng); + const auto z = coordinate_distribution(rng); + float max_probability = 0.; + for (int dx = 0; dx < width; ++dx) { + for (int dy = 0; dy < width; ++dy) { + for (int dz = 0; dz < width; ++dz) { + max_probability = std::max( + max_probability, hybrid_grid.GetProbability( + Eigen::Array3i(x + dx, y + dy, z + dz))); + } + } + } + + EXPECT_NEAR(max_probability, + PrecomputationGrid3D::ToProbability( + precomputed_grids.back().value(Eigen::Array3i(x, y, z))), + 1e-2); + } + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc new file mode 100644 index 0000000..6ac9e0a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc @@ -0,0 +1,118 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h" + +#include + +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +RealTimeCorrelativeScanMatcher3D::RealTimeCorrelativeScanMatcher3D( + const proto::RealTimeCorrelativeScanMatcherOptions& options) + : options_(options) {} + +float RealTimeCorrelativeScanMatcher3D::Match( + const transform::Rigid3d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, + transform::Rigid3d* pose_estimate) const { + CHECK(pose_estimate != nullptr); + float best_score = -1.f; + for (const transform::Rigid3f& transform : GenerateExhaustiveSearchTransforms( + hybrid_grid.resolution(), point_cloud)) { + const transform::Rigid3f candidate = + initial_pose_estimate.cast() * transform; + const float score = ScoreCandidate( + hybrid_grid, sensor::TransformPointCloud(point_cloud, candidate), + transform); + if (score > best_score) { + best_score = score; + *pose_estimate = candidate.cast(); + } + } + return best_score; +} + +std::vector +RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms( + const float resolution, const sensor::PointCloud& point_cloud) const { + std::vector result; + const int linear_window_size = + common::RoundToInt(options_.linear_search_window() / resolution); + // We set this value to something on the order of resolution to make sure that + // the std::acos() below is defined. + float max_scan_range = 3.f * resolution; + for (const sensor::RangefinderPoint& point : point_cloud) { + const float range = point.position.norm(); + max_scan_range = std::max(range, max_scan_range); + } + const float kSafetyMargin = 1.f - 1e-3f; + const float angular_step_size = + kSafetyMargin * std::acos(1.f - common::Pow2(resolution) / + (2.f * common::Pow2(max_scan_range))); + const int angular_window_size = + common::RoundToInt(options_.angular_search_window() / angular_step_size); + for (int z = -linear_window_size; z <= linear_window_size; ++z) { + for (int y = -linear_window_size; y <= linear_window_size; ++y) { + for (int x = -linear_window_size; x <= linear_window_size; ++x) { + for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { + for (int ry = -angular_window_size; ry <= angular_window_size; ++ry) { + for (int rx = -angular_window_size; rx <= angular_window_size; + ++rx) { + const Eigen::Vector3f angle_axis(rx * angular_step_size, + ry * angular_step_size, + rz * angular_step_size); + result.emplace_back( + Eigen::Vector3f(x * resolution, y * resolution, + z * resolution), + transform::AngleAxisVectorToRotationQuaternion(angle_axis)); + } + } + } + } + } + } + return result; +} + +float RealTimeCorrelativeScanMatcher3D::ScoreCandidate( + const HybridGrid& hybrid_grid, + const sensor::PointCloud& transformed_point_cloud, + const transform::Rigid3f& transform) const { + float score = 0.f; + for (const sensor::RangefinderPoint& point : transformed_point_cloud) { + score += + hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point.position)); + } + score /= static_cast(transformed_point_cloud.size()); + const float angle = transform::GetAngle(transform); + score *= + std::exp(-common::Pow2(transform.translation().norm() * + options_.translation_delta_cost_weight() + + angle * options_.rotation_delta_cost_weight())); + CHECK_GT(score, 0.f); + return score; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h new file mode 100644 index 0000000..bf23377 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h @@ -0,0 +1,66 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// A voxel accurate scan matcher, exhaustively evaluating the scan matching +// search space. +class RealTimeCorrelativeScanMatcher3D { + public: + explicit RealTimeCorrelativeScanMatcher3D( + const scan_matching::proto::RealTimeCorrelativeScanMatcherOptions& + options); + + RealTimeCorrelativeScanMatcher3D(const RealTimeCorrelativeScanMatcher3D&) = + delete; + RealTimeCorrelativeScanMatcher3D& operator=( + const RealTimeCorrelativeScanMatcher3D&) = delete; + + // Aligns 'point_cloud' within the 'hybrid_grid' given an + // 'initial_pose_estimate' then updates 'pose_estimate' with the result and + // returns the score. + float Match(const transform::Rigid3d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, + const HybridGrid& hybrid_grid, + transform::Rigid3d* pose_estimate) const; + + private: + std::vector GenerateExhaustiveSearchTransforms( + float resolution, const sensor::PointCloud& point_cloud) const; + float ScoreCandidate(const HybridGrid& hybrid_grid, + const sensor::PointCloud& transformed_point_cloud, + const transform::Rigid3f& transform) const; + + const proto::RealTimeCorrelativeScanMatcherOptions options_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc new file mode 100644 index 0000000..da3223d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc @@ -0,0 +1,122 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test { + protected: + RealTimeCorrelativeScanMatcher3DTest() + : hybrid_grid_(0.1f), + expected_pose_(Eigen::Vector3d(-1., 0., 0.), + Eigen::Quaterniond::Identity()) { + for (const Eigen::Vector3f& point : + {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), + Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), + Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), + Eigen::Vector3f(-7.f, 3.f, 1.f)}) { + point_cloud_.push_back({point}); + hybrid_grid_.SetProbability( + hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + } + + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + linear_search_window = 0.3, + angular_search_window = math.rad(1.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1., + })text"); + real_time_correlative_scan_matcher_.reset( + new RealTimeCorrelativeScanMatcher3D( + CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary.get()))); + } + + void TestFromInitialPose(const transform::Rigid3d& initial_pose) { + transform::Rigid3d pose; + + const float score = real_time_correlative_scan_matcher_->Match( + initial_pose, point_cloud_, hybrid_grid_, &pose); + LOG(INFO) << "Score: " << score; + EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3)); + } + + HybridGrid hybrid_grid_; + transform::Rigid3d expected_pose_; + sensor::PointCloud point_cloud_; + std::unique_ptr + real_time_correlative_scan_matcher_; +}; + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, PerfectEstimate) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))); +} + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongX) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.))); +} + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2))); +} + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongXYZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2))); +} + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundX) { + TestFromInitialPose(transform::Rigid3d( + Eigen::Vector3d(-1., 0., 0.), + Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.)))); +} + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundY) { + TestFromInitialPose(transform::Rigid3d( + Eigen::Vector3d(-1., 0., 0.), + Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.)))); +} + +TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundYZ) { + TestFromInitialPose(transform::Rigid3d( + Eigen::Vector3d(-1., 0., 0.), + Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.)))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h new file mode 100644 index 0000000..301f45b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h @@ -0,0 +1,80 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/common/math.h" +#include "ceres/ceres.h" +#include "ceres/rotation.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'. +// Cost increases with the solution's distance from 'target_rotation'. +class RotationDeltaCostFunctor3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const Eigen::Quaterniond& target_rotation) { + return new ceres::AutoDiffCostFunction( + new RotationDeltaCostFunctor3D(scaling_factor, target_rotation)); + } + + template + bool operator()(const T* const rotation_quaternion, T* residual) const { + std::array delta; + common::QuaternionProduct(target_rotation_inverse_, rotation_quaternion, + delta.data()); + // Will compute the squared norm of the imaginary component of the delta + // quaternion which is sin(phi/2)^2. + residual[0] = scaling_factor_ * delta[1]; + residual[1] = scaling_factor_ * delta[2]; + residual[2] = scaling_factor_ * delta[3]; + return true; + } + + private: + // Constructs a new RotationDeltaCostFunctor3D from the given + // 'target_rotation'. + explicit RotationDeltaCostFunctor3D(const double scaling_factor, + const Eigen::Quaterniond& target_rotation) + : scaling_factor_(scaling_factor) { + target_rotation_inverse_[0] = target_rotation.w(); + target_rotation_inverse_[1] = -target_rotation.x(); + target_rotation_inverse_[2] = -target_rotation.y(); + target_rotation_inverse_[3] = -target_rotation.z(); + } + + RotationDeltaCostFunctor3D(const RotationDeltaCostFunctor3D&) = delete; + RotationDeltaCostFunctor3D& operator=(const RotationDeltaCostFunctor3D&) = + delete; + + const double scaling_factor_; + double target_rotation_inverse_[4]; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d_test.cc new file mode 100644 index 0000000..4e9a29b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d_test.cc @@ -0,0 +1,87 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +constexpr double kPrecision = 1e-8; + +double ComputeRotationDeltaSquaredCost( + const Eigen::Quaterniond& rotation, const double scaling_factor, + const Eigen::Quaterniond& target_rotation) { + std::unique_ptr cost_function( + RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(scaling_factor, + target_rotation)); + const std::array parameter_quaternion = { + {rotation.w(), rotation.x(), rotation.y(), rotation.z()}}; + const std::vector parameters = {parameter_quaternion.data()}; + std::vector residuals(cost_function->num_residuals()); + EXPECT_TRUE(cost_function->Evaluate(parameters.data(), residuals.data(), + nullptr /* jacobian */)); + double sum_of_squares = 0; + for (double residual : residuals) { + sum_of_squares += residual * residual; + } + return sum_of_squares; +} + +TEST(RotationDeltaCostFunctor3DTest, SameRotationGivesZeroCost) { + EXPECT_NEAR( + 0., + ComputeRotationDeltaSquaredCost(Eigen::Quaterniond::Identity(), 1.0, + Eigen::Quaterniond::Identity()), + kPrecision); + + Eigen::Quaterniond rotation( + Eigen::AngleAxisd(0.9, Eigen::Vector3d(0.2, 0.1, 0.3).normalized())); + EXPECT_NEAR(0., ComputeRotationDeltaSquaredCost(rotation, 1.0, rotation), + kPrecision); +} + +TEST(RotationDeltaCostFunctor3DTest, ComputesCorrectCost) { + double scaling_factor = 1.2; + double angle = 0.8; + Eigen::Quaterniond rotation( + Eigen::AngleAxisd(angle, Eigen::Vector3d(0.2, 0.1, 0.8).normalized())); + Eigen::Quaterniond target_rotation( + Eigen::AngleAxisd(0.2, Eigen::Vector3d(-0.5, 0.3, 0.4).normalized())); + double expected_cost = std::pow(scaling_factor * std::sin(angle / 2.0), 2); + EXPECT_NEAR(expected_cost, + ComputeRotationDeltaSquaredCost(rotation, scaling_factor, + Eigen::Quaterniond::Identity()), + kPrecision); + EXPECT_NEAR(expected_cost, + ComputeRotationDeltaSquaredCost(target_rotation * rotation, + scaling_factor, target_rotation), + kPrecision); + EXPECT_NEAR(expected_cost, + ComputeRotationDeltaSquaredCost(rotation * target_rotation, + scaling_factor, target_rotation), + kPrecision); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc new file mode 100644 index 0000000..a584ff4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -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. + */ + +#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" + +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +namespace { + +constexpr float kMinDistance = 0.2f; +constexpr float kMaxDistance = 0.9f; +constexpr float kSliceHeight = 0.2f; + +void AddValueToHistogram(float angle, const float value, + Eigen::VectorXf* histogram) { + // Map the angle to [0, pi), i.e. a vector and its inverse are considered to + // represent the same angle. + while (angle > static_cast(M_PI)) { + angle -= static_cast(M_PI); + } + while (angle < 0.f) { + angle += static_cast(M_PI); + } + const float zero_to_one = angle / static_cast(M_PI); + const int bucket = common::Clamp( + common::RoundToInt(histogram->size() * zero_to_one - 0.5f), 0, + histogram->size() - 1); + (*histogram)(bucket) += value; +} + +Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) { + CHECK(!slice.empty()); + Eigen::Vector3f sum = Eigen::Vector3f::Zero(); + for (const sensor::RangefinderPoint& point : slice) { + sum += point.position; + } + return sum / static_cast(slice.size()); +} + +void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice, + Eigen::VectorXf* const histogram) { + if (slice.empty()) { + return; + } + // We compute the angle of the ray from a point to the centroid of the whole + // point cloud. If it is orthogonal to the angle we compute between points, we + // will add the angle between points to the histogram with the maximum weight. + // This is to reject, e.g., the angles observed on the ceiling and floor. + const Eigen::Vector3f centroid = ComputeCentroid(slice); + Eigen::Vector3f last_point_position = slice.points().front().position; + for (const sensor::RangefinderPoint& point : slice) { + const Eigen::Vector2f delta = + (point.position - last_point_position).head<2>(); + const Eigen::Vector2f direction = (point.position - centroid).head<2>(); + const float distance = delta.norm(); + if (distance < kMinDistance || direction.norm() < kMinDistance) { + continue; + } + if (distance > kMaxDistance) { + last_point_position = point.position; + continue; + } + const float angle = common::atan2(delta); + const float value = std::max( + 0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized()))); + AddValueToHistogram(angle, value, histogram); + } +} + +// A function to sort the points in each slice by angle around the centroid. +// This is because the returns from different rangefinders are interleaved in +// the data. +sensor::PointCloud SortSlice(const sensor::PointCloud& slice) { + struct SortableAnglePointPair { + bool operator<(const SortableAnglePointPair& rhs) const { + return angle < rhs.angle; + } + + float angle; + sensor::RangefinderPoint point; + }; + const Eigen::Vector3f centroid = ComputeCentroid(slice); + std::vector by_angle; + by_angle.reserve(slice.size()); + for (const sensor::RangefinderPoint& point : slice) { + const Eigen::Vector2f delta = (point.position - centroid).head<2>(); + if (delta.norm() < kMinDistance) { + continue; + } + by_angle.push_back(SortableAnglePointPair{common::atan2(delta), point}); + } + std::sort(by_angle.begin(), by_angle.end()); + sensor::PointCloud result; + for (const auto& pair : by_angle) { + result.push_back(pair.point); + } + return result; +} + +float MatchHistograms(const Eigen::VectorXf& submap_histogram, + const Eigen::VectorXf& scan_histogram) { + // We compute the dot product of normalized histograms as a measure of + // similarity. + const float scan_histogram_norm = scan_histogram.norm(); + const float submap_histogram_norm = submap_histogram.norm(); + const float normalization = scan_histogram_norm * submap_histogram_norm; + if (normalization < 1e-3f) { + return 1.f; + } + return submap_histogram.dot(scan_histogram) / normalization; +} + +} // namespace + +RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram) + : histogram_(histogram) {} + +// Rotates the given 'histogram' by the given 'angle'. This might lead to +// rotations of a fractional bucket which is handled by linearly interpolating. +Eigen::VectorXf RotationalScanMatcher::RotateHistogram( + const Eigen::VectorXf& histogram, const float angle) { + if (histogram.size() == 0) { + return histogram; + } + const float rotate_by_buckets = -angle * histogram.size() / M_PI; + int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f); + const float fraction = rotate_by_buckets - full_buckets; + CHECK_GT(histogram.size(), 0); + while (full_buckets < 0) { + full_buckets += histogram.size(); + } + Eigen::VectorXf rotated_histogram_0 = Eigen::VectorXf::Zero(histogram.size()); + Eigen::VectorXf rotated_histogram_1 = Eigen::VectorXf::Zero(histogram.size()); + for (int i = 0; i != histogram.size(); ++i) { + rotated_histogram_0[i] = histogram[(i + full_buckets) % histogram.size()]; + rotated_histogram_1[i] = + histogram[(i + 1 + full_buckets) % histogram.size()]; + } + return fraction * rotated_histogram_1 + + (1.f - fraction) * rotated_histogram_0; +} + +Eigen::VectorXf RotationalScanMatcher::ComputeHistogram( + const sensor::PointCloud& point_cloud, const int histogram_size) { + Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size); + std::map slices; + for (const sensor::RangefinderPoint& point : point_cloud) { + slices[common::RoundToInt(point.position.z() / kSliceHeight)].push_back( + point); + } + for (const auto& slice : slices) { + AddPointCloudSliceToHistogram(SortSlice(slice.second), &histogram); + } + return histogram; +} + +std::vector RotationalScanMatcher::Match( + const Eigen::VectorXf& histogram, const float initial_angle, + const std::vector& angles) const { + std::vector result; + result.reserve(angles.size()); + for (const float angle : angles) { + const Eigen::VectorXf scan_histogram = + RotateHistogram(histogram, initial_angle + angle); + result.push_back(MatchHistograms(*histogram_, scan_histogram)); + } + return result; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h new file mode 100644 index 0000000..5ae5901 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ + +#include + +#include "Eigen/Geometry" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +class RotationalScanMatcher { + public: + // Rotates the given 'histogram' by the given 'angle'. This might lead to + // rotations of a fractional bucket which is handled by linearly + // interpolating. + static Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram, + float angle); + + // Computes the histogram for a gravity aligned 'point_cloud'. + static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud, + int histogram_size); + + explicit RotationalScanMatcher(const Eigen::VectorXf* histogram); + + // Scores how well 'histogram' rotated by 'initial_angle' can be understood as + // further rotated by certain 'angles' relative to the 'nodes'. Each angle + // results in a score between 0 (worst) and 1 (best). + std::vector Match(const Eigen::VectorXf& histogram, + float initial_angle, + const std::vector& angles) const; + + private: + const Eigen::VectorXf* histogram_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc new file mode 100644 index 0000000..93eac7f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc @@ -0,0 +1,72 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) { + Eigen::VectorXf histogram(7); + histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f; + RotationalScanMatcher matcher(&histogram); + const auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f}); + ASSERT_EQ(2, scores.size()); + EXPECT_NEAR(1.f, scores[0], 1e-6); + EXPECT_GT(1.f, scores[1]); +} + +TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) { + constexpr int kNumBuckets = 10; + constexpr float kAnglePerBucket = M_PI / kNumBuckets; + constexpr float kNoInitialRotation = 0.f; + const Eigen::VectorXf histogram_no_initial_rotation = + Eigen::VectorXf::Unit(kNumBuckets, 3); + RotationalScanMatcher matcher(&histogram_no_initial_rotation); + for (float t = 0.f; t < 1.f; t += 0.1f) { + // 't' is the fraction of overlap and we have to divide by the norm of the + // histogram to get the expected score. + const float expected_score = t / std::hypot(t, 1 - t); + // We rotate the 't'-th fraction of a bucket into the matcher's histogram. + auto scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2), + kNoInitialRotation, {t * kAnglePerBucket}); + ASSERT_EQ(1, scores.size()); + EXPECT_NEAR(expected_score, scores[0], 1e-6); + // Also verify rotating out of a bucket. + scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2), + kNoInitialRotation, {(2 - t) * kAnglePerBucket}); + ASSERT_EQ(1, scores.size()); + EXPECT_NEAR(expected_score, scores[0], 1e-6); + // And into and out of a bucket with negative angle. + scores = + matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 4), kNoInitialRotation, + {-t * kAnglePerBucket, (t - 2) * kAnglePerBucket}); + ASSERT_EQ(2, scores.size()); + EXPECT_NEAR(expected_score, scores[0], 1e-6); + EXPECT_NEAR(expected_score, scores[1], 1e-6); + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h new file mode 100644 index 0000000..7959eaf --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h @@ -0,0 +1,71 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ + +#include "Eigen/Core" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes the cost of translating 'translation' to 'target_translation'. +// Cost increases with the solution's distance from 'target_translation'. +class TranslationDeltaCostFunctor3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const Eigen::Vector3d& target_translation) { + return new ceres::AutoDiffCostFunction( + new TranslationDeltaCostFunctor3D(scaling_factor, target_translation)); + } + + template + bool operator()(const T* const translation, T* residual) const { + residual[0] = scaling_factor_ * (translation[0] - x_); + residual[1] = scaling_factor_ * (translation[1] - y_); + residual[2] = scaling_factor_ * (translation[2] - z_); + return true; + } + + private: + // Constructs a new TranslationDeltaCostFunctor3D from the given + // 'target_translation'. + explicit TranslationDeltaCostFunctor3D( + const double scaling_factor, const Eigen::Vector3d& target_translation) + : scaling_factor_(scaling_factor), + x_(target_translation.x()), + y_(target_translation.y()), + z_(target_translation.z()) {} + + TranslationDeltaCostFunctor3D(const TranslationDeltaCostFunctor3D&) = delete; + TranslationDeltaCostFunctor3D& operator=( + const TranslationDeltaCostFunctor3D&) = delete; + + const double scaling_factor_; + const double x_; + const double y_; + const double z_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/collated_trajectory_builder.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/collated_trajectory_builder.cc new file mode 100644 index 0000000..b72aa26 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/collated_trajectory_builder.cc @@ -0,0 +1,90 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/collated_trajectory_builder.h" + +#include "cartographer/common/time.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +namespace { + +constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; + +} // namespace + +CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( + const proto::TrajectoryBuilderOptions& trajectory_options, + sensor::CollatorInterface* const sensor_collator, const int trajectory_id, + const std::set& expected_sensor_ids, + std::unique_ptr wrapped_trajectory_builder) + : sensor_collator_(sensor_collator), + collate_landmarks_(trajectory_options.collate_landmarks()), + collate_fixed_frame_(trajectory_options.collate_fixed_frame()), + trajectory_id_(trajectory_id), + wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), + last_logging_time_(std::chrono::steady_clock::now()) { + absl::flat_hash_set expected_sensor_id_strings; + for (const auto& sensor_id : expected_sensor_ids) { + if (sensor_id.type == SensorId::SensorType::LANDMARK && + !collate_landmarks_) { + continue; + } + if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE && + !collate_fixed_frame_) { + continue; + } + expected_sensor_id_strings.insert(sensor_id.id); + } + sensor_collator_->AddTrajectory( + trajectory_id, expected_sensor_id_strings, + [this](const std::string& sensor_id, std::unique_ptr data) { + HandleCollatedSensorData(sensor_id, std::move(data)); + }); +} + +void CollatedTrajectoryBuilder::AddData(std::unique_ptr data) { + sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); +} + +void CollatedTrajectoryBuilder::HandleCollatedSensorData( + const std::string& sensor_id, std::unique_ptr data) { + auto it = rate_timers_.find(sensor_id); + if (it == rate_timers_.end()) { + it = rate_timers_ + .emplace( + std::piecewise_construct, std::forward_as_tuple(sensor_id), + std::forward_as_tuple( + common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) + .first; + } + it->second.Pulse(data->GetTime()); + + if (std::chrono::steady_clock::now() - last_logging_time_ > + common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { + for (const auto& pair : rate_timers_) { + LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); + } + last_logging_time_ = std::chrono::steady_clock::now(); + } + + data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/collated_trajectory_builder.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/collated_trajectory_builder.h new file mode 100644 index 0000000..c00a979 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/collated_trajectory_builder.h @@ -0,0 +1,115 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_COLLATED_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_COLLATED_TRAJECTORY_BUILDER_H_ + +#include +#include +#include +#include +#include + +#include "cartographer/common/internal/rate_timer.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/sensor/collator_interface.h" +#include "cartographer/sensor/internal/dispatchable.h" + +namespace cartographer { +namespace mapping { + +// Collates sensor data using a sensor::CollatorInterface, then passes it on to +// a mapping::TrajectoryBuilderInterface which is common for 2D and 3D. +class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { + public: + using SensorId = TrajectoryBuilderInterface::SensorId; + + CollatedTrajectoryBuilder( + const proto::TrajectoryBuilderOptions& trajectory_options, + sensor::CollatorInterface* sensor_collator, int trajectory_id, + const std::set& expected_sensor_ids, + std::unique_ptr wrapped_trajectory_builder); + ~CollatedTrajectoryBuilder() override {} + + CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; + CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = + delete; + + void AddSensorData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data) override { + AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); + } + + void AddSensorData(const std::string& sensor_id, + const sensor::ImuData& imu_data) override { + AddData(sensor::MakeDispatchable(sensor_id, imu_data)); + } + + void AddSensorData(const std::string& sensor_id, + const sensor::OdometryData& odometry_data) override { + AddData(sensor::MakeDispatchable(sensor_id, odometry_data)); + } + + void AddSensorData( + const std::string& sensor_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) override { + if (collate_fixed_frame_) { + AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); + return; + } + wrapped_trajectory_builder_->AddSensorData(sensor_id, + fixed_frame_pose_data); + } + + void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) override { + if (collate_landmarks_) { + AddData(sensor::MakeDispatchable(sensor_id, landmark_data)); + return; + } + wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data); + } + + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override { + AddData(std::move(local_slam_result_data)); + } + + private: + void AddData(std::unique_ptr data); + + void HandleCollatedSensorData(const std::string& sensor_id, + std::unique_ptr data); + + sensor::CollatorInterface* const sensor_collator_; + const bool collate_landmarks_; + const bool collate_fixed_frame_; + const int trajectory_id_; + std::unique_ptr wrapped_trajectory_builder_; + + // Time at which we last logged the rates of incoming sensor data. + std::chrono::steady_clock::time_point last_logging_time_; + std::map> rate_timers_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_COLLATED_TRAJECTORY_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components.cc new file mode 100644 index 0000000..131fb4f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components.cc @@ -0,0 +1,132 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/connected_components.h" + +#include + +#include "absl/container/flat_hash_set.h" +#include "cartographer/mapping/proto/connected_components.pb.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +ConnectedComponents::ConnectedComponents() + : lock_(), forest_(), connection_map_() {} + +void ConnectedComponents::Add(const int trajectory_id) { + absl::MutexLock locker(&lock_); + forest_.emplace(trajectory_id, trajectory_id); +} + +void ConnectedComponents::Connect(const int trajectory_id_a, + const int trajectory_id_b) { + absl::MutexLock locker(&lock_); + Union(trajectory_id_a, trajectory_id_b); + auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); + ++connection_map_[sorted_pair]; +} + +void ConnectedComponents::Union(const int trajectory_id_a, + const int trajectory_id_b) { + forest_.emplace(trajectory_id_a, trajectory_id_a); + forest_.emplace(trajectory_id_b, trajectory_id_b); + const int representative_a = FindSet(trajectory_id_a); + const int representative_b = FindSet(trajectory_id_b); + forest_[representative_a] = representative_b; +} + +int ConnectedComponents::FindSet(const int trajectory_id) { + auto it = forest_.find(trajectory_id); + CHECK(it != forest_.end()); + if (it->first != it->second) { + // Path compression for efficiency. + it->second = FindSet(it->second); + } + return it->second; +} + +bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a, + const int trajectory_id_b) { + if (trajectory_id_a == trajectory_id_b) { + return true; + } + + absl::MutexLock locker(&lock_); + + if (forest_.count(trajectory_id_a) == 0 || + forest_.count(trajectory_id_b) == 0) { + return false; + } + return FindSet(trajectory_id_a) == FindSet(trajectory_id_b); +} + +std::vector> ConnectedComponents::Components() { + // Map from cluster exemplar -> growing cluster. + absl::flat_hash_map> map; + absl::MutexLock locker(&lock_); + for (const auto& trajectory_id_entry : forest_) { + map[FindSet(trajectory_id_entry.first)].push_back( + trajectory_id_entry.first); + } + + std::vector> result; + result.reserve(map.size()); + for (auto& pair : map) { + result.emplace_back(std::move(pair.second)); + } + return result; +} + +std::vector ConnectedComponents::GetComponent(const int trajectory_id) { + absl::MutexLock locker(&lock_); + const int set_id = FindSet(trajectory_id); + std::vector trajectory_ids; + for (const auto& entry : forest_) { + if (FindSet(entry.first) == set_id) { + trajectory_ids.push_back(entry.first); + } + } + return trajectory_ids; +} + +int ConnectedComponents::ConnectionCount(const int trajectory_id_a, + const int trajectory_id_b) { + absl::MutexLock locker(&lock_); + const auto it = + connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b)); + return it != connection_map_.end() ? it->second : 0; +} + +proto::ConnectedComponents ToProto( + std::vector> connected_components) { + proto::ConnectedComponents proto; + for (auto& connected_component : connected_components) { + std::sort(connected_component.begin(), connected_component.end()); + } + std::sort(connected_components.begin(), connected_components.end()); + for (const auto& connected_component : connected_components) { + 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; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components.h new file mode 100644 index 0000000..05f327e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components.h @@ -0,0 +1,92 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_CONNECTED_COMPONENTS_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_CONNECTED_COMPONENTS_H_ + +#include + +#include "absl/container/flat_hash_map.h" +#include "absl/synchronization/mutex.h" +#include "cartographer/mapping/proto/connected_components.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 ConnectedComponents { + public: + ConnectedComponents(); + + ConnectedComponents(const ConnectedComponents&) = delete; + ConnectedComponents& operator=(const ConnectedComponents&) = delete; + + // Add a trajectory which is initially connected to only itself. + void Add(int trajectory_id) LOCKS_EXCLUDED(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(int trajectory_id_a, int trajectory_id_b) LOCKS_EXCLUDED(lock_); + + // Determines if two trajectories have been (transitively) connected. If + // either trajectory is not being tracked, returns false, except when it is + // the same trajectory, where it returns true. This function is invariant to + // the order of its arguments. + bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) + LOCKS_EXCLUDED(lock_); + + // Return the number of _direct_ connections between 'trajectory_id_a' and + // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0. + // This function is invariant to the order of its arguments. + int ConnectionCount(int trajectory_id_a, int trajectory_id_b) + LOCKS_EXCLUDED(lock_); + + // The trajectory IDs, grouped by connectivity. + std::vector> Components() LOCKS_EXCLUDED(lock_); + + // The list of trajectory IDs that belong to the same connected component as + // 'trajectory_id'. + std::vector GetComponent(int trajectory_id) LOCKS_EXCLUDED(lock_); + + private: + // Find the representative and compresses the path to it. + int FindSet(int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(lock_); + void Union(int trajectory_id_a, int trajectory_id_b) + EXCLUSIVE_LOCKS_REQUIRED(lock_); + + absl::Mutex lock_; + // Tracks transitive connectivity using a disjoint set forest, i.e. each + // entry points towards the representative for the given trajectory. + std::map forest_ GUARDED_BY(lock_); + // Tracks the number of direct connections between a pair of trajectories. + std::map, int> connection_map_ GUARDED_BY(lock_); +}; + +// Returns a proto encoding connected components. +proto::ConnectedComponents ToProto( + std::vector> connected_components); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONNECTED_COMPONENTS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components_test.cc new file mode 100644 index 0000000..22feb8d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/connected_components_test.cc @@ -0,0 +1,117 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/connected_components.h" + +#include +#include +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +constexpr int kNumTrajectories = 10; + +TEST(ConnectedComponentsTest, TransitivelyConnected) { + ConnectedComponents connected_components; + + // Make sure nothing's connected until we connect some things. + for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) { + for (int trajectory_b = 0; trajectory_b < kNumTrajectories; + ++trajectory_b) { + EXPECT_EQ(trajectory_a == trajectory_b, + connected_components.TransitivelyConnected(trajectory_a, + trajectory_b)); + } + } + + // Connect some stuff up. + connected_components.Connect(0, 1); + EXPECT_TRUE(connected_components.TransitivelyConnected(0, 1)); + connected_components.Connect(8, 9); + EXPECT_TRUE(connected_components.TransitivelyConnected(8, 9)); + EXPECT_FALSE(connected_components.TransitivelyConnected(0, 9)); + + connected_components.Connect(1, 8); + for (int i : {0, 1}) { + for (int j : {8, 9}) { + EXPECT_TRUE(connected_components.TransitivelyConnected(i, j)); + } + } +} + +TEST(ConnectedComponentsTest, EmptyConnectedComponents) { + ConnectedComponents connected_components; + auto connections = connected_components.Components(); + EXPECT_EQ(0, connections.size()); +} + +TEST(ConnectedComponentsTest, ConnectedComponents) { + ConnectedComponents connected_components; + for (int i = 0; i <= 4; ++i) { + connected_components.Connect(0, i); + } + for (int i = 5; i <= 9; ++i) { + connected_components.Connect(5, i); + } + auto connections = connected_components.Components(); + ASSERT_EQ(2, connections.size()); + // The clustering is arbitrary; we need to figure out which one is which. + const std::vector* zero_cluster = nullptr; + const std::vector* five_cluster = nullptr; + if (std::find(connections[0].begin(), connections[0].end(), 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(), + i) != zero_cluster->end()); + EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), i) != + five_cluster->end()); + } +} + +TEST(ConnectedComponentsTest, ConnectionCount) { + ConnectedComponents connected_components; + for (int i = 0; i < kNumTrajectories; ++i) { + connected_components.Connect(0, 1); + // Permute the arguments to check invariance. + EXPECT_EQ(i + 1, connected_components.ConnectionCount(1, 0)); + } + for (int i = 1; i < 9; ++i) { + EXPECT_EQ(0, connected_components.ConnectionCount(i, i + 1)); + } +} + +TEST(ConnectedComponentsTest, ReflexiveConnectivity) { + ConnectedComponents connected_components; + EXPECT_TRUE(connected_components.TransitivelyConnected(0, 0)); + EXPECT_EQ(0, connected_components.ConnectionCount(0, 0)); + connected_components.Add(0); + EXPECT_TRUE(connected_components.TransitivelyConnected(0, 0)); + EXPECT_EQ(0, connected_components.ConnectionCount(0, 0)); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder.cc new file mode 100644 index 0000000..21106bd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder.cc @@ -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. + */ + +#include "cartographer/mapping/internal/constraints/constraint_builder.h" + +#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" +#include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h" +#include "cartographer/sensor/internal/voxel_filter.h" + +namespace cartographer { +namespace mapping { +namespace constraints { + +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.set_min_score(parameter_dictionary->GetDouble("min_score")); + options.set_global_localization_min_score( + parameter_dictionary->GetDouble("global_localization_min_score")); + options.set_loop_closure_translation_weight( + parameter_dictionary->GetDouble("loop_closure_translation_weight")); + options.set_loop_closure_rotation_weight( + parameter_dictionary->GetDouble("loop_closure_rotation_weight")); + options.set_log_matches(parameter_dictionary->GetBool("log_matches")); + *options.mutable_fast_correlative_scan_matcher_options() = + scan_matching::CreateFastCorrelativeScanMatcherOptions2D( + parameter_dictionary->GetDictionary("fast_correlative_scan_matcher") + .get()); + *options.mutable_ceres_scan_matcher_options() = + scan_matching::CreateCeresScanMatcherOptions2D( + parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); + *options.mutable_fast_correlative_scan_matcher_options_3d() = + scan_matching::CreateFastCorrelativeScanMatcherOptions3D( + parameter_dictionary + ->GetDictionary("fast_correlative_scan_matcher_3d") + .get()); + *options.mutable_ceres_scan_matcher_options_3d() = + scan_matching::CreateCeresScanMatcherOptions3D( + parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get()); + return options; +} + +} // namespace constraints +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder.h new file mode 100644 index 0000000..7532b17 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder.h @@ -0,0 +1,34 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h" + +namespace cartographer { +namespace mapping { +namespace constraints { + +proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace constraints +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d.cc new file mode 100644 index 0000000..ad3dfb7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -0,0 +1,347 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/constraints/constraint_builder_2d.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer/metrics/histogram.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace constraints { + +static auto* kConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kQueueLengthMetric = metrics::Gauge::Null(); +static auto* kConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null(); + +transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) { + return transform::Project2D(submap.local_pose()); +} + +ConstraintBuilder2D::ConstraintBuilder2D( + const constraints::proto::ConstraintBuilderOptions& options, + common::ThreadPoolInterface* const thread_pool) + : options_(options), + thread_pool_(thread_pool), + finish_node_task_(absl::make_unique()), + when_done_task_(absl::make_unique()), + ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} + +ConstraintBuilder2D::~ConstraintBuilder2D() { + absl::MutexLock locker(&mutex_); + CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); + CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); + CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; + CHECK_EQ(num_started_nodes_, num_finished_nodes_); + CHECK(when_done_ == nullptr); +} + +void ConstraintBuilder2D::MaybeAddConstraint( + const SubmapId& submap_id, const Submap2D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data, + const transform::Rigid2d& initial_relative_pose) { + if (initial_relative_pose.translation().norm() > + options_.max_constraint_distance()) { + return; + } + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } + + absl::MutexLock locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "MaybeAddConstraint was called while WhenDone was scheduled."; + } + constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); + auto* const constraint = &constraints_.back(); + const auto* scan_matcher = + DispatchScanMatcherConstruction(submap_id, submap->grid()); + auto constraint_task = absl::make_unique(); + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ + constant_data, initial_relative_pose, *scan_matcher, + constraint); + }); + constraint_task->AddDependency(scan_matcher->creation_task_handle); + auto constraint_task_handle = + thread_pool_->Schedule(std::move(constraint_task)); + finish_node_task_->AddDependency(constraint_task_handle); +} + +void ConstraintBuilder2D::MaybeAddGlobalConstraint( + const SubmapId& submap_id, const Submap2D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data) { + absl::MutexLock locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; + } + constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); + auto* const constraint = &constraints_.back(); + const auto* scan_matcher = + DispatchScanMatcherConstruction(submap_id, submap->grid()); + auto constraint_task = absl::make_unique(); + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ + constant_data, transform::Rigid2d::Identity(), + *scan_matcher, constraint); + }); + constraint_task->AddDependency(scan_matcher->creation_task_handle); + auto constraint_task_handle = + thread_pool_->Schedule(std::move(constraint_task)); + finish_node_task_->AddDependency(constraint_task_handle); +} + +void ConstraintBuilder2D::NotifyEndOfNode() { + absl::MutexLock locker(&mutex_); + CHECK(finish_node_task_ != nullptr); + finish_node_task_->SetWorkItem([this] { + absl::MutexLock locker(&mutex_); + ++num_finished_nodes_; + }); + auto finish_node_task_handle = + thread_pool_->Schedule(std::move(finish_node_task_)); + finish_node_task_ = absl::make_unique(); + when_done_task_->AddDependency(finish_node_task_handle); + ++num_started_nodes_; +} + +void ConstraintBuilder2D::WhenDone( + const std::function& callback) { + absl::MutexLock locker(&mutex_); + CHECK(when_done_ == nullptr); + // TODO(gaschler): Consider using just std::function, it can also be empty. + when_done_ = absl::make_unique>(callback); + CHECK(when_done_task_ != nullptr); + when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); }); + thread_pool_->Schedule(std::move(when_done_task_)); + when_done_task_ = absl::make_unique(); +} + +const ConstraintBuilder2D::SubmapScanMatcher* +ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id, + const Grid2D* const grid) { + CHECK(grid); + if (submap_scan_matchers_.count(submap_id) != 0) { + return &submap_scan_matchers_.at(submap_id); + } + auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); + submap_scan_matcher.grid = grid; + auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options(); + auto scan_matcher_task = absl::make_unique(); + scan_matcher_task->SetWorkItem( + [&submap_scan_matcher, &scan_matcher_options]() { + submap_scan_matcher.fast_correlative_scan_matcher = + absl::make_unique( + *submap_scan_matcher.grid, scan_matcher_options); + }); + submap_scan_matcher.creation_task_handle = + thread_pool_->Schedule(std::move(scan_matcher_task)); + return &submap_scan_matchers_.at(submap_id); +} + +void ConstraintBuilder2D::ComputeConstraint( + const SubmapId& submap_id, const Submap2D* const submap, + const NodeId& node_id, bool match_full_submap, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid2d& initial_relative_pose, + const SubmapScanMatcher& submap_scan_matcher, + std::unique_ptr* constraint) { + CHECK(submap_scan_matcher.fast_correlative_scan_matcher); + const transform::Rigid2d initial_pose = + ComputeSubmapPose(*submap) * initial_relative_pose; + + // The 'constraint_transform' (submap i <- node j) is computed from: + // - a 'filtered_gravity_aligned_point_cloud' in node j, + // - the initial guess 'initial_pose' for (map <- node j), + // - the result 'pose_estimate' of Match() (map <- node j). + // - the ComputeSubmapPose() (map <- submap i) + float score = 0.; + transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); + + // Compute 'pose_estimate' in three stages: + // 1. Fast estimate using the fast correlative scan matcher. + // 2. Prune if the score is too low. + // 3. Refine. + if (match_full_submap) { + kGlobalConstraintsSearchedMetric->Increment(); + if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap( + constant_data->filtered_gravity_aligned_point_cloud, + options_.global_localization_min_score(), &score, &pose_estimate)) { + CHECK_GT(score, options_.global_localization_min_score()); + CHECK_GE(node_id.trajectory_id, 0); + CHECK_GE(submap_id.trajectory_id, 0); + kGlobalConstraintsFoundMetric->Increment(); + kGlobalConstraintScoresMetric->Observe(score); + } else { + return; + } + } else { + kConstraintsSearchedMetric->Increment(); + if (submap_scan_matcher.fast_correlative_scan_matcher->Match( + initial_pose, constant_data->filtered_gravity_aligned_point_cloud, + options_.min_score(), &score, &pose_estimate)) { + // We've reported a successful local match. + CHECK_GT(score, options_.min_score()); + kConstraintsFoundMetric->Increment(); + kConstraintScoresMetric->Observe(score); + } else { + return; + } + } + { + absl::MutexLock locker(&mutex_); + score_histogram_.Add(score); + } + + // Use the CSM estimate as both the initial and previous pose. This has the + // effect that, in the absence of better information, we prefer the original + // CSM estimate. + ceres::Solver::Summary unused_summary; + ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate, + constant_data->filtered_gravity_aligned_point_cloud, + *submap_scan_matcher.grid, &pose_estimate, + &unused_summary); + + const transform::Rigid2d constraint_transform = + ComputeSubmapPose(*submap).inverse() * pose_estimate; + constraint->reset(new Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + options_.loop_closure_translation_weight(), + options_.loop_closure_rotation_weight()}, + Constraint::INTER_SUBMAP}); + + if (options_.log_matches()) { + std::ostringstream info; + info << "Node " << node_id << " with " + << constant_data->filtered_gravity_aligned_point_cloud.size() + << " points on submap " << submap_id << std::fixed; + if (match_full_submap) { + info << " matches"; + } else { + const transform::Rigid2d difference = + initial_pose.inverse() * pose_estimate; + info << " differs by translation " << std::setprecision(2) + << difference.translation().norm() << " rotation " + << std::setprecision(3) << std::abs(difference.normalized_angle()); + } + info << " with score " << std::setprecision(1) << 100. * score << "%."; + LOG(INFO) << info.str(); + } +} + +void ConstraintBuilder2D::RunWhenDoneCallback() { + Result result; + std::unique_ptr> callback; + { + absl::MutexLock locker(&mutex_); + CHECK(when_done_ != nullptr); + for (const std::unique_ptr& constraint : constraints_) { + if (constraint == nullptr) continue; + result.push_back(*constraint); + } + if (options_.log_matches()) { + LOG(INFO) << constraints_.size() << " computations resulted in " + << result.size() << " additional constraints."; + LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10); + } + constraints_.clear(); + callback = std::move(when_done_); + when_done_.reset(); + kQueueLengthMetric->Set(constraints_.size()); + } + (*callback)(result); +} + +int ConstraintBuilder2D::GetNumFinishedNodes() { + absl::MutexLock locker(&mutex_); + return num_finished_nodes_; +} + +void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { + absl::MutexLock locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "DeleteScanMatcher was called while WhenDone was scheduled."; + } + submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); +} + +void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) { + auto* counts = factory->NewCounterFamily( + "mapping_constraints_constraint_builder_2d_constraints", + "Constraints computed"); + kConstraintsSearchedMetric = + counts->Add({{"search_region", "local"}, {"matcher", "searched"}}); + kConstraintsFoundMetric = + counts->Add({{"search_region", "local"}, {"matcher", "found"}}); + kGlobalConstraintsSearchedMetric = + counts->Add({{"search_region", "global"}, {"matcher", "searched"}}); + kGlobalConstraintsFoundMetric = + counts->Add({{"search_region", "global"}, {"matcher", "found"}}); + auto* queue_length = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_2d_queue_length", "Queue length"); + kQueueLengthMetric = queue_length->Add({}); + auto boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = factory->NewHistogramFamily( + "mapping_constraints_constraint_builder_2d_scores", + "Constraint scores built", boundaries); + kConstraintScoresMetric = scores->Add({{"search_region", "local"}}); + kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}}); + auto* num_matchers = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_2d_num_submap_scan_matchers", + "Current number of constructed submap scan matchers"); + kNumSubmapScanMatchersMetric = num_matchers->Add({}); +} + +} // namespace constraints +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d.h new file mode 100644 index 0000000..6667fdb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -0,0 +1,176 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_2D_H_ + +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/histogram.h" +#include "cartographer/common/math.h" +#include "cartographer/common/task.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace constraints { + +// Returns (map <- submap) where 'submap' is a coordinate system at the origin +// of the Submap. +transform::Rigid2d ComputeSubmapPose(const Submap2D& submap); + +// Asynchronously computes constraints. +// +// Intermingle an arbitrary number of calls to 'MaybeAddConstraint', +// 'MaybeAddGlobalConstraint', and 'NotifyEndOfNode', then call 'WhenDone' once. +// After all computations are done the 'callback' will be called with the result +// and another MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. +// +// This class is thread-safe. +class ConstraintBuilder2D { + public: + using Constraint = PoseGraphInterface::Constraint; + using Result = std::vector; + + ConstraintBuilder2D(const proto::ConstraintBuilderOptions& options, + common::ThreadPoolInterface* thread_pool); + ~ConstraintBuilder2D(); + + ConstraintBuilder2D(const ConstraintBuilder2D&) = delete; + ConstraintBuilder2D& operator=(const ConstraintBuilder2D&) = delete; + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The + // 'initial_relative_pose' is relative to the 'submap'. + // + // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until + // all computations are finished. + void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap, + const NodeId& node_id, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid2d& initial_relative_pose); + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. + // This performs full-submap matching. + // + // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until + // all computations are finished. + void MaybeAddGlobalConstraint( + const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id, + const TrajectoryNode::Data* const constant_data); + + // Must be called after all computations related to one node have been added. + void NotifyEndOfNode(); + + // Registers the 'callback' to be called with the results, after all + // computations triggered by 'MaybeAdd*Constraint' have finished. + // 'callback' is executed in the 'ThreadPool'. + void WhenDone(const std::function& callback); + + // Returns the number of consecutive finished nodes. + int GetNumFinishedNodes(); + + // Delete data related to 'submap_id'. + void DeleteScanMatcher(const SubmapId& submap_id); + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + struct SubmapScanMatcher { + const Grid2D* grid = nullptr; + std::unique_ptr + fast_correlative_scan_matcher; + std::weak_ptr creation_task_handle; + }; + + // The returned 'grid' and 'fast_correlative_scan_matcher' must only be + // accessed after 'creation_task_handle' has completed. + const SubmapScanMatcher* DispatchScanMatcherConstruction( + const SubmapId& submap_id, const Grid2D* grid) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Runs in a background thread and does computations for an additional + // constraint, assuming 'submap' and 'compressed_point_cloud' do not change + // anymore. As output, it may create a new Constraint in 'constraint'. + void ComputeConstraint(const SubmapId& submap_id, const Submap2D* submap, + const NodeId& node_id, bool match_full_submap, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid2d& initial_relative_pose, + const SubmapScanMatcher& submap_scan_matcher, + std::unique_ptr* constraint) + LOCKS_EXCLUDED(mutex_); + + void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_); + + const constraints::proto::ConstraintBuilderOptions options_; + common::ThreadPoolInterface* thread_pool_; + absl::Mutex mutex_; + + // 'callback' set by WhenDone(). + std::unique_ptr> when_done_ + GUARDED_BY(mutex_); + + // TODO(gaschler): Use atomics instead of mutex to access these counters. + // Number of the node in reaction to which computations are currently + // added. This is always the number of nodes seen so far, even when older + // nodes are matched against a new submap. + int num_started_nodes_ GUARDED_BY(mutex_) = 0; + + int num_finished_nodes_ GUARDED_BY(mutex_) = 0; + + std::unique_ptr finish_node_task_ GUARDED_BY(mutex_); + + std::unique_ptr when_done_task_ GUARDED_BY(mutex_); + + // Constraints currently being computed in the background. A deque is used to + // keep pointers valid when adding more entries. Constraint search results + // with below-threshold scores are also 'nullptr'. + std::deque> constraints_ GUARDED_BY(mutex_); + + // Map of dispatched or constructed scan matchers by 'submap_id'. + std::map submap_scan_matchers_ + GUARDED_BY(mutex_); + std::map per_submap_sampler_; + + scan_matching::CeresScanMatcher2D ceres_scan_matcher_; + + // Histogram of scan matcher scores. + common::Histogram score_histogram_ GUARDED_BY(mutex_); +}; + +} // namespace constraints +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc new file mode 100644 index 0000000..f80fad9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -0,0 +1,117 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" + +#include + +#include "cartographer/common/internal/testing/thread_pool_for_testing.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/internal/constraints/constraint_builder.h" +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace constraints { +namespace { + +class MockCallback { + public: + MOCK_METHOD1(Run, void(const ConstraintBuilder2D::Result&)); +}; + +class ConstraintBuilder2DTest : public ::testing::Test { + protected: + void SetUp() override { + auto constraint_builder_parameters = testing::ResolveLuaParameters(R"text( + include "pose_graph.lua" + POSE_GRAPH.constraint_builder.sampling_ratio = 1 + POSE_GRAPH.constraint_builder.min_score = 0 + POSE_GRAPH.constraint_builder.global_localization_min_score = 0 + return POSE_GRAPH.constraint_builder)text"); + constraint_builder_ = absl::make_unique( + CreateConstraintBuilderOptions(constraint_builder_parameters.get()), + &thread_pool_); + } + + std::unique_ptr constraint_builder_; + MockCallback mock_; + common::testing::ThreadPoolForTesting thread_pool_; +}; + +TEST_F(ConstraintBuilder2DTest, CallsBack) { + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 0); + EXPECT_CALL(mock_, Run(::testing::IsEmpty())); + constraint_builder_->NotifyEndOfNode(); + constraint_builder_->WhenDone( + [this](const constraints::ConstraintBuilder2D::Result& result) { + mock_.Run(result); + }); + thread_pool_.WaitUntilIdle(); + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 1); +} + +TEST_F(ConstraintBuilder2DTest, FindsConstraints) { + TrajectoryNode::Data node_data; + node_data.filtered_gravity_aligned_point_cloud.push_back( + {Eigen::Vector3f(0.1, 0.2, 0.3)}); + node_data.gravity_alignment = Eigen::Quaterniond::Identity(); + node_data.local_pose = transform::Rigid3d::Identity(); + SubmapId submap_id{0, 1}; + MapLimits map_limits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)); + ValueConversionTables conversion_tables; + Submap2D submap( + Eigen::Vector2f(4.f, 5.f), + absl::make_unique(map_limits, &conversion_tables), + &conversion_tables); + int expected_nodes = 0; + for (int i = 0; i < 2; ++i) { + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); + for (int j = 0; j < 2; ++j) { + constraint_builder_->MaybeAddConstraint(submap_id, &submap, NodeId{0, 0}, + &node_data, + transform::Rigid2d::Identity()); + } + constraint_builder_->MaybeAddGlobalConstraint(submap_id, &submap, + NodeId{0, 0}, &node_data); + constraint_builder_->NotifyEndOfNode(); + thread_pool_.WaitUntilIdle(); + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes); + constraint_builder_->NotifyEndOfNode(); + thread_pool_.WaitUntilIdle(); + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes); + EXPECT_CALL(mock_, + Run(::testing::AllOf( + ::testing::SizeIs(3), + ::testing::Each(::testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))))); + constraint_builder_->WhenDone( + [this](const constraints::ConstraintBuilder2D::Result& result) { + mock_.Run(result); + }); + thread_pool_.WaitUntilIdle(); + constraint_builder_->DeleteScanMatcher(submap_id); + } +} + +} // namespace +} // namespace constraints +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d.cc new file mode 100644 index 0000000..87b3742 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -0,0 +1,390 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/constraints/constraint_builder_3d.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer/metrics/histogram.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace constraints { + +static auto* kConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsSearchedMetric = metrics::Counter::Null(); +static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null(); +static auto* kQueueLengthMetric = metrics::Gauge::Null(); +static auto* kConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kConstraintRotationalScoresMetric = metrics::Histogram::Null(); +static auto* kConstraintLowResolutionScoresMetric = metrics::Histogram::Null(); +static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kGlobalConstraintRotationalScoresMetric = + metrics::Histogram::Null(); +static auto* kGlobalConstraintLowResolutionScoresMetric = + metrics::Histogram::Null(); +static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null(); + +ConstraintBuilder3D::ConstraintBuilder3D( + const proto::ConstraintBuilderOptions& options, + common::ThreadPoolInterface* const thread_pool) + : options_(options), + thread_pool_(thread_pool), + finish_node_task_(absl::make_unique()), + when_done_task_(absl::make_unique()), + ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} + +ConstraintBuilder3D::~ConstraintBuilder3D() { + absl::MutexLock locker(&mutex_); + CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); + CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); + CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; + CHECK_EQ(num_started_nodes_, num_finished_nodes_); + CHECK(when_done_ == nullptr); +} + +void ConstraintBuilder3D::MaybeAddConstraint( + const SubmapId& submap_id, const Submap3D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose) { + if ((global_node_pose.translation() - global_submap_pose.translation()) + .norm() > options_.max_constraint_distance()) { + return; + } + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } + + absl::MutexLock locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "MaybeAddConstraint was called while WhenDone was scheduled."; + } + constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); + auto* const constraint = &constraints_.back(); + const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap); + auto constraint_task = absl::make_unique(); + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ + constant_data, global_node_pose, global_submap_pose, + *scan_matcher, constraint); + }); + constraint_task->AddDependency(scan_matcher->creation_task_handle); + auto constraint_task_handle = + thread_pool_->Schedule(std::move(constraint_task)); + finish_node_task_->AddDependency(constraint_task_handle); +} + +void ConstraintBuilder3D::MaybeAddGlobalConstraint( + const SubmapId& submap_id, const Submap3D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data, + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation) { + absl::MutexLock locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; + } + constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); + auto* const constraint = &constraints_.back(); + const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap); + auto constraint_task = absl::make_unique(); + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ + constant_data, + transform::Rigid3d::Rotation(global_node_rotation), + transform::Rigid3d::Rotation(global_submap_rotation), + *scan_matcher, constraint); + }); + constraint_task->AddDependency(scan_matcher->creation_task_handle); + auto constraint_task_handle = + thread_pool_->Schedule(std::move(constraint_task)); + finish_node_task_->AddDependency(constraint_task_handle); +} + +void ConstraintBuilder3D::NotifyEndOfNode() { + absl::MutexLock locker(&mutex_); + CHECK(finish_node_task_ != nullptr); + finish_node_task_->SetWorkItem([this] { + absl::MutexLock locker(&mutex_); + ++num_finished_nodes_; + }); + auto finish_node_task_handle = + thread_pool_->Schedule(std::move(finish_node_task_)); + finish_node_task_ = absl::make_unique(); + when_done_task_->AddDependency(finish_node_task_handle); + ++num_started_nodes_; +} + +void ConstraintBuilder3D::WhenDone( + const std::function& callback) { + absl::MutexLock locker(&mutex_); + CHECK(when_done_ == nullptr); + // TODO(gaschler): Consider using just std::function, it can also be empty. + when_done_ = absl::make_unique>(callback); + CHECK(when_done_task_ != nullptr); + when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); }); + thread_pool_->Schedule(std::move(when_done_task_)); + when_done_task_ = absl::make_unique(); +} + +const ConstraintBuilder3D::SubmapScanMatcher* +ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id, + const Submap3D* submap) { + if (submap_scan_matchers_.count(submap_id) != 0) { + return &submap_scan_matchers_.at(submap_id); + } + auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); + submap_scan_matcher.high_resolution_hybrid_grid = + &submap->high_resolution_hybrid_grid(); + submap_scan_matcher.low_resolution_hybrid_grid = + &submap->low_resolution_hybrid_grid(); + auto& scan_matcher_options = + options_.fast_correlative_scan_matcher_options_3d(); + const Eigen::VectorXf* histogram = + &submap->rotational_scan_matcher_histogram(); + auto scan_matcher_task = absl::make_unique(); + scan_matcher_task->SetWorkItem( + [&submap_scan_matcher, &scan_matcher_options, histogram]() { + submap_scan_matcher.fast_correlative_scan_matcher = + absl::make_unique( + *submap_scan_matcher.high_resolution_hybrid_grid, + submap_scan_matcher.low_resolution_hybrid_grid, histogram, + scan_matcher_options); + }); + submap_scan_matcher.creation_task_handle = + thread_pool_->Schedule(std::move(scan_matcher_task)); + return &submap_scan_matchers_.at(submap_id); +} + +void ConstraintBuilder3D::ComputeConstraint( + const SubmapId& submap_id, const NodeId& node_id, bool match_full_submap, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, + const SubmapScanMatcher& submap_scan_matcher, + std::unique_ptr* constraint) { + CHECK(submap_scan_matcher.fast_correlative_scan_matcher); + // The 'constraint_transform' (submap i <- node j) is computed from: + // - a 'high_resolution_point_cloud' in node j and + // - the initial guess 'initial_pose' (submap i <- node j). + std::unique_ptr + match_result; + + // Compute 'pose_estimate' in three stages: + // 1. Fast estimate using the fast correlative scan matcher. + // 2. Prune if the score is too low. + // 3. Refine. + if (match_full_submap) { + kGlobalConstraintsSearchedMetric->Increment(); + match_result = + submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap( + global_node_pose.rotation(), global_submap_pose.rotation(), + *constant_data, options_.global_localization_min_score()); + if (match_result != nullptr) { + CHECK_GT(match_result->score, options_.global_localization_min_score()); + CHECK_GE(node_id.trajectory_id, 0); + CHECK_GE(submap_id.trajectory_id, 0); + kGlobalConstraintsFoundMetric->Increment(); + kGlobalConstraintScoresMetric->Observe(match_result->score); + kGlobalConstraintRotationalScoresMetric->Observe( + match_result->rotational_score); + kGlobalConstraintLowResolutionScoresMetric->Observe( + match_result->low_resolution_score); + } else { + return; + } + } else { + kConstraintsSearchedMetric->Increment(); + match_result = submap_scan_matcher.fast_correlative_scan_matcher->Match( + global_node_pose, global_submap_pose, *constant_data, + options_.min_score()); + if (match_result != nullptr) { + // We've reported a successful local match. + CHECK_GT(match_result->score, options_.min_score()); + kConstraintsFoundMetric->Increment(); + kConstraintScoresMetric->Observe(match_result->score); + kConstraintRotationalScoresMetric->Observe( + match_result->rotational_score); + kConstraintLowResolutionScoresMetric->Observe( + match_result->low_resolution_score); + } else { + return; + } + } + { + absl::MutexLock locker(&mutex_); + score_histogram_.Add(match_result->score); + rotational_score_histogram_.Add(match_result->rotational_score); + low_resolution_score_histogram_.Add(match_result->low_resolution_score); + } + + // Use the CSM estimate as both the initial and previous pose. This has the + // effect that, in the absence of better information, we prefer the original + // CSM estimate. + ceres::Solver::Summary unused_summary; + transform::Rigid3d constraint_transform; + ceres_scan_matcher_.Match(match_result->pose_estimate.translation(), + match_result->pose_estimate, + {{&constant_data->high_resolution_point_cloud, + submap_scan_matcher.high_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}, + {&constant_data->low_resolution_point_cloud, + submap_scan_matcher.low_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}}, + &constraint_transform, &unused_summary); + + constraint->reset(new Constraint{ + submap_id, + node_id, + {constraint_transform, options_.loop_closure_translation_weight(), + options_.loop_closure_rotation_weight()}, + Constraint::INTER_SUBMAP}); + + if (options_.log_matches()) { + std::ostringstream info; + info << "Node " << node_id << " with " + << constant_data->high_resolution_point_cloud.size() + << " points on submap " << submap_id << std::fixed; + if (match_full_submap) { + info << " matches"; + } else { + // Compute the difference between (submap i <- node j) according to loop + // closure ('constraint_transform') and according to global SLAM state. + const transform::Rigid3d difference = global_node_pose.inverse() * + global_submap_pose * + constraint_transform; + info << " differs by translation " << std::setprecision(2) + << difference.translation().norm() << " rotation " + << std::setprecision(3) << transform::GetAngle(difference); + } + info << " with score " << std::setprecision(1) << 100. * match_result->score + << "%."; + LOG(INFO) << info.str(); + } +} + +void ConstraintBuilder3D::RunWhenDoneCallback() { + Result result; + std::unique_ptr> callback; + { + absl::MutexLock locker(&mutex_); + CHECK(when_done_ != nullptr); + for (const std::unique_ptr& constraint : constraints_) { + if (constraint == nullptr) continue; + result.push_back(*constraint); + } + if (options_.log_matches()) { + LOG(INFO) << constraints_.size() << " computations resulted in " + << result.size() << " additional constraints.\n" + << "Score histogram:\n" + << score_histogram_.ToString(10) << "\n" + << "Rotational score histogram:\n" + << rotational_score_histogram_.ToString(10) << "\n" + << "Low resolution score histogram:\n" + << low_resolution_score_histogram_.ToString(10); + } + constraints_.clear(); + callback = std::move(when_done_); + when_done_.reset(); + kQueueLengthMetric->Set(constraints_.size()); + } + (*callback)(result); +} + +int ConstraintBuilder3D::GetNumFinishedNodes() { + absl::MutexLock locker(&mutex_); + return num_finished_nodes_; +} + +void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { + absl::MutexLock locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "DeleteScanMatcher was called while WhenDone was scheduled."; + } + submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); +} + +void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) { + auto* counts = factory->NewCounterFamily( + "mapping_constraints_constraint_builder_3d_constraints", + "Constraints computed"); + kConstraintsSearchedMetric = + counts->Add({{"search_region", "local"}, {"matcher", "searched"}}); + kConstraintsFoundMetric = + counts->Add({{"search_region", "local"}, {"matcher", "found"}}); + kGlobalConstraintsSearchedMetric = + counts->Add({{"search_region", "global"}, {"matcher", "searched"}}); + kGlobalConstraintsFoundMetric = + counts->Add({{"search_region", "global"}, {"matcher", "found"}}); + auto* queue_length = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_3d_queue_length", "Queue length"); + kQueueLengthMetric = queue_length->Add({}); + auto boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = factory->NewHistogramFamily( + "mapping_constraints_constraint_builder_3d_scores", + "Constraint scores built", boundaries); + kConstraintScoresMetric = + scores->Add({{"search_region", "local"}, {"kind", "score"}}); + kConstraintRotationalScoresMetric = + scores->Add({{"search_region", "local"}, {"kind", "rotational_score"}}); + kConstraintLowResolutionScoresMetric = scores->Add( + {{"search_region", "local"}, {"kind", "low_resolution_score"}}); + kGlobalConstraintScoresMetric = + scores->Add({{"search_region", "global"}, {"kind", "score"}}); + kGlobalConstraintRotationalScoresMetric = + scores->Add({{"search_region", "global"}, {"kind", "rotational_score"}}); + kGlobalConstraintLowResolutionScoresMetric = scores->Add( + {{"search_region", "global"}, {"kind", "low_resolution_score"}}); + auto* num_matchers = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_3d_num_submap_scan_matchers", + "Current number of constructed submap scan matchers"); + kNumSubmapScanMatchersMetric = num_matchers->Add({}); +} + +} // namespace constraints +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d.h new file mode 100644 index 0000000..247a9da --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_ + +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/histogram.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/common/task.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { +namespace constraints { + +// Asynchronously computes constraints. +// +// Intermingle an arbitrary number of calls to 'MaybeAddConstraint', +// 'MaybeAddGlobalConstraint', and 'NotifyEndOfNode', then call 'WhenDone' once. +// After all computations are done the 'callback' will be called with the result +// and another MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. +// +// This class is thread-safe. +class ConstraintBuilder3D { + public: + using Constraint = mapping::PoseGraphInterface::Constraint; + using Result = std::vector; + + ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options, + common::ThreadPoolInterface* thread_pool); + ~ConstraintBuilder3D(); + + ConstraintBuilder3D(const ConstraintBuilder3D&) = delete; + ConstraintBuilder3D& operator=(const ConstraintBuilder3D&) = delete; + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. + // + // 'global_node_pose' and 'global_submap_pose' are initial estimates of poses + // in the global map frame, i.e. both are gravity aligned. + // + // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until + // all computations are finished. + void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap, + const NodeId& node_id, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose); + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. + // This performs full-submap matching. + // + // 'global_node_rotation' and 'global_submap_rotation' are initial estimates + // of roll and pitch, i.e. their yaw is essentially ignored. + // + // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until + // all computations are finished. + void MaybeAddGlobalConstraint( + const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id, + const TrajectoryNode::Data* const constant_data, + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation); + + // Must be called after all computations related to one node have been added. + void NotifyEndOfNode(); + + // Registers the 'callback' to be called with the results, after all + // computations triggered by 'MaybeAdd*Constraint' have finished. + // 'callback' is executed in the 'ThreadPool'. + void WhenDone(const std::function& callback); + + // Returns the number of consecutive finished nodes. + int GetNumFinishedNodes(); + + // Delete data related to 'submap_id'. + void DeleteScanMatcher(const SubmapId& submap_id); + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + struct SubmapScanMatcher { + const HybridGrid* high_resolution_hybrid_grid = nullptr; + const HybridGrid* low_resolution_hybrid_grid = nullptr; + std::unique_ptr + fast_correlative_scan_matcher; + std::weak_ptr creation_task_handle; + }; + + // The returned 'grid' and 'fast_correlative_scan_matcher' must only be + // accessed after 'creation_task_handle' has completed. + const SubmapScanMatcher* DispatchScanMatcherConstruction( + const SubmapId& submap_id, const Submap3D* submap) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); + + // Runs in a background thread and does computations for an additional + // constraint. + // As output, it may create a new Constraint in 'constraint'. + void ComputeConstraint(const SubmapId& submap_id, const NodeId& node_id, + bool match_full_submap, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, + const SubmapScanMatcher& submap_scan_matcher, + std::unique_ptr* constraint) + LOCKS_EXCLUDED(mutex_); + + void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_); + + const proto::ConstraintBuilderOptions options_; + common::ThreadPoolInterface* thread_pool_; + absl::Mutex mutex_; + + // 'callback' set by WhenDone(). + std::unique_ptr> when_done_ + GUARDED_BY(mutex_); + + // TODO(gaschler): Use atomics instead of mutex to access these counters. + // Number of the node in reaction to which computations are currently + // added. This is always the number of nodes seen so far, even when older + // nodes are matched against a new submap. + int num_started_nodes_ GUARDED_BY(mutex_) = 0; + + int num_finished_nodes_ GUARDED_BY(mutex_) = 0; + + std::unique_ptr finish_node_task_ GUARDED_BY(mutex_); + + std::unique_ptr when_done_task_ GUARDED_BY(mutex_); + + // Constraints currently being computed in the background. A deque is used to + // keep pointers valid when adding more entries. Constraint search results + // with below-threshold scores are also 'nullptr'. + std::deque> constraints_ GUARDED_BY(mutex_); + + // Map of dispatched or constructed scan matchers by 'submap_id'. + std::map submap_scan_matchers_ + GUARDED_BY(mutex_); + std::map per_submap_sampler_; + + scan_matching::CeresScanMatcher3D ceres_scan_matcher_; + + // Histograms of scan matcher scores. + common::Histogram score_histogram_ GUARDED_BY(mutex_); + common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_); + common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_); +}; + +} // namespace constraints +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc new file mode 100644 index 0000000..5099336 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc @@ -0,0 +1,120 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" + +#include + +#include "cartographer/common/internal/testing/thread_pool_for_testing.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/constraints/constraint_builder.h" +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace constraints { +namespace { + +class MockCallback { + public: + MOCK_METHOD1(Run, void(const ConstraintBuilder3D::Result&)); +}; + +class ConstraintBuilder3DTest : public ::testing::Test { + protected: + void SetUp() override { + auto constraint_builder_parameters = testing::ResolveLuaParameters(R"text( + include "pose_graph.lua" + POSE_GRAPH.constraint_builder.sampling_ratio = 1 + POSE_GRAPH.constraint_builder.min_score = 0 + POSE_GRAPH.constraint_builder.global_localization_min_score = 0 + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.min_low_resolution_score = 0 + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.min_rotational_score = 0 + return POSE_GRAPH.constraint_builder)text"); + constraint_builder_ = absl::make_unique( + CreateConstraintBuilderOptions(constraint_builder_parameters.get()), + &thread_pool_); + } + + std::unique_ptr constraint_builder_; + MockCallback mock_; + common::testing::ThreadPoolForTesting thread_pool_; +}; + +TEST_F(ConstraintBuilder3DTest, CallsBack) { + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 0); + EXPECT_CALL(mock_, Run(::testing::IsEmpty())); + constraint_builder_->NotifyEndOfNode(); + constraint_builder_->WhenDone( + [this](const constraints::ConstraintBuilder3D::Result& result) { + mock_.Run(result); + }); + thread_pool_.WaitUntilIdle(); + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 1); +} + +TEST_F(ConstraintBuilder3DTest, FindsConstraints) { + auto node_data = std::make_shared(); + node_data->gravity_alignment = Eigen::Quaterniond::Identity(); + node_data->high_resolution_point_cloud.push_back( + {Eigen::Vector3f(0.1, 0.2, 0.3)}); + node_data->low_resolution_point_cloud.push_back( + {Eigen::Vector3f(0.1, 0.2, 0.3)}); + node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3); + node_data->local_pose = transform::Rigid3d::Identity(); + SubmapId submap_id{0, 1}; + Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity(), + Eigen::VectorXf::Zero(3)); + int expected_nodes = 0; + for (int i = 0; i < 2; ++i) { + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); + for (int j = 0; j < 2; ++j) { + constraint_builder_->MaybeAddConstraint( + submap_id, &submap, NodeId{0, 0}, node_data.get(), + transform::Rigid3d::Identity(), transform::Rigid3d::Identity()); + } + constraint_builder_->MaybeAddGlobalConstraint( + submap_id, &submap, NodeId{0, 0}, node_data.get(), + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity()); + constraint_builder_->NotifyEndOfNode(); + thread_pool_.WaitUntilIdle(); + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes); + constraint_builder_->NotifyEndOfNode(); + thread_pool_.WaitUntilIdle(); + EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes); + EXPECT_CALL(mock_, + Run(::testing::AllOf( + ::testing::SizeIs(3), + ::testing::Each(::testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))))); + constraint_builder_->WhenDone( + [this](const constraints::ConstraintBuilder3D::Result& result) { + mock_.Run(result); + }); + thread_pool_.WaitUntilIdle(); + constraint_builder_->DeleteScanMatcher(submap_id); + } +} + +} // namespace +} // namespace constraints +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc new file mode 100644 index 0000000..e0edcb7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc @@ -0,0 +1,28 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" + +namespace cartographer { +namespace mapping { + +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b) { + return Eigen::Quaterniond::FromTwoVectors(a, b); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h new file mode 100644 index 0000000..e8d04b3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h @@ -0,0 +1,34 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ +#define CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ + +#include "Eigen/Geometry" + +namespace cartographer { +namespace mapping { + +// Calls Eigen::Quaterniond::FromTwoVectors(). This is in its own compilation +// unit since it can take more than 10 s to build while using more than 1 GB of +// memory causing slow build times and high peak memory usage. +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/global_trajectory_builder.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/global_trajectory_builder.cc new file mode 100644 index 0000000..a60e88a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/global_trajectory_builder.cc @@ -0,0 +1,180 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/global_trajectory_builder.h" + +#include + +#include "absl/memory/memory.h" +#include "absl/types/optional.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/internal/motion_filter.h" +#include "cartographer/metrics/family_factory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace { + +static auto* kLocalSlamMatchingResults = metrics::Counter::Null(); +static auto* kLocalSlamInsertionResults = metrics::Counter::Null(); + +template +class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { + public: + // Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no + // 'TimedPointCloudData' may be added in that case. + GlobalTrajectoryBuilder( + std::unique_ptr local_trajectory_builder, + const int trajectory_id, PoseGraph* const pose_graph, + const LocalSlamResultCallback& local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) + : trajectory_id_(trajectory_id), + pose_graph_(pose_graph), + local_trajectory_builder_(std::move(local_trajectory_builder)), + local_slam_result_callback_(local_slam_result_callback), + pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {} + ~GlobalTrajectoryBuilder() override {} + + GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; + GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; + + void AddSensorData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data) override { + CHECK(local_trajectory_builder_) + << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder."; + std::unique_ptr + matching_result = local_trajectory_builder_->AddRangeData( + sensor_id, timed_point_cloud_data); + if (matching_result == nullptr) { + // The range data has not been fully accumulated yet. + return; + } + kLocalSlamMatchingResults->Increment(); + std::unique_ptr insertion_result; + if (matching_result->insertion_result != nullptr) { + kLocalSlamInsertionResults->Increment(); + auto node_id = pose_graph_->AddNode( + matching_result->insertion_result->constant_data, trajectory_id_, + matching_result->insertion_result->insertion_submaps); + CHECK_EQ(node_id.trajectory_id, trajectory_id_); + insertion_result = absl::make_unique(InsertionResult{ + node_id, matching_result->insertion_result->constant_data, + std::vector>( + matching_result->insertion_result->insertion_submaps.begin(), + matching_result->insertion_result->insertion_submaps.end())}); + } + if (local_slam_result_callback_) { + local_slam_result_callback_( + trajectory_id_, matching_result->time, matching_result->local_pose, + std::move(matching_result->range_data_in_local), + std::move(insertion_result)); + } + } + + void AddSensorData(const std::string& sensor_id, + const sensor::ImuData& imu_data) override { + if (local_trajectory_builder_) { + local_trajectory_builder_->AddImuData(imu_data); + } + pose_graph_->AddImuData(trajectory_id_, imu_data); + } + + void AddSensorData(const std::string& sensor_id, + const sensor::OdometryData& odometry_data) override { + CHECK(odometry_data.pose.IsValid()) << odometry_data.pose; + if (local_trajectory_builder_) { + local_trajectory_builder_->AddOdometryData(odometry_data); + } + // TODO(MichaelGrupp): Instead of having an optional filter on this level, + // odometry could be marginalized between nodes in the pose graph. + // Related issue: cartographer-project/cartographer/#1768 + if (pose_graph_odometry_motion_filter_.has_value() && + pose_graph_odometry_motion_filter_.value().IsSimilar( + odometry_data.time, odometry_data.pose)) { + return; + } + pose_graph_->AddOdometryData(trajectory_id_, odometry_data); + } + + void AddSensorData( + const std::string& sensor_id, + const sensor::FixedFramePoseData& fixed_frame_pose) override { + if (fixed_frame_pose.pose.has_value()) { + CHECK(fixed_frame_pose.pose.value().IsValid()) + << fixed_frame_pose.pose.value(); + } + pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); + } + + void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) override { + pose_graph_->AddLandmarkData(trajectory_id_, landmark_data); + } + + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override { + CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with " + "local_trajectory_builder_ present."; + local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_); + } + + private: + const int trajectory_id_; + PoseGraph* const pose_graph_; + std::unique_ptr local_trajectory_builder_; + LocalSlamResultCallback local_slam_result_callback_; + absl::optional pose_graph_odometry_motion_filter_; +}; + +} // namespace + +std::unique_ptr CreateGlobalTrajectoryBuilder2D( + std::unique_ptr local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph2D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) { + return absl::make_unique< + GlobalTrajectoryBuilder>( + std::move(local_trajectory_builder), trajectory_id, pose_graph, + local_slam_result_callback, pose_graph_odometry_motion_filter); +} + +std::unique_ptr CreateGlobalTrajectoryBuilder3D( + std::unique_ptr local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph3D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) { + return absl::make_unique< + GlobalTrajectoryBuilder>( + std::move(local_trajectory_builder), trajectory_id, pose_graph, + local_slam_result_callback, pose_graph_odometry_motion_filter); +} + +void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory* factory) { + auto* results = factory->NewCounterFamily( + "mapping_global_trajectory_builder_local_slam_results", + "Local SLAM results"); + kLocalSlamMatchingResults = results->Add({{"type", "MatchingResult"}}); + kLocalSlamInsertionResults = results->Add({{"type", "InsertionResult"}}); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/global_trajectory_builder.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/global_trajectory_builder.h new file mode 100644 index 0000000..c0980f5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/global_trajectory_builder.h @@ -0,0 +1,53 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_GLOBAL_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_GLOBAL_TRAJECTORY_BUILDER_H_ + +#include + +#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" +#include "cartographer/mapping/internal/2d/pose_graph_2d.h" +#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/metrics/family_factory.h" + +namespace cartographer { +namespace mapping { + +std::unique_ptr CreateGlobalTrajectoryBuilder2D( + std::unique_ptr local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph2D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter); + +std::unique_ptr CreateGlobalTrajectoryBuilder3D( + std::unique_ptr local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph3D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter); + +void GlobalTrajectoryBuilderRegisterMetrics( + metrics::FamilyFactory* family_factory); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_GLOBAL_TRAJECTORY_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/imu_based_pose_extrapolator.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/imu_based_pose_extrapolator.cc new file mode 100644 index 0000000..ddb88ea --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/imu_based_pose_extrapolator.cc @@ -0,0 +1,439 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/mapping/internal/3d/imu_integration.h" +#include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +using ::cartographer::transform::TimestampedTransform; + +ImuBasedPoseExtrapolator::ImuBasedPoseExtrapolator( + const proto::ImuBasedPoseExtrapolatorOptions& options) + : options_(options), + solver_options_( + common::CreateCeresSolverOptions(options_.solver_options())) {} + +ImuBasedPoseExtrapolator::~ImuBasedPoseExtrapolator() { + LOG(INFO) << "Number of iterations for pose extrapolation:"; + LOG(INFO) << num_iterations_hist_.ToString(10); +} + +std::unique_ptr +ImuBasedPoseExtrapolator::InitializeWithImu( + const proto::ImuBasedPoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses) { + CHECK(!imu_data.empty()); + LOG(INFO) << options.DebugString(); + auto extrapolator = absl::make_unique(options); + std::copy(imu_data.begin(), imu_data.end(), + std::back_inserter(extrapolator->imu_data_)); + if (!initial_poses.empty()) { + for (const auto& pose : initial_poses) { + if (pose.time > imu_data.front().time) { + extrapolator->AddPose(pose.time, pose.transform); + } + } + } else { + extrapolator->AddPose( + imu_data.back().time, + transform::Rigid3d::Rotation(FromTwoVectors( + imu_data.back().linear_acceleration, Eigen::Vector3d::UnitZ()))); + } + return extrapolator; +} + +common::Time ImuBasedPoseExtrapolator::GetLastPoseTime() const { + if (timed_pose_queue_.empty()) { + return common::Time::min(); + } + return timed_pose_queue_.back().time; +} + +common::Time ImuBasedPoseExtrapolator::GetLastExtrapolatedTime() const { + return last_extrapolated_time_; +} + +void ImuBasedPoseExtrapolator::AddPose(const common::Time time, + const transform::Rigid3d& pose) { + timed_pose_queue_.push_back(TimestampedTransform{time, pose}); + while (timed_pose_queue_.size() > 3 && + timed_pose_queue_[1].time <= + time - common::FromSeconds(options_.pose_queue_duration())) { + if (!previous_solution_.empty()) { + CHECK_EQ(timed_pose_queue_.front().time, previous_solution_.front().time); + previous_solution_.pop_front(); + } + timed_pose_queue_.pop_front(); + } + TrimImuData(); +} + +void ImuBasedPoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) { + CHECK(timed_pose_queue_.empty() || + imu_data.time >= timed_pose_queue_.back().time); + imu_data_.push_back(imu_data); + TrimImuData(); +} + +void ImuBasedPoseExtrapolator::AddOdometryData( + const sensor::OdometryData& odometry_data) { + CHECK(timed_pose_queue_.empty() || + odometry_data.time >= timed_pose_queue_.back().time); + odometry_data_.push_back(odometry_data); + TrimOdometryData(); +} + +ImuBasedPoseExtrapolator::ExtrapolationResult +ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + const auto& time = times.back(); + const auto& newest_timed_pose = timed_pose_queue_.back(); + CHECK_GE(time, newest_timed_pose.time); + CHECK_GE(times.size(), 1); + last_extrapolated_time_ = time; + + if (timed_pose_queue_.size() < 3 || + common::ToSeconds(time - newest_timed_pose.time) < 1e-6) { + return ExtrapolationResult{ + std::vector( + times.size() - 1, timed_pose_queue_.back().transform.cast()), + timed_pose_queue_.back().transform, Eigen::Vector3d::Zero(), + timed_pose_queue_.back().transform.rotation()}; + } + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + // Track gravity alignment over time and use this as a frame here so that + // we can estimate the gravity alignment of the current pose. + optimization::CeresPose gravity_from_local( + gravity_from_local_, nullptr, + absl::make_unique(), &problem); + // Use deque so addresses stay constant during problem formulation. + std::deque nodes; + std::vector node_times; + + for (size_t i = 0; i < timed_pose_queue_.size(); i++) { + const bool is_last = (i == (timed_pose_queue_.size() - 1)); + const auto& timed_pose = timed_pose_queue_[i]; + node_times.push_back(timed_pose.time); + + transform::Rigid3d gravity_from_node; + // Use the last scan match result (timed_pose_queue_.back()) for + // initialization here instead of the last result from the optimization. + // This keeps poses from slowly drifting apart due to lack of feedback + // from the scan matching here. + if (!previous_solution_.empty() && !is_last) { + CHECK_GT(previous_solution_.size(), i); + CHECK_EQ(timed_pose.time, previous_solution_[i].time); + gravity_from_node = previous_solution_[i].transform; + } else { + gravity_from_node = gravity_from_local_ * timed_pose.transform; + } + + if (is_last) { + nodes.emplace_back(gravity_from_node, nullptr, + absl::make_unique>(), + &problem); + problem.SetParameterBlockConstant(nodes.back().translation()); + } else { + nodes.emplace_back(gravity_from_node, nullptr, + absl::make_unique(), + &problem); + } + } + + double gravity_constant = 9.8; + bool fix_gravity = false; + if (options_.gravity_constant() > 0) { + fix_gravity = true; + gravity_constant = options_.gravity_constant(); + } + + auto imu_it_prev_prev = imu_data_.begin(); + while (imu_it_prev_prev != std::prev(imu_data_.end()) && + std::next(imu_it_prev_prev)->time <= timed_pose_queue_.back().time) { + ++imu_it_prev_prev; + } + + const TimestampedTransform prev_gravity_from_tracking = + TimestampedTransform{node_times.back(), nodes.back().ToRigid()}; + const TimestampedTransform prev_prev_gravity_from_tracking = + TimestampedTransform{node_times.at(node_times.size() - 2), + nodes.at(nodes.size() - 2).ToRigid()}; + const transform::Rigid3d initial_estimate = + ExtrapolatePoseWithImu( + prev_gravity_from_tracking.transform, prev_gravity_from_tracking.time, + prev_prev_gravity_from_tracking.transform, + prev_prev_gravity_from_tracking.time, + gravity_constant * Eigen::Vector3d::UnitZ(), time, imu_data_, + &imu_it_prev_prev) + .pose; + nodes.emplace_back(initial_estimate, nullptr, + absl::make_unique(), + &problem); + node_times.push_back(time); + + // Add cost functions for node constraints. + for (size_t i = 0; i < timed_pose_queue_.size(); i++) { + const auto& timed_pose = timed_pose_queue_[i]; + problem.AddResidualBlock( + optimization::SpaCostFunction3D::CreateAutoDiffCostFunction( + PoseGraphInterface::Constraint::Pose{ + timed_pose.transform, options_.pose_translation_weight(), + options_.pose_rotation_weight()}), + nullptr /* loss function */, gravity_from_local.rotation(), + gravity_from_local.translation(), nodes.at(i).rotation(), + nodes.at(i).translation()); + } + + CHECK(!imu_data_.empty()); + CHECK_LE(imu_data_.front().time, timed_pose_queue_.front().time); + + std::array imu_calibration{{1., 0., 0., 0.}}; + + problem.AddParameterBlock(imu_calibration.data(), 4, + new ceres::QuaternionParameterization()); + problem.SetParameterBlockConstant(imu_calibration.data()); + + auto imu_it = imu_data_.begin(); + CHECK(imu_data_.size() == 1 || + std::next(imu_it)->time > timed_pose_queue_.front().time); + + transform::Rigid3d last_node_odometry; + common::Time last_node_odometry_time; + + for (size_t i = 1; i < nodes.size(); i++) { + const common::Time first_time = node_times[i - 1]; + const common::Time second_time = node_times[i]; + + auto imu_it2 = imu_it; + const IntegrateImuResult result = + IntegrateImu(imu_data_, first_time, second_time, &imu_it); + if ((i + 1) < nodes.size()) { + const common::Time third_time = node_times[i + 1]; + const common::Duration first_duration = second_time - first_time; + const common::Duration second_duration = third_time - second_time; + const common::Time first_center = first_time + first_duration / 2; + const common::Time second_center = second_time + second_duration / 2; + const IntegrateImuResult result_to_first_center = + IntegrateImu(imu_data_, first_time, first_center, &imu_it2); + const IntegrateImuResult result_center_to_center = + IntegrateImu(imu_data_, first_center, second_center, &imu_it2); + // 'delta_velocity' is the change in velocity from the point in time + // halfway between the first and second poses to halfway between + // second and third pose. It is computed from IMU data and still + // contains a delta due to gravity. The orientation of this vector is + // in the IMU frame at the second pose. + const Eigen::Vector3d delta_velocity = + (result.delta_rotation.inverse() * + result_to_first_center.delta_rotation) * + result_center_to_center.delta_velocity; + problem.AddResidualBlock( + AccelerationCostFunction3D::CreateAutoDiffCostFunction( + options_.imu_acceleration_weight(), delta_velocity, + common::ToSeconds(first_duration), + common::ToSeconds(second_duration)), + nullptr /* loss function */, nodes.at(i).rotation(), + nodes.at(i - 1).translation(), nodes.at(i).translation(), + nodes.at(i + 1).translation(), &gravity_constant, + imu_calibration.data()); + // TODO(danielsievers): Fix gravity in CostFunction. + if (fix_gravity) { + problem.SetParameterBlockConstant(&gravity_constant); + } else { + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&gravity_constant, 0, 0.0); + } + } + problem.AddResidualBlock( + RotationCostFunction3D::CreateAutoDiffCostFunction( + options_.imu_rotation_weight(), result.delta_rotation), + nullptr /* loss function */, nodes.at(i - 1).rotation(), + nodes.at(i).rotation(), imu_calibration.data()); + + // Add a relative pose constraint based on the odometry (if available). + if (HasOdometryDataForTime(first_time) && + HasOdometryDataForTime(second_time)) { + // Here keep track of last node odometry to avoid double computation. + // Do this if first loop, or if there were some missing odometry nodes + // then recalculate. + if (i == 1 || last_node_odometry_time != first_time) { + last_node_odometry = InterpolateOdometry(first_time); + last_node_odometry_time = first_time; + } + const transform::Rigid3d current_node_odometry = + InterpolateOdometry(second_time); + const transform::Rigid3d relative_odometry = + CalculateOdometryBetweenNodes(last_node_odometry, + current_node_odometry); + + problem.AddResidualBlock( + optimization::SpaCostFunction3D::CreateAutoDiffCostFunction( + PoseGraphInterface::Constraint::Pose{ + relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, nodes.at(i - 1).rotation(), + nodes.at(i - 1).translation(), nodes.at(i).rotation(), + nodes.at(i).translation()); + // Use the current node odometry in the next iteration + last_node_odometry = current_node_odometry; + last_node_odometry_time = second_time; + } + } + + // Solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options_, &problem, &summary); + LOG_IF_EVERY_N(INFO, !fix_gravity, 20) << "Gravity was: " << gravity_constant; + + const auto gravity_estimate = nodes.back().ToRigid().rotation(); + + const auto& last_pose = timed_pose_queue_.back(); + const auto extrapolated_pose = TimestampedTransform{ + time, last_pose.transform * + nodes.at(nodes.size() - 2).ToRigid().inverse() * + nodes.back().ToRigid()}; + + num_iterations_hist_.Add(summary.iterations.size()); + + gravity_from_local_ = gravity_from_local.ToRigid(); + + previous_solution_.clear(); + for (size_t i = 0; i < nodes.size(); ++i) { + previous_solution_.push_back( + TimestampedTransform{node_times.at(i), nodes.at(i).ToRigid()}); + } + + const Eigen::Vector3d current_velocity = + (extrapolated_pose.transform.translation() - + last_pose.transform.translation()) / + common::ToSeconds(time - last_pose.time); + + return ExtrapolationResult{ + InterpolatePoses(last_pose, extrapolated_pose, times.begin(), + std::prev(times.end())), + extrapolated_pose.transform, current_velocity, gravity_estimate}; +} + +std::vector ImuBasedPoseExtrapolator::InterpolatePoses( + const TimestampedTransform& start, const TimestampedTransform& end, + const std::vector::const_iterator times_begin, + const std::vector::const_iterator times_end) { + std::vector poses; + poses.reserve(std::distance(times_begin, times_end)); + const float duration_scale = 1. / common::ToSeconds(end.time - start.time); + + const Eigen::Quaternionf start_rotation = + Eigen::Quaternionf(start.transform.rotation()); + const Eigen::Quaternionf end_rotation = + Eigen::Quaternionf(end.transform.rotation()); + const Eigen::Vector3f start_translation = + start.transform.translation().cast(); + const Eigen::Vector3f end_translation = + end.transform.translation().cast(); + + for (auto it = times_begin; it != times_end; ++it) { + const float factor = common::ToSeconds(*it - start.time) * duration_scale; + const Eigen::Vector3f origin = + start_translation + (end_translation - start_translation) * factor; + const Eigen::Quaternionf rotation = + start_rotation.slerp(factor, end_rotation); + poses.emplace_back(origin, rotation); + } + return poses; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::ExtrapolatePose( + const common::Time time) { + return ExtrapolatePosesWithGravity(std::vector{time}) + .current_pose; +} + +Eigen::Quaterniond ImuBasedPoseExtrapolator::EstimateGravityOrientation( + const common::Time time) { + return ExtrapolatePosesWithGravity(std::vector{time}) + .gravity_from_tracking; +} + +template +void ImuBasedPoseExtrapolator::TrimDequeData(std::deque* data) { + while (data->size() > 1 && !timed_pose_queue_.empty() && + data->at(1).time <= timed_pose_queue_.front().time) { + data->pop_front(); + } +} + +void ImuBasedPoseExtrapolator::TrimImuData() { + TrimDequeData(&imu_data_); +} + +void ImuBasedPoseExtrapolator::TrimOdometryData() { + TrimDequeData(&odometry_data_); +} + +// Odometry methods +bool ImuBasedPoseExtrapolator::HasOdometryData() const { + return odometry_data_.size() >= 2; +} + +bool ImuBasedPoseExtrapolator::HasOdometryDataForTime( + const common::Time& time) const { + return HasOdometryData() && odometry_data_.front().time < time && + time < odometry_data_.back().time; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::InterpolateOdometry( + const common::Time& time) const { + // Only interpolate if time is within odometry data range. + CHECK(HasOdometryDataForTime(time)) + << "Odometry data range does not include time " << time; + std::deque::const_iterator data = std::upper_bound( + odometry_data_.begin(), odometry_data_.end(), time, + [](const common::Time& time, const sensor::OdometryData& odometry_data) { + return time < odometry_data.time; + }); + const TimestampedTransform first{std::prev(data)->time, + std::prev(data)->pose}; + const TimestampedTransform second{data->time, data->pose}; + return Interpolate(first, second, time).transform; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::CalculateOdometryBetweenNodes( + const transform::Rigid3d& first_node_odometry, + const transform::Rigid3d& second_node_odometry) const { + return first_node_odometry.inverse() * second_node_odometry; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/imu_based_pose_extrapolator.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/imu_based_pose_extrapolator.h new file mode 100644 index 0000000..6adc9b7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/imu_based_pose_extrapolator.h @@ -0,0 +1,102 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ +#define CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ + +#include +#include +#include + +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/histogram.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +// Uses the linear acceleration and rotational velocities to estimate a pose. +class ImuBasedPoseExtrapolator : public PoseExtrapolatorInterface { + public: + explicit ImuBasedPoseExtrapolator( + const proto::ImuBasedPoseExtrapolatorOptions& options); + ~ImuBasedPoseExtrapolator() override; + + static std::unique_ptr InitializeWithImu( + const proto::ImuBasedPoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; + + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; + + // Gravity alignment estimate. + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; + + private: + template + void TrimDequeData(std::deque* data); + + void TrimImuData(); + void TrimOdometryData(); + + // Odometry methods. + bool HasOdometryData() const; + bool HasOdometryDataForTime(const common::Time& first_time) const; + transform::Rigid3d InterpolateOdometry(const common::Time& first_time) const; + transform::Rigid3d CalculateOdometryBetweenNodes( + const transform::Rigid3d& first_node_odometry, + const transform::Rigid3d& second_node_odometry) const; + + std::vector InterpolatePoses( + const ::cartographer::transform::TimestampedTransform& start, + const ::cartographer::transform::TimestampedTransform& end, + const std::vector::const_iterator times_begin, + const std::vector::const_iterator times_end); + + std::deque<::cartographer::transform::TimestampedTransform> timed_pose_queue_; + std::deque<::cartographer::transform::TimestampedTransform> + previous_solution_; + + std::deque imu_data_; + std::deque odometry_data_; + common::Time last_extrapolated_time_ = common::Time::min(); + + transform::Rigid3d gravity_from_local_ = transform::Rigid3d::Identity(); + + const proto::ImuBasedPoseExtrapolatorOptions options_; + const ceres::Solver::Options solver_options_; + + common::Histogram num_iterations_hist_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/local_slam_result_data.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/local_slam_result_data.h new file mode 100644 index 0000000..2af12cf --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/local_slam_result_data.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H_ +#define CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H_ + +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace mapping { + +class LocalSlamResultData : public sensor::Data { + public: + LocalSlamResultData(const std::string& sensor_id, common::Time time) + : Data(sensor_id), time_(time) {} + + common::Time GetTime() const override { return time_; } + virtual void AddToPoseGraph(int trajectory_id, + PoseGraph* pose_graph) const = 0; + + private: + common::Time time_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter.cc new file mode 100644 index 0000000..a328206 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter.cc @@ -0,0 +1,140 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include "cartographer/mapping/internal/motion_filter.h" +#include "Eigen/Core" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" +#include +#include +int num_origin=1; +int num_little=1; +namespace cartographer { +namespace mapping { + +proto::MotionFilterOptions CreateMotionFilterOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::MotionFilterOptions options; + options.set_max_time_seconds( + parameter_dictionary->GetDouble("max_time_seconds")); + options.set_max_distance_meters( + parameter_dictionary->GetDouble("max_distance_meters")); + options.set_max_angle_radians( + parameter_dictionary->GetDouble("max_angle_radians")); + return options; +} + +MotionFilter::MotionFilter(const proto::MotionFilterOptions& options) + : options_(options) {} + +bool MotionFilter::IsSimilar(const common::Time time, + const transform::Rigid3d& pose) { + LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500) + << "Motion filter reduced the number of nodes to " + << 100. * num_different_ / num_total_ << "%."; + ++num_total_; + + if (transform::GetAngle(pose.inverse() * last_pose_) > 0.02) { + last_time_ = time; + last_pose_ = pose; + //++num_different_; + return true; + } + if (num_total_ > 1 && + time - last_time_ <= common::FromSeconds(0.8) && + (pose.translation() - last_pose_.translation()).norm() <= + options_.max_distance_meters() && + transform::GetAngle(pose.inverse() * last_pose_) <= + options_.max_angle_radians() ) { + //认为小车没动 所以没有记录上一时刻的位姿 + return true; + } + + last_time_ = time; + last_pose_ = pose; + ++num_different_; + return false; +} + +bool MotionFilter::IsSimilarNew(const common::Time time, + const transform::Rigid3d& pose, + //引用传递和指针传递的区别？ + // ... const sensor::RangeData& range_data + sensor::RangeData& range_data) { + + LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500) + << "Motion filter reduced the number of nodes to " + << 100. * num_different_ / num_total_ << "%."; + ++num_total_; + + //无论是否检测到小物体，摒弃旋转时的雷达帧 + if (transform::GetAngle(pose.inverse() * last_pose_) > 0.02) { + last_time_ = time; + last_pose_ = pose; + return true; + } + + if(num_total_ <= 1 || + time - last_time_ > common::FromSeconds(options_.max_time_seconds()) || + (pose.translation() - last_pose_.translation()).norm() > options_.max_distance_meters() || + transform::GetAngle(pose.inverse() * last_pose_) > options_.max_angle_radians()) { + + last_time_ = time; + last_pose_ = pose; + ++num_different_; + return false; + } + + //检测该雷达帧是否含有小物体 + int flag = 0; + for(int i = 0; i < range_data.returns.intensities().size();i++){ + if(range_data.returns.intensities()[i] > 100){ + flag = 1; + break; + } + } + if(flag == 0){ + return true; + } + + + if(num_total_ <= 1 || + time - sem_last_time_ > common::FromSeconds(0.3) || + (pose.translation() - last_pose_.translation()).norm() > options_.max_distance_meters() || + transform::GetAngle(pose.inverse() * last_pose_) > options_.max_angle_radians()) { + + for(int i = 0;i 100) + continue; + else{ + //range_data.returns.pointreturn().erase(range_data.returns.pointreturn().begin()+i); + range_data.returns.pointreturn()[i].position.fill(0); + } + } + sem_last_time_ = time; + //last_pose_ = pose; + ++num_different_; + return false; + } + else{ + return true; + } +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter.h new file mode 100644 index 0000000..7dc9a36 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter.h @@ -0,0 +1,57 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_MOTION_FILTER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_MOTION_FILTER_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/motion_filter_options.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/sensor/point_cloud.h" +namespace cartographer { +namespace mapping { + +proto::MotionFilterOptions CreateMotionFilterOptions( + common::LuaParameterDictionary* parameter_dictionary); + +// Takes poses as input and filters them to get fewer poses. +class MotionFilter { + public: + explicit MotionFilter(const proto::MotionFilterOptions& options); + + // If the accumulated motion (linear, rotational, or time) is above the + // threshold, returns false. Otherwise the relative motion is accumulated and + // true is returned. + bool IsSimilar(common::Time time, const transform::Rigid3d& pose); + bool IsSimilarNew(common::Time time, const transform::Rigid3d& pose,sensor::RangeData& range_data_in_local); + + private: + const proto::MotionFilterOptions options_; + int num_total_ = 0; + int num_different_ = 0; + common::Time last_time_; + common::Time sem_last_time_; + transform::Rigid3d last_pose_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_MOTION_FILTER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter_test.cc new file mode 100644 index 0000000..c5002fa --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/motion_filter_test.cc @@ -0,0 +1,109 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/motion_filter.h" + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +class MotionFilterTest : public ::testing::Test { + protected: + MotionFilterTest() { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "max_time_seconds = 0.5, " + "max_distance_meters = 0.2, " + "max_angle_radians = 2., " + "}"); + options_ = CreateMotionFilterOptions(parameter_dictionary.get()); + } + + common::Time SecondsSinceEpoch(int seconds) { + return common::FromUniversal(seconds * 10000000); + } + + proto::MotionFilterOptions options_; +}; + +TEST_F(MotionFilterTest, NotInitialized) { + MotionFilter motion_filter(options_); + EXPECT_FALSE( + motion_filter.IsSimilar(common::Time(), transform::Rigid3d::Identity())); +} + +TEST_F(MotionFilterTest, NoChange) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); +} + +TEST_F(MotionFilterTest, TimeElapsed) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(43), + transform::Rigid3d::Identity())); + EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(43), + transform::Rigid3d::Identity())); +} + +TEST_F(MotionFilterTest, LinearMotion) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0., 0.)))); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.45, 0., 0.)))); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0., 0.)))); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0.15, 0.)))); +} + +TEST_F(MotionFilterTest, RotationalMotion) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 1.9, Eigen::Vector3d::UnitY())))); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 2.1, Eigen::Vector3d::UnitY())))); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 4., Eigen::Vector3d::UnitY())))); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 5.9, Eigen::Vector3d::UnitY())))); + EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/ceres_pose.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/ceres_pose.cc new file mode 100644 index 0000000..807a67d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/ceres_pose.cc @@ -0,0 +1,48 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/optimization/ceres_pose.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +CeresPose::Data FromPose(const transform::Rigid3d& pose) { + return CeresPose::Data{{{pose.translation().x(), pose.translation().y(), + pose.translation().z()}}, + {{pose.rotation().w(), pose.rotation().x(), + pose.rotation().y(), pose.rotation().z()}}}; +} + +CeresPose::CeresPose( + const transform::Rigid3d& pose, + std::unique_ptr translation_parametrization, + std::unique_ptr rotation_parametrization, + ceres::Problem* problem) + : data_(std::make_shared(FromPose(pose))) { + problem->AddParameterBlock(data_->translation.data(), 3, + translation_parametrization.release()); + problem->AddParameterBlock(data_->rotation.data(), 4, + rotation_parametrization.release()); +} + +const transform::Rigid3d CeresPose::ToRigid() const { + return transform::Rigid3d::FromArrays(data_->rotation, data_->translation); +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/ceres_pose.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/ceres_pose.h new file mode 100644 index 0000000..d852d80 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/ceres_pose.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +class CeresPose { + public: + CeresPose( + const transform::Rigid3d& rigid, + std::unique_ptr translation_parametrization, + std::unique_ptr rotation_parametrization, + ceres::Problem* problem); + + const transform::Rigid3d ToRigid() const; + + double* translation() { return data_->translation.data(); } + const double* translation() const { return data_->translation.data(); } + + double* rotation() { return data_->rotation.data(); } + const double* rotation() const { return data_->rotation.data(); } + + struct Data { + std::array translation; + // Rotation quaternion as (w, x, y, z). + std::array rotation; + }; + + Data& data() { return *data_; } + + private: + std::shared_ptr data_; +}; + +CeresPose::Data FromPose(const transform::Rigid3d& pose); + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h new file mode 100644 index 0000000..20325f1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h @@ -0,0 +1,104 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { + +// Penalizes differences between IMU data and optimized accelerations. +class AccelerationCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, + const Eigen::Vector3d& delta_velocity_imu_frame, + const double first_delta_time_seconds, + const double second_delta_time_seconds) { + return new ceres::AutoDiffCostFunction< + AccelerationCostFunction3D, 3 /* residuals */, + 4 /* rotation variables */, 3 /* position variables */, + 3 /* position variables */, 3 /* position variables */, + 1 /* gravity variables */, 4 /* rotation variables */>( + new AccelerationCostFunction3D(scaling_factor, delta_velocity_imu_frame, + first_delta_time_seconds, + second_delta_time_seconds)); + } + + template + bool operator()(const T* const middle_rotation, const T* const start_position, + const T* const middle_position, const T* const end_position, + const T* const gravity_constant, + const T* const imu_calibration, T* residual) const { + const Eigen::Quaternion eigen_imu_calibration( + imu_calibration[0], imu_calibration[1], imu_calibration[2], + imu_calibration[3]); + const Eigen::Matrix imu_delta_velocity = + ToEigen(middle_rotation) * eigen_imu_calibration * + delta_velocity_imu_frame_.cast() - + *gravity_constant * + (0.5 * (first_delta_time_seconds_ + second_delta_time_seconds_) * + Eigen::Vector3d::UnitZ()) + .cast(); + const Eigen::Matrix start_velocity = + (Eigen::Map>(middle_position) - + Eigen::Map>(start_position)) / + T(first_delta_time_seconds_); + const Eigen::Matrix end_velocity = + (Eigen::Map>(end_position) - + Eigen::Map>(middle_position)) / + T(second_delta_time_seconds_); + const Eigen::Matrix delta_velocity = end_velocity - start_velocity; + + (Eigen::Map>(residual) = + T(scaling_factor_) * (imu_delta_velocity - delta_velocity)); + return true; + } + + private: + AccelerationCostFunction3D(const double scaling_factor, + const Eigen::Vector3d& delta_velocity_imu_frame, + const double first_delta_time_seconds, + const double second_delta_time_seconds) + : scaling_factor_(scaling_factor), + delta_velocity_imu_frame_(delta_velocity_imu_frame), + first_delta_time_seconds_(first_delta_time_seconds), + second_delta_time_seconds_(second_delta_time_seconds) {} + + AccelerationCostFunction3D(const AccelerationCostFunction3D&) = delete; + AccelerationCostFunction3D& operator=(const AccelerationCostFunction3D&) = + delete; + + template + static Eigen::Quaternion ToEigen(const T* const quaternion) { + return Eigen::Quaternion(quaternion[0], quaternion[1], quaternion[2], + quaternion[3]); + } + + const double scaling_factor_; + const Eigen::Vector3d delta_velocity_imu_frame_; + const double first_delta_time_seconds_; + const double second_delta_time_seconds_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h new file mode 100644 index 0000000..8b54e29 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h @@ -0,0 +1,89 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +// Computes the error between the given relative pose and the difference of +// poses 'start' and 'end' which are both in an arbitrary common frame. +// +// 'start' and 'end' poses have the format [x, y, rotation]. +template +static std::array ComputeUnscaledError( + const transform::Rigid2d& relative_pose, const T* const start, + const T* const end); +template +std::array ScaleError(const std::array& error, + double translation_weight, double rotation_weight); + +// Computes the error between the given relative pose and the difference of +// poses 'start' and 'end' which are both in an arbitrary common frame. +// +// 'start' and 'end' translation has the format [x, y, z]. +// 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3]. +template +static std::array ComputeUnscaledError( + const transform::Rigid3d& relative_pose, const T* const start_rotation, + const T* const start_translation, const T* const end_rotation, + const T* const end_translation); + +template +std::array ScaleError(const std::array& error, + double translation_weight, double rotation_weight); + +// Computes spherical linear interpolation of unit quaternions. +// +// 'start' and 'end' are quaternions in the format [w, n_1, n_2, n_3]. +template +std::array SlerpQuaternions(const T* const start, const T* const end, + double factor); + +// Interpolates 3D poses. Linear interpolation is performed for translation and +// spherical-linear one for rotation. +template +std::tuple /* rotation */, std::array /* translation */> +InterpolateNodes3D(const T* const prev_node_rotation, + const T* const prev_node_translation, + const T* const next_node_rotation, + const T* const next_node_translation, + const double interpolation_parameter); + +// Embeds 2D poses into 3D and interpolates them. Linear interpolation is +// performed for translation and spherical-linear one for rotation. +template +std::tuple /* rotation */, std::array /* translation */> +InterpolateNodes2D(const T* const prev_node_pose, + const Eigen::Quaterniond& prev_node_gravity_alignment, + const T* const next_node_pose, + const Eigen::Quaterniond& next_node_gravity_alignment, + const double interpolation_parameter); + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h" + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h new file mode 100644 index 0000000..6f20889 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h @@ -0,0 +1,197 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_ + +#include "Eigen/Core" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +template +static std::array ComputeUnscaledError( + const transform::Rigid2d& relative_pose, const T* const start, + const T* const end) { + const T cos_theta_i = cos(start[2]); + const T sin_theta_i = sin(start[2]); + const T delta_x = end[0] - start[0]; + const T delta_y = end[1] - start[1]; + const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, + -sin_theta_i * delta_x + cos_theta_i * delta_y, + end[2] - start[2]}; + return {{T(relative_pose.translation().x()) - h[0], + T(relative_pose.translation().y()) - h[1], + common::NormalizeAngleDifference( + T(relative_pose.rotation().angle()) - h[2])}}; +} + +template +std::array ScaleError(const std::array& error, + double translation_weight, double rotation_weight) { + // clang-format off + return {{ + error[0] * translation_weight, + error[1] * translation_weight, + error[2] * rotation_weight + }}; + // clang-format on +} + +template +static std::array ComputeUnscaledError( + const transform::Rigid3d& relative_pose, const T* const start_rotation, + const T* const start_translation, const T* const end_rotation, + const T* const end_translation) { + const Eigen::Quaternion R_i_inverse(start_rotation[0], -start_rotation[1], + -start_rotation[2], + -start_rotation[3]); + + const Eigen::Matrix delta(end_translation[0] - start_translation[0], + end_translation[1] - start_translation[1], + end_translation[2] - start_translation[2]); + const Eigen::Matrix h_translation = R_i_inverse * delta; + + const Eigen::Quaternion h_rotation_inverse = + Eigen::Quaternion(end_rotation[0], -end_rotation[1], -end_rotation[2], + -end_rotation[3]) * + Eigen::Quaternion(start_rotation[0], start_rotation[1], + start_rotation[2], start_rotation[3]); + + const Eigen::Matrix angle_axis_difference = + transform::RotationQuaternionToAngleAxisVector( + h_rotation_inverse * relative_pose.rotation().cast()); + + return {{T(relative_pose.translation().x()) - h_translation[0], + T(relative_pose.translation().y()) - h_translation[1], + T(relative_pose.translation().z()) - h_translation[2], + angle_axis_difference[0], angle_axis_difference[1], + angle_axis_difference[2]}}; +} + +template +std::array ScaleError(const std::array& error, + double translation_weight, double rotation_weight) { + // clang-format off + return {{ + error[0] * translation_weight, + error[1] * translation_weight, + error[2] * translation_weight, + error[3] * rotation_weight, + error[4] * rotation_weight, + error[5] * rotation_weight + }}; + // clang-format on +} + +// Eigen implementation of slerp is not compatible with Ceres on all supported +// platforms. Our own implementation is used instead. +template +std::array SlerpQuaternions(const T* const start, const T* const end, + double factor) { + // Angle 'theta' is the half-angle "between" quaternions. It can be computed + // as the arccosine of their dot product. + const T cos_theta = start[0] * end[0] + start[1] * end[1] + + start[2] * end[2] + start[3] * end[3]; + // Avoid using ::abs which would cast to integer. + const T abs_cos_theta = ceres::abs(cos_theta); + // If numerical error brings 'cos_theta' outside [-1 + epsilon, 1 - epsilon] + // interval, then the quaternions are likely to be collinear. + T prev_scale(1. - factor); + T next_scale(factor); + if (abs_cos_theta < T(1. - 1e-5)) { + const T theta = acos(abs_cos_theta); + const T sin_theta = sin(theta); + prev_scale = sin((1. - factor) * theta) / sin_theta; + next_scale = sin(factor * theta) / sin_theta; + } + if (cos_theta < T(0.)) { + next_scale = -next_scale; + } + return {{prev_scale * start[0] + next_scale * end[0], + prev_scale * start[1] + next_scale * end[1], + prev_scale * start[2] + next_scale * end[2], + prev_scale * start[3] + next_scale * end[3]}}; +} + +template +std::tuple /* rotation */, std::array /* translation */> +InterpolateNodes3D(const T* const prev_node_rotation, + const T* const prev_node_translation, + const T* const next_node_rotation, + const T* const next_node_translation, + const double interpolation_parameter) { + return std::make_tuple( + SlerpQuaternions(prev_node_rotation, next_node_rotation, + interpolation_parameter), + std::array{ + {prev_node_translation[0] + + interpolation_parameter * + (next_node_translation[0] - prev_node_translation[0]), + prev_node_translation[1] + + interpolation_parameter * + (next_node_translation[1] - prev_node_translation[1]), + prev_node_translation[2] + + interpolation_parameter * + (next_node_translation[2] - prev_node_translation[2])}}); +} + +template +std::tuple /* rotation */, std::array /* translation */> +InterpolateNodes2D(const T* const prev_node_pose, + const Eigen::Quaterniond& prev_node_gravity_alignment, + const T* const next_node_pose, + const Eigen::Quaterniond& next_node_gravity_alignment, + const double interpolation_parameter) { + // The following is equivalent to (Embed3D(prev_node_pose) * + // Rigid3d::Rotation(prev_node_gravity_alignment)).rotation(). + const Eigen::Quaternion prev_quaternion( + (Eigen::AngleAxis(prev_node_pose[2], Eigen::Matrix::UnitZ()) * + prev_node_gravity_alignment.cast()) + .normalized()); + const std::array prev_node_rotation = { + {prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(), + prev_quaternion.z()}}; + + // The following is equivalent to (Embed3D(next_node_pose) * + // Rigid3d::Rotation(next_node_gravity_alignment)).rotation(). + const Eigen::Quaternion next_quaternion( + (Eigen::AngleAxis(next_node_pose[2], Eigen::Matrix::UnitZ()) * + next_node_gravity_alignment.cast()) + .normalized()); + const std::array next_node_rotation = { + {next_quaternion.w(), next_quaternion.x(), next_quaternion.y(), + next_quaternion.z()}}; + + return std::make_tuple( + SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(), + interpolation_parameter), + std::array{ + {prev_node_pose[0] + interpolation_parameter * + (next_node_pose[0] - prev_node_pose[0]), + prev_node_pose[1] + interpolation_parameter * + (next_node_pose[1] - prev_node_pose[1]), + T(0)}}); +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h new file mode 100644 index 0000000..d5e0719 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h @@ -0,0 +1,98 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +// Cost function measuring the weighted error between the observed pose given by +// the landmark measurement and the linearly interpolated pose of embedded in 3D +// space node poses. +class LandmarkCostFunction2D { + public: + using LandmarkObservation = + PoseGraphInterface::LandmarkNode::LandmarkObservation; + + static ceres::CostFunction* CreateAutoDiffCostFunction( + const LandmarkObservation& observation, const NodeSpec2D& prev_node, + const NodeSpec2D& next_node) { + return new ceres::AutoDiffCostFunction< + LandmarkCostFunction2D, 6 /* residuals */, + 3 /* previous node pose variables */, 3 /* next node pose variables */, + 4 /* landmark rotation variables */, + 3 /* landmark translation variables */>( + new LandmarkCostFunction2D(observation, prev_node, next_node)); + } + + template + bool operator()(const T* const prev_node_pose, const T* const next_node_pose, + const T* const landmark_rotation, + const T* const landmark_translation, T* const e) const { + const std::tuple, std::array> + interpolated_rotation_and_translation = InterpolateNodes2D( + prev_node_pose, prev_node_gravity_alignment_, next_node_pose, + next_node_gravity_alignment_, interpolation_parameter_); + const std::array error = ScaleError( + ComputeUnscaledError( + landmark_to_tracking_transform_, + std::get<0>(interpolated_rotation_and_translation).data(), + std::get<1>(interpolated_rotation_and_translation).data(), + landmark_rotation, landmark_translation), + translation_weight_, rotation_weight_); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + LandmarkCostFunction2D(const LandmarkObservation& observation, + const NodeSpec2D& prev_node, + const NodeSpec2D& next_node) + : landmark_to_tracking_transform_( + observation.landmark_to_tracking_transform), + prev_node_gravity_alignment_(prev_node.gravity_alignment), + next_node_gravity_alignment_(next_node.gravity_alignment), + translation_weight_(observation.translation_weight), + rotation_weight_(observation.rotation_weight), + interpolation_parameter_( + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time)) {} + + const transform::Rigid3d landmark_to_tracking_transform_; + const Eigen::Quaterniond prev_node_gravity_alignment_; + const Eigen::Quaterniond next_node_gravity_alignment_; + const double translation_weight_; + const double rotation_weight_; + const double interpolation_parameter_; +}; + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc new file mode 100644 index 0000000..e5d4c2a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc @@ -0,0 +1,77 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h" + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +using ::testing::DoubleEq; +using ::testing::ElementsAre; + +using LandmarkObservation = + PoseGraphInterface::LandmarkNode::LandmarkObservation; + +TEST(LandmarkCostFunctionTest, SmokeTest) { + NodeSpec2D prev_node; + prev_node.time = common::FromUniversal(0); + prev_node.gravity_alignment = Eigen::Quaterniond::Identity(); + NodeSpec2D next_node; + next_node.time = common::FromUniversal(10); + next_node.gravity_alignment = Eigen::Quaterniond::Identity(); + + std::unique_ptr cost_function( + LandmarkCostFunction2D::CreateAutoDiffCostFunction( + LandmarkObservation{ + 0 /* trajectory ID */, + common::FromUniversal(5) /* time */, + transform::Rigid3d::Translation(Eigen::Vector3d(1., 1., 1.)), + 1. /* translation_weight */, + 2. /* rotation_weight */, + }, + prev_node, next_node)); + + const std::array prev_node_pose{{2., 0., 0.}}; + const std::array next_node_pose{{0., 2., 0.}}; + const std::array landmark_rotation{{1., 0., 0., 0.}}; + const std::array landmark_translation{{1., 2., 1.}}; + const std::array parameter_blocks{ + {prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(), + landmark_translation.data()}}; + + std::array residuals; + std::array, 6> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data(); + + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + jacobians_ptrs.data()); + EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.), + DoubleEq(0.), DoubleEq(0.), DoubleEq(0.))); +} + +} // namespace +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h new file mode 100644 index 0000000..4d297e1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h @@ -0,0 +1,99 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +// Cost function measuring the weighted error between the observed pose given by +// the landmark measurement and the linearly interpolated pose. +class LandmarkCostFunction3D { + public: + using LandmarkObservation = + PoseGraphInterface::LandmarkNode::LandmarkObservation; + + static ceres::CostFunction* CreateAutoDiffCostFunction( + const LandmarkObservation& observation, const NodeSpec3D& prev_node, + const NodeSpec3D& next_node) { + return new ceres::AutoDiffCostFunction< + LandmarkCostFunction3D, 6 /* residuals */, + 4 /* previous node rotation variables */, + 3 /* previous node translation variables */, + 4 /* next node rotation variables */, + 3 /* next node translation variables */, + 4 /* landmark rotation variables */, + 3 /* landmark translation variables */>( + new LandmarkCostFunction3D(observation, prev_node, next_node)); + } + + template + bool operator()(const T* const prev_node_rotation, + const T* const prev_node_translation, + const T* const next_node_rotation, + const T* const next_node_translation, + const T* const landmark_rotation, + const T* const landmark_translation, T* const e) const { + const std::tuple, std::array> + interpolated_rotation_and_translation = InterpolateNodes3D( + prev_node_rotation, prev_node_translation, next_node_rotation, + next_node_translation, interpolation_parameter_); + const std::array error = ScaleError( + ComputeUnscaledError( + landmark_to_tracking_transform_, + std::get<0>(interpolated_rotation_and_translation).data(), + std::get<1>(interpolated_rotation_and_translation).data(), + landmark_rotation, landmark_translation), + translation_weight_, rotation_weight_); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + LandmarkCostFunction3D(const LandmarkObservation& observation, + const NodeSpec3D& prev_node, + const NodeSpec3D& next_node) + : landmark_to_tracking_transform_( + observation.landmark_to_tracking_transform), + translation_weight_(observation.translation_weight), + rotation_weight_(observation.rotation_weight), + interpolation_parameter_( + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time)) {} + + const transform::Rigid3d landmark_to_tracking_transform_; + const double translation_weight_; + const double rotation_weight_; + const double interpolation_parameter_; +}; + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc new file mode 100644 index 0000000..a5c620d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc @@ -0,0 +1,77 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h" + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +using ::testing::DoubleEq; +using ::testing::ElementsAre; + +using LandmarkObservation = + PoseGraphInterface::LandmarkNode::LandmarkObservation; + +TEST(LandmarkCostFunction3DTest, SmokeTest) { + NodeSpec3D prev_node; + prev_node.time = common::FromUniversal(0); + NodeSpec3D next_node; + next_node.time = common::FromUniversal(10); + + std::unique_ptr cost_function( + LandmarkCostFunction3D::CreateAutoDiffCostFunction( + LandmarkObservation{ + 0 /* trajectory ID */, + common::FromUniversal(5) /* time */, + transform::Rigid3d::Translation(Eigen::Vector3d(1., 1., 1.)), + 1. /* translation_weight */, + 2. /* rotation_weight */, + }, + prev_node, next_node)); + + const std::array prev_node_rotation{{1., 0., 0., 0.}}; + const std::array prev_node_translation{{0., 0., 0.}}; + const std::array next_node_rotation{{1., 0., 0., 0.}}; + const std::array next_node_translation{{2., 2., 2.}}; + const std::array landmark_rotation{{1., 0., 0., 0.}}; + const std::array landmark_translation{{1., 2., 2.}}; + const std::array parameter_blocks{ + {prev_node_rotation.data(), prev_node_translation.data(), + next_node_rotation.data(), next_node_translation.data(), + landmark_rotation.data(), landmark_translation.data()}}; + + std::array residuals; + std::array, 6> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data(); + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + jacobians_ptrs.data()); + EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.), + DoubleEq(0.), DoubleEq(0.), DoubleEq(0.))); +} + +} // namespace +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h new file mode 100644 index 0000000..53390fc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h @@ -0,0 +1,74 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { + +// Penalizes differences between IMU data and optimized orientations. +class RotationCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, + const Eigen::Quaterniond& delta_rotation_imu_frame) { + return new ceres::AutoDiffCostFunction< + RotationCostFunction3D, 3 /* residuals */, 4 /* rotation variables */, + 4 /* rotation variables */, 4 /* rotation variables */ + >(new RotationCostFunction3D(scaling_factor, delta_rotation_imu_frame)); + } + + template + bool operator()(const T* const start_rotation, const T* const end_rotation, + const T* const imu_calibration, T* residual) const { + const Eigen::Quaternion start(start_rotation[0], start_rotation[1], + start_rotation[2], start_rotation[3]); + const Eigen::Quaternion end(end_rotation[0], end_rotation[1], + end_rotation[2], end_rotation[3]); + const Eigen::Quaternion eigen_imu_calibration( + imu_calibration[0], imu_calibration[1], imu_calibration[2], + imu_calibration[3]); + const Eigen::Quaternion error = + end.conjugate() * start * eigen_imu_calibration * + delta_rotation_imu_frame_.cast() * eigen_imu_calibration.conjugate(); + residual[0] = scaling_factor_ * error.x(); + residual[1] = scaling_factor_ * error.y(); + residual[2] = scaling_factor_ * error.z(); + return true; + } + + private: + RotationCostFunction3D(const double scaling_factor, + const Eigen::Quaterniond& delta_rotation_imu_frame) + : scaling_factor_(scaling_factor), + delta_rotation_imu_frame_(delta_rotation_imu_frame) {} + + RotationCostFunction3D(const RotationCostFunction3D&) = delete; + RotationCostFunction3D& operator=(const RotationCostFunction3D&) = delete; + + const double scaling_factor_; + const Eigen::Quaterniond delta_rotation_imu_frame_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc new file mode 100644 index 0000000..4c3aee9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc @@ -0,0 +1,150 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h" + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +class SpaCostFunction2D { + public: + explicit SpaCostFunction2D( + const PoseGraphInterface::Constraint::Pose& observed_relative_pose) + : observed_relative_pose_(observed_relative_pose) {} + + template + bool operator()(const T* const start_pose, const T* const end_pose, + T* e) const { + const std::array error = + ScaleError(ComputeUnscaledError( + transform::Project2D(observed_relative_pose_.zbar_ij), + start_pose, end_pose), + observed_relative_pose_.translation_weight, + observed_relative_pose_.rotation_weight); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + const PoseGraphInterface::Constraint::Pose observed_relative_pose_; +}; + +class AnalyticalSpaCostFunction2D + : public ceres::SizedCostFunction<3 /* number of residuals */, + 3 /* size of start pose */, + 3 /* size of end pose */> { + public: + explicit AnalyticalSpaCostFunction2D( + const PoseGraphInterface::Constraint::Pose& constraint_pose) + : observed_relative_pose_(transform::Project2D(constraint_pose.zbar_ij)), + translation_weight_(constraint_pose.translation_weight), + rotation_weight_(constraint_pose.rotation_weight) {} + virtual ~AnalyticalSpaCostFunction2D() {} + + bool Evaluate(double const* const* parameters, double* residuals, + double** jacobians) const override { + double const* start = parameters[0]; + double const* end = parameters[1]; + + const double cos_start_rotation = cos(start[2]); + const double sin_start_rotation = sin(start[2]); + const double delta_x = end[0] - start[0]; + const double delta_y = end[1] - start[1]; + + residuals[0] = + translation_weight_ * + (observed_relative_pose_.translation().x() - + (cos_start_rotation * delta_x + sin_start_rotation * delta_y)); + residuals[1] = + translation_weight_ * + (observed_relative_pose_.translation().y() - + (-sin_start_rotation * delta_x + cos_start_rotation * delta_y)); + residuals[2] = + rotation_weight_ * + common::NormalizeAngleDifference( + observed_relative_pose_.rotation().angle() - (end[2] - start[2])); + if (jacobians == NULL) return true; + + const double weighted_cos_start_rotation = + translation_weight_ * cos_start_rotation; + const double weighted_sin_start_rotation = + translation_weight_ * sin_start_rotation; + + // Jacobians in Ceres are ordered by the parameter blocks: + // jacobian[i] = [(dr_0 / dx_i)^T, ..., (dr_n / dx_i)^T]. + if (jacobians[0] != NULL) { + jacobians[0][0] = weighted_cos_start_rotation; + jacobians[0][1] = weighted_sin_start_rotation; + jacobians[0][2] = weighted_sin_start_rotation * delta_x - + weighted_cos_start_rotation * delta_y; + jacobians[0][3] = -weighted_sin_start_rotation; + jacobians[0][4] = weighted_cos_start_rotation; + jacobians[0][5] = weighted_cos_start_rotation * delta_x + + weighted_sin_start_rotation * delta_y; + jacobians[0][6] = 0; + jacobians[0][7] = 0; + jacobians[0][8] = rotation_weight_; + } + if (jacobians[1] != NULL) { + jacobians[1][0] = -weighted_cos_start_rotation; + jacobians[1][1] = -weighted_sin_start_rotation; + jacobians[1][2] = 0; + jacobians[1][3] = weighted_sin_start_rotation; + jacobians[1][4] = -weighted_cos_start_rotation; + jacobians[1][5] = 0; + jacobians[1][6] = 0; + jacobians[1][7] = 0; + jacobians[1][8] = -rotation_weight_; + } + return true; + } + + private: + const transform::Rigid2d observed_relative_pose_; + const double translation_weight_; + const double rotation_weight_; +}; + +} // namespace + +ceres::CostFunction* CreateAutoDiffSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& observed_relative_pose) { + return new ceres::AutoDiffCostFunction( + new SpaCostFunction2D(observed_relative_pose)); +} + +ceres::CostFunction* CreateAnalyticalSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& observed_relative_pose) { + return new AnalyticalSpaCostFunction2D(observed_relative_pose); +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h new file mode 100644 index 0000000..01d9363 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ + +#include "cartographer/mapping/pose_graph_interface.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +ceres::CostFunction* CreateAutoDiffSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& pose); + +ceres::CostFunction* CreateAnalyticalSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& pose); + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc new file mode 100644 index 0000000..409ba5d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc @@ -0,0 +1,150 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h" + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +using ::testing::ElementsAre; + +constexpr int kPoseDimension = 3; +constexpr int kResidualsCount = 3; +constexpr int kParameterBlocksCount = 2; +constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension; + +using ResidualType = std::array; +using JacobianType = std::array, + kParameterBlocksCount>; + +::testing::Matcher Near(double expected, double precision = 1e-05) { + return testing::DoubleNear(expected, precision); +} + +class SpaCostFunction2DTest : public ::testing::Test { + public: + SpaCostFunction2DTest() + : constraint_(PoseGraphInterface::Constraint::Pose{ + transform::Rigid3d(Eigen::Vector3d(1., 1., 1.), + Eigen::Quaterniond(1., 1., -1., -1.)), + 1, 10}), + auto_diff_cost_(CreateAutoDiffSpaCostFunction(constraint_)), + analytical_cost_(CreateAnalyticalSpaCostFunction(constraint_)) { + for (int i = 0; i < kParameterBlocksCount; ++i) { + jacobian_ptrs_[i] = jacobian_[i].data(); + } + } + + std::pair EvaluateAnalyticalCost( + const std::array& parameter_blocks) { + return Evaluate(parameter_blocks, analytical_cost_); + } + + std::pair EvaluateAutoDiffCost( + const std::array& parameter_blocks) { + return Evaluate(parameter_blocks, auto_diff_cost_); + } + + private: + std::pair Evaluate( + const std::array& parameter_blocks, + const std::unique_ptr& cost_function) { + cost_function->Evaluate(parameter_blocks.data(), residuals_.data(), + jacobian_ptrs_.data()); + return std::make_pair(std::cref(residuals_), std::cref(jacobian_)); + } + + ResidualType residuals_; + JacobianType jacobian_; + std::array jacobian_ptrs_; + PoseGraphInterface::Constraint::Pose constraint_; + std::unique_ptr auto_diff_cost_; + std::unique_ptr analytical_cost_; +}; + +TEST_F(SpaCostFunction2DTest, CompareAutoDiffAndAnalytical) { + std::array start_pose{{1., 1., 1.}}; + std::array end_pose{{10., 1., 100.}}; + std::array parameter_blocks{ + {start_pose.data(), end_pose.data()}}; + + ResidualType auto_diff_residual, analytical_residual; + JacobianType auto_diff_jacobian, analytical_jacobian; + std::tie(auto_diff_residual, auto_diff_jacobian) = + EvaluateAutoDiffCost(parameter_blocks); + std::tie(analytical_residual, analytical_jacobian) = + EvaluateAnalyticalCost(parameter_blocks); + + for (int i = 0; i < kResidualsCount; ++i) { + EXPECT_THAT(auto_diff_residual[i], Near(analytical_residual[i])); + } + for (int i = 0; i < kParameterBlocksCount; ++i) { + for (int j = 0; j < kJacobianColDimension; ++j) { + EXPECT_THAT(auto_diff_jacobian[i][j], Near(analytical_jacobian[i][j])); + } + } +} + +TEST_F(SpaCostFunction2DTest, EvaluateAnalyticalCost) { + std::array start_pose{{1., 1., 1.}}; + std::array end_pose{{10., 1., 100.}}; + std::array parameter_blocks{ + {start_pose.data(), end_pose.data()}}; + + auto residuals_and_jacobian = EvaluateAnalyticalCost(parameter_blocks); + EXPECT_THAT(residuals_and_jacobian.first, + ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333))); + EXPECT_THAT( + residuals_and_jacobian.second, + ElementsAre( + ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324), + Near(-0.841471), Near(0.540302), Near(4.86272), Near(0), + Near(0), Near(10)), + ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471), + Near(-0.540302), Near(0), Near(0), Near(0), Near(-10)))); +} + +TEST_F(SpaCostFunction2DTest, EvaluateAutoDiffCost) { + std::array start_pose{{1., 1., 1.}}; + std::array end_pose{{10., 1., 100.}}; + std::array parameter_blocks{ + {start_pose.data(), end_pose.data()}}; + + auto residuals_and_jacobian = EvaluateAutoDiffCost(parameter_blocks); + EXPECT_THAT(residuals_and_jacobian.first, + ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333))); + EXPECT_THAT( + residuals_and_jacobian.second, + ElementsAre( + ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324), + Near(-0.841471), Near(0.540302), Near(4.86272), Near(0), + Near(0), Near(10)), + ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471), + Near(-0.540302), Near(0), Near(0), Near(0), Near(-10)))); +} + +} // namespace +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h new file mode 100644 index 0000000..d4a400b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h @@ -0,0 +1,69 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +class SpaCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const PoseGraph::Constraint::Pose& pose) { + return new ceres::AutoDiffCostFunction< + SpaCostFunction3D, 6 /* residuals */, 4 /* rotation variables */, + 3 /* translation variables */, 4 /* rotation variables */, + 3 /* translation variables */>(new SpaCostFunction3D(pose)); + } + + template + bool operator()(const T* const c_i_rotation, const T* const c_i_translation, + const T* const c_j_rotation, const T* const c_j_translation, + T* const e) const { + const std::array error = ScaleError( + ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation, + c_j_rotation, c_j_translation), + pose_.translation_weight, pose_.rotation_weight); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + explicit SpaCostFunction3D(const PoseGraph::Constraint::Pose& pose) + : pose_(pose) {} + + const PoseGraph::Constraint::Pose pose_; +}; + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_2d.cc new file mode 100644 index 0000000..3faf255 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -0,0 +1,472 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/optimization/optimization_problem_2d.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/histogram.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +using ::cartographer::mapping::optimization::CeresPose; +using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; +using TrajectoryData = + ::cartographer::mapping::PoseGraphInterface::TrajectoryData; + +// For fixed frame pose. +std::unique_ptr Interpolate( + const sensor::MapByTime& map_by_time, + const int trajectory_id, const common::Time time) { + const auto it = map_by_time.lower_bound(trajectory_id, time); + if (it == map_by_time.EndOfTrajectory(trajectory_id) || + !it->pose.has_value()) { + return nullptr; + } + if (it == map_by_time.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose.value()); + } + return nullptr; + } + const auto prev_it = std::prev(it); + if (prev_it->pose.has_value()) { + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, + prev_it->pose.value()}, + transform::TimestampedTransform{it->time, it->pose.value()}, + time) + .transform); + } + return nullptr; +} + +// Converts a pose into the 3 optimization variable format used for Ceres: +// translation in x and y, followed by the rotation angle representing the +// orientation. +std::array FromPose(const transform::Rigid2d& pose) { + return {{pose.translation().x(), pose.translation().y(), + pose.normalized_angle()}}; +} + +// Converts a pose as represented for Ceres back to an transform::Rigid2d pose. +transform::Rigid2d ToPose(const std::array& values) { + return transform::Rigid2d({values[0], values[1]}, values[2]); +} + +// Selects a trajectory node closest in time to the landmark observation and +// applies a relative transform from it. +transform::Rigid3d GetInitialLandmarkPose( + const LandmarkNode::LandmarkObservation& observation, + const NodeSpec2D& prev_node, const NodeSpec2D& next_node, + const std::array& prev_node_pose, + const std::array& next_node_pose) { + const double interpolation_parameter = + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time); + + const std::tuple, std::array> + rotation_and_translation = + InterpolateNodes2D(prev_node_pose.data(), prev_node.gravity_alignment, + next_node_pose.data(), next_node.gravity_alignment, + interpolation_parameter); + return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation), + std::get<1>(rotation_and_translation)) * + observation.landmark_to_tracking_transform; +} + +void AddLandmarkCostFunctions( + const std::map& landmark_nodes, + const MapById& node_data, + MapById>* C_nodes, + std::map* C_landmarks, ceres::Problem* problem, + double huber_scale) { + for (const auto& landmark_node : landmark_nodes) { + for (const auto& observation : landmark_node.second.landmark_observations) { + const std::string& landmark_id = landmark_node.first; + const auto& begin_of_trajectory = + node_data.BeginOfTrajectory(observation.trajectory_id); + // The landmark observation was made before the trajectory was created. + if (observation.time < begin_of_trajectory->data.time) { + continue; + } + // Find the trajectory nodes before and after the landmark observation. + auto next = + node_data.lower_bound(observation.trajectory_id, observation.time); + // The landmark observation was made, but the next trajectory node has + // not been added yet. + if (next == node_data.EndOfTrajectory(observation.trajectory_id)) { + continue; + } + if (next == begin_of_trajectory) { + next = std::next(next); + } + auto prev = std::prev(next); + // Add parameter blocks for the landmark ID if they were not added before. + std::array* prev_node_pose = &C_nodes->at(prev->id); + std::array* next_node_pose = &C_nodes->at(next->id); + if (!C_landmarks->count(landmark_id)) { + const transform::Rigid3d starting_point = + landmark_node.second.global_landmark_pose.has_value() + ? landmark_node.second.global_landmark_pose.value() + : GetInitialLandmarkPose(observation, prev->data, next->data, + *prev_node_pose, *next_node_pose); + C_landmarks->emplace( + landmark_id, + CeresPose(starting_point, nullptr /* translation_parametrization */, + absl::make_unique(), + problem)); + // Set landmark constant if it is frozen. + if (landmark_node.second.frozen) { + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).translation()); + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).rotation()); + } + } + problem->AddResidualBlock( + LandmarkCostFunction2D::CreateAutoDiffCostFunction( + observation, prev->data, next->data), + new ceres::HuberLoss(huber_scale), prev_node_pose->data(), + next_node_pose->data(), C_landmarks->at(landmark_id).rotation(), + C_landmarks->at(landmark_id).translation()); + } + } +} + +} // namespace + +OptimizationProblem2D::OptimizationProblem2D( + const proto::OptimizationProblemOptions& options) + : options_(options) {} + +OptimizationProblem2D::~OptimizationProblem2D() {} + +void OptimizationProblem2D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { + // IMU data is not used in 2D optimization, so we ignore this part of the + // interface. +} + +void OptimizationProblem2D::AddOdometryData( + const int trajectory_id, const sensor::OdometryData& odometry_data) { + odometry_data_.Append(trajectory_id, odometry_data); +} + +void OptimizationProblem2D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); +} + +void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id, + const NodeSpec2D& node_data) { + node_data_.Append(trajectory_id, node_data); + trajectory_data_[trajectory_id]; +} + +void OptimizationProblem2D::SetTrajectoryData( + int trajectory_id, const TrajectoryData& trajectory_data) { + trajectory_data_[trajectory_id] = trajectory_data; +} + +void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, + const NodeSpec2D& node_data) { + node_data_.Insert(node_id, node_data); + trajectory_data_[node_id.trajectory_id]; +} + +void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { + empty_imu_data_.Trim(node_data_, node_id); + odometry_data_.Trim(node_data_, node_id); + fixed_frame_pose_data_.Trim(node_data_, node_id); + node_data_.Trim(node_id); + if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { + trajectory_data_.erase(node_id.trajectory_id); + } +} + +void OptimizationProblem2D::AddSubmap( + const int trajectory_id, const transform::Rigid2d& global_submap_pose) { + submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose}); +} + +void OptimizationProblem2D::InsertSubmap( + const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) { + submap_data_.Insert(submap_id, SubmapSpec2D{global_submap_pose}); +} + +void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) { + submap_data_.Trim(submap_id); +} + +void OptimizationProblem2D::SetMaxNumIterations( + const int32 max_num_iterations) { + options_.mutable_ceres_solver_options()->set_max_num_iterations( + max_num_iterations); +} + +void OptimizationProblem2D::Solve( + const std::vector& constraints, + const std::map& + trajectories_state, + const std::map& landmark_nodes) { + if (node_data_.empty()) { + // Nothing to optimize. + return; + } + + std::set frozen_trajectories; + for (const auto& it : trajectories_state) { + if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) { + frozen_trajectories.insert(it.first); + } + } + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + // Set the starting point. + // TODO(hrapp): Move ceres data into SubmapSpec. + MapById> C_submaps; + MapById> C_nodes; + std::map C_landmarks; + bool first_submap = true; + for (const auto& submap_id_data : submap_data_) { + const bool frozen = + frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; + C_submaps.Insert(submap_id_data.id, + FromPose(submap_id_data.data.global_pose)); + problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3); + if (first_submap || frozen) { + first_submap = false; + // Fix the pose of the first submap or all submaps of a frozen + // trajectory. + problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data()); + } + } + for (const auto& node_id_data : node_data_) { + const bool frozen = + frozen_trajectories.count(node_id_data.id.trajectory_id) != 0; + C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d)); + problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3); + if (frozen) { + problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data()); + } + } + // Add cost functions for intra- and inter-submap constraints. + for (const Constraint& constraint : constraints) { + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction(constraint.pose), + // Loop closure constraints should have a loss function. + constraint.tag == Constraint::INTER_SUBMAP + ? new ceres::HuberLoss(options_.huber_scale()) + : nullptr, + C_submaps.at(constraint.submap_id).data(), + C_nodes.at(constraint.node_id).data()); + } + // Add cost functions for landmarks. + AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, + &problem, options_.huber_scale()); + // Add penalties for violating odometry or changes between consecutive nodes + // if odometry is not available. + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (frozen_trajectories.count(trajectory_id) != 0) { + node_it = trajectory_end; + continue; + } + + auto prev_node_it = node_it; + for (++node_it; node_it != trajectory_end; ++node_it) { + const NodeId first_node_id = prev_node_it->id; + const NodeSpec2D& first_node_data = prev_node_it->data; + prev_node_it = node_it; + const NodeId second_node_id = node_it->id; + const NodeSpec2D& second_node_data = node_it->data; + + if (second_node_id.node_index != first_node_id.node_index + 1) { + continue; + } + + // Add a relative pose constraint based on the odometry (if available). + std::unique_ptr relative_odometry = + CalculateOdometryBetweenNodes(trajectory_id, first_node_data, + second_node_data); + if (relative_odometry != nullptr) { + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction(Constraint::Pose{ + *relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, C_nodes.at(first_node_id).data(), + C_nodes.at(second_node_id).data()); + } + + // Add a relative pose constraint based on consecutive local SLAM poses. + const transform::Rigid3d relative_local_slam_pose = + transform::Embed3D(first_node_data.local_pose_2d.inverse() * + second_node_data.local_pose_2d); + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction( + Constraint::Pose{relative_local_slam_pose, + options_.local_slam_pose_translation_weight(), + options_.local_slam_pose_rotation_weight()}), + nullptr /* loss function */, C_nodes.at(first_node_id).data(), + C_nodes.at(second_node_id).data()); + } + } + + std::map> C_fixed_frames; + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) { + node_it = trajectory_end; + continue; + } + + const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + bool fixed_frame_pose_initialized = false; + for (; node_it != trajectory_end; ++node_it) { + const NodeId node_id = node_it->id; + const NodeSpec2D& node_data = node_it->data; + + const std::unique_ptr fixed_frame_pose = + Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); + if (fixed_frame_pose == nullptr) { + continue; + } + + const Constraint::Pose constraint_pose{ + *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), + options_.fixed_frame_pose_rotation_weight()}; + + if (!fixed_frame_pose_initialized) { + transform::Rigid2d fixed_frame_pose_in_map; + if (trajectory_data.fixed_frame_origin_in_map.has_value()) { + fixed_frame_pose_in_map = transform::Project2D( + trajectory_data.fixed_frame_origin_in_map.value()); + } else { + fixed_frame_pose_in_map = + node_data.global_pose_2d * + transform::Project2D(constraint_pose.zbar_ij).inverse(); + } + + C_fixed_frames.emplace(trajectory_id, + FromPose(fixed_frame_pose_in_map)); + fixed_frame_pose_initialized = true; + } + + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction(constraint_pose), + options_.fixed_frame_pose_use_tolerant_loss() + ? new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) + : nullptr, + C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data()); + } + } + + // Solve. + ceres::Solver::Summary summary; + ceres::Solve( + common::CreateCeresSolverOptions(options_.ceres_solver_options()), + &problem, &summary); + if (options_.log_solver_summary()) { + LOG(INFO) << summary.FullReport(); + } + + // Store the result. + for (const auto& C_submap_id_data : C_submaps) { + submap_data_.at(C_submap_id_data.id).global_pose = + ToPose(C_submap_id_data.data); + } + for (const auto& C_node_id_data : C_nodes) { + node_data_.at(C_node_id_data.id).global_pose_2d = + ToPose(C_node_id_data.data); + } + for (const auto& C_fixed_frame : C_fixed_frames) { + trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map = + transform::Embed3D(ToPose(C_fixed_frame.second)); + } + for (const auto& C_landmark : C_landmarks) { + landmark_data_[C_landmark.first] = C_landmark.second.ToRigid(); + } +} + +std::unique_ptr OptimizationProblem2D::InterpolateOdometry( + const int trajectory_id, const common::Time time) const { + const auto it = odometry_data_.lower_bound(trajectory_id, time); + if (it == odometry_data_.EndOfTrajectory(trajectory_id)) { + return nullptr; + } + if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose); + } + return nullptr; + } + const auto prev_it = std::prev(it); + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose}, + transform::TimestampedTransform{it->time, it->pose}, time) + .transform); +} + +std::unique_ptr +OptimizationProblem2D::CalculateOdometryBetweenNodes( + const int trajectory_id, const NodeSpec2D& first_node_data, + const NodeSpec2D& second_node_data) const { + if (odometry_data_.HasTrajectory(trajectory_id)) { + const std::unique_ptr first_node_odometry = + InterpolateOdometry(trajectory_id, first_node_data.time); + const std::unique_ptr second_node_odometry = + InterpolateOdometry(trajectory_id, second_node_data.time); + if (first_node_odometry != nullptr && second_node_odometry != nullptr) { + transform::Rigid3d relative_odometry = + transform::Rigid3d::Rotation(first_node_data.gravity_alignment) * + first_node_odometry->inverse() * (*second_node_odometry) * + transform::Rigid3d::Rotation( + second_node_data.gravity_alignment.inverse()); + return absl::make_unique(relative_odometry); + } + } + return nullptr; +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_2d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_2d.h new file mode 100644 index 0000000..0a1e39d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_2d.h @@ -0,0 +1,141 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_ + +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/pose_graph/optimization_problem_options.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/map_by_time.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +struct NodeSpec2D { + common::Time time; + transform::Rigid2d local_pose_2d; + transform::Rigid2d global_pose_2d; + Eigen::Quaterniond gravity_alignment; +}; + +struct SubmapSpec2D { + transform::Rigid2d global_pose; +}; + +class OptimizationProblem2D + : public OptimizationProblemInterface { + public: + explicit OptimizationProblem2D( + const optimization::proto::OptimizationProblemOptions& options); + ~OptimizationProblem2D(); + + OptimizationProblem2D(const OptimizationProblem2D&) = delete; + OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete; + + void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override; + void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) override; + void AddTrajectoryNode(int trajectory_id, + const NodeSpec2D& node_data) override; + void InsertTrajectoryNode(const NodeId& node_id, + const NodeSpec2D& node_data) override; + void TrimTrajectoryNode(const NodeId& node_id) override; + void AddSubmap(int trajectory_id, + const transform::Rigid2d& global_submap_pose) override; + void InsertSubmap(const SubmapId& submap_id, + const transform::Rigid2d& global_submap_pose) override; + void TrimSubmap(const SubmapId& submap_id) override; + void SetMaxNumIterations(int32 max_num_iterations) override; + + void Solve( + const std::vector& constraints, + const std::map& + trajectories_state, + const std::map& landmark_nodes) override; + + const MapById& node_data() const override { + return node_data_; + } + const MapById& submap_data() const override { + return submap_data_; + } + const std::map& landmark_data() + const override { + return landmark_data_; + } + const sensor::MapByTime& imu_data() const override { + return empty_imu_data_; + } + const sensor::MapByTime& odometry_data() + const override { + return odometry_data_; + } + + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); + void SetTrajectoryData( + int trajectory_id, + const PoseGraphInterface::TrajectoryData& trajectory_data); + const sensor::MapByTime& fixed_frame_pose_data() + const { + return fixed_frame_pose_data_; + } + const std::map& trajectory_data() + const { + return trajectory_data_; + } + + private: + std::unique_ptr InterpolateOdometry( + int trajectory_id, common::Time time) const; + // Computes the relative pose between two nodes based on odometry data. + std::unique_ptr CalculateOdometryBetweenNodes( + int trajectory_id, const NodeSpec2D& first_node_data, + const NodeSpec2D& second_node_data) const; + + optimization::proto::OptimizationProblemOptions options_; + MapById node_data_; + MapById submap_data_; + std::map landmark_data_; + sensor::MapByTime empty_imu_data_; + sensor::MapByTime odometry_data_; + sensor::MapByTime fixed_frame_pose_data_; + std::map trajectory_data_; +}; + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d.cc new file mode 100644 index 0000000..fca36d1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -0,0 +1,627 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/optimization/optimization_problem_3d.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/math.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/3d/imu_integration.h" +#include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h" +#include "cartographer/transform/timestamped_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" +#include "ceres/rotation.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; +using TrajectoryData = + ::cartographer::mapping::PoseGraphInterface::TrajectoryData; + +// For odometry. +std::unique_ptr Interpolate( + const sensor::MapByTime& map_by_time, + const int trajectory_id, const common::Time time) { + const auto it = map_by_time.lower_bound(trajectory_id, time); + if (it == map_by_time.EndOfTrajectory(trajectory_id)) { + return nullptr; + } + if (it == map_by_time.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose); + } + return nullptr; + } + const auto prev_it = std::prev(it); + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose}, + transform::TimestampedTransform{it->time, it->pose}, time) + .transform); +} + +// For fixed frame pose. +std::unique_ptr Interpolate( + const sensor::MapByTime& map_by_time, + const int trajectory_id, const common::Time time) { + const auto it = map_by_time.lower_bound(trajectory_id, time); + if (it == map_by_time.EndOfTrajectory(trajectory_id) || + !it->pose.has_value()) { + return nullptr; + } + if (it == map_by_time.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose.value()); + } + return nullptr; + } + const auto prev_it = std::prev(it); + if (prev_it->pose.has_value()) { + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, + prev_it->pose.value()}, + transform::TimestampedTransform{it->time, it->pose.value()}, + time) + .transform); + } + return nullptr; +} + +// Selects a trajectory node closest in time to the landmark observation and +// applies a relative transform from it. +transform::Rigid3d GetInitialLandmarkPose( + const LandmarkNode::LandmarkObservation& observation, + const NodeSpec3D& prev_node, const NodeSpec3D& next_node, + const CeresPose& prev_node_pose, const CeresPose& next_node_pose) { + const double interpolation_parameter = + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time); + + const std::tuple, std::array> + rotation_and_translation = InterpolateNodes3D( + prev_node_pose.rotation(), prev_node_pose.translation(), + next_node_pose.rotation(), next_node_pose.translation(), + interpolation_parameter); + return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation), + std::get<1>(rotation_and_translation)) * + observation.landmark_to_tracking_transform; +} + +void AddLandmarkCostFunctions( + const std::map& landmark_nodes, + const MapById& node_data, + MapById* C_nodes, + std::map* C_landmarks, ceres::Problem* problem, + double huber_scale) { + for (const auto& landmark_node : landmark_nodes) { + // Do not use landmarks that were not optimized for localization. + for (const auto& observation : landmark_node.second.landmark_observations) { + const std::string& landmark_id = landmark_node.first; + const auto& begin_of_trajectory = + node_data.BeginOfTrajectory(observation.trajectory_id); + // The landmark observation was made before the trajectory was created. + if (observation.time < begin_of_trajectory->data.time) { + continue; + } + // Find the trajectory nodes before and after the landmark observation. + auto next = + node_data.lower_bound(observation.trajectory_id, observation.time); + // The landmark observation was made, but the next trajectory node has + // not been added yet. + if (next == node_data.EndOfTrajectory(observation.trajectory_id)) { + continue; + } + if (next == begin_of_trajectory) { + next = std::next(next); + } + auto prev = std::prev(next); + // Add parameter blocks for the landmark ID if they were not added before. + CeresPose* prev_node_pose = &C_nodes->at(prev->id); + CeresPose* next_node_pose = &C_nodes->at(next->id); + if (!C_landmarks->count(landmark_id)) { + const transform::Rigid3d starting_point = + landmark_node.second.global_landmark_pose.has_value() + ? landmark_node.second.global_landmark_pose.value() + : GetInitialLandmarkPose(observation, prev->data, next->data, + *prev_node_pose, *next_node_pose); + C_landmarks->emplace( + landmark_id, + CeresPose(starting_point, nullptr /* translation_parametrization */, + absl::make_unique(), + problem)); + // Set landmark constant if it is frozen. + if (landmark_node.second.frozen) { + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).translation()); + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).rotation()); + } + } + problem->AddResidualBlock( + LandmarkCostFunction3D::CreateAutoDiffCostFunction( + observation, prev->data, next->data), + new ceres::HuberLoss(huber_scale), prev_node_pose->rotation(), + prev_node_pose->translation(), next_node_pose->rotation(), + next_node_pose->translation(), + C_landmarks->at(landmark_id).rotation(), + C_landmarks->at(landmark_id).translation()); + } + } +} + +} // namespace + +OptimizationProblem3D::OptimizationProblem3D( + const optimization::proto::OptimizationProblemOptions& options) + : options_(options) {} + +OptimizationProblem3D::~OptimizationProblem3D() {} + +void OptimizationProblem3D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { + imu_data_.Append(trajectory_id, imu_data); +} + +void OptimizationProblem3D::AddOdometryData( + const int trajectory_id, const sensor::OdometryData& odometry_data) { + odometry_data_.Append(trajectory_id, odometry_data); +} + +void OptimizationProblem3D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); +} + +void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id, + const NodeSpec3D& node_data) { + node_data_.Append(trajectory_id, node_data); + trajectory_data_[trajectory_id]; +} + +void OptimizationProblem3D::SetTrajectoryData( + int trajectory_id, const TrajectoryData& trajectory_data) { + trajectory_data_[trajectory_id] = trajectory_data; +} + +void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id, + const NodeSpec3D& node_data) { + node_data_.Insert(node_id, node_data); + trajectory_data_[node_id.trajectory_id]; +} + +void OptimizationProblem3D::TrimTrajectoryNode(const NodeId& node_id) { + imu_data_.Trim(node_data_, node_id); + odometry_data_.Trim(node_data_, node_id); + fixed_frame_pose_data_.Trim(node_data_, node_id); + node_data_.Trim(node_id); + if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { + trajectory_data_.erase(node_id.trajectory_id); + } +} + +void OptimizationProblem3D::AddSubmap( + const int trajectory_id, const transform::Rigid3d& global_submap_pose) { + submap_data_.Append(trajectory_id, SubmapSpec3D{global_submap_pose}); +} + +void OptimizationProblem3D::InsertSubmap( + const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose) { + submap_data_.Insert(submap_id, SubmapSpec3D{global_submap_pose}); +} + +void OptimizationProblem3D::TrimSubmap(const SubmapId& submap_id) { + submap_data_.Trim(submap_id); +} + +void OptimizationProblem3D::SetMaxNumIterations( + const int32 max_num_iterations) { + options_.mutable_ceres_solver_options()->set_max_num_iterations( + max_num_iterations); +} + +void OptimizationProblem3D::Solve( + const std::vector& constraints, + const std::map& + trajectories_state, + const std::map& landmark_nodes) { + if (node_data_.empty()) { + // Nothing to optimize. + return; + } + + std::set frozen_trajectories; + for (const auto& it : trajectories_state) { + if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) { + frozen_trajectories.insert(it.first); + } + } + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + const auto translation_parameterization = + [this]() -> std::unique_ptr { + return options_.fix_z_in_3d() + ? absl::make_unique( + 3, std::vector{2}) + : nullptr; + }; + + // Set the starting point. + CHECK(!submap_data_.empty()); + MapById C_submaps; + MapById C_nodes; + std::map C_landmarks; + bool first_submap = true; + for (const auto& submap_id_data : submap_data_) { + const bool frozen = + frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; + if (first_submap) { + first_submap = false; + // Fix the first submap of the first trajectory except for allowing + // gravity alignment. + C_submaps.Insert( + submap_id_data.id, + CeresPose(submap_id_data.data.global_pose, + translation_parameterization(), + absl::make_unique>(), + &problem)); + problem.SetParameterBlockConstant( + C_submaps.at(submap_id_data.id).translation()); + } else { + C_submaps.Insert( + submap_id_data.id, + CeresPose(submap_id_data.data.global_pose, + translation_parameterization(), + absl::make_unique(), + &problem)); + } + if (frozen) { + problem.SetParameterBlockConstant( + C_submaps.at(submap_id_data.id).rotation()); + problem.SetParameterBlockConstant( + C_submaps.at(submap_id_data.id).translation()); + } + } + for (const auto& node_id_data : node_data_) { + const bool frozen = + frozen_trajectories.count(node_id_data.id.trajectory_id) != 0; + C_nodes.Insert( + node_id_data.id, + CeresPose(node_id_data.data.global_pose, translation_parameterization(), + absl::make_unique(), + &problem)); + if (frozen) { + problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).rotation()); + problem.SetParameterBlockConstant( + C_nodes.at(node_id_data.id).translation()); + } + } + // Add cost functions for intra- and inter-submap constraints. + for (const Constraint& constraint : constraints) { + problem.AddResidualBlock( + SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose), + // Loop closure constraints should have a loss function. + constraint.tag == Constraint::INTER_SUBMAP + ? new ceres::HuberLoss(options_.huber_scale()) + : nullptr /* loss function */, + C_submaps.at(constraint.submap_id).rotation(), + C_submaps.at(constraint.submap_id).translation(), + C_nodes.at(constraint.node_id).rotation(), + C_nodes.at(constraint.node_id).translation()); + } + // Add cost functions for landmarks. + AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, + &problem, options_.huber_scale()); + // Add constraints based on IMU observations of angular velocities and + // linear acceleration. + if (!options_.fix_z_in_3d()) { + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (frozen_trajectories.count(trajectory_id) != 0) { + // We skip frozen trajectories. + node_it = trajectory_end; + continue; + } + TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + + problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4, + new ceres::QuaternionParameterization()); + if (!options_.use_online_imu_extrinsics_in_3d()) { + problem.SetParameterBlockConstant( + trajectory_data.imu_calibration.data()); + } + CHECK(imu_data_.HasTrajectory(trajectory_id)); + const auto imu_data = imu_data_.trajectory(trajectory_id); + CHECK(imu_data.begin() != imu_data.end()); + + auto imu_it = imu_data.begin(); + auto prev_node_it = node_it; + for (++node_it; node_it != trajectory_end; ++node_it) { + const NodeId first_node_id = prev_node_it->id; + const NodeSpec3D& first_node_data = prev_node_it->data; + prev_node_it = node_it; + const NodeId second_node_id = node_it->id; + const NodeSpec3D& second_node_data = node_it->data; + + if (second_node_id.node_index != first_node_id.node_index + 1) { + continue; + } + + // Skip IMU data before the node. + while (std::next(imu_it) != imu_data.end() && + std::next(imu_it)->time <= first_node_data.time) { + ++imu_it; + } + + auto imu_it2 = imu_it; + const IntegrateImuResult result = IntegrateImu( + imu_data, first_node_data.time, second_node_data.time, &imu_it); + const auto next_node_it = std::next(node_it); + const common::Time first_time = first_node_data.time; + const common::Time second_time = second_node_data.time; + const common::Duration first_duration = second_time - first_time; + if (next_node_it != trajectory_end && + next_node_it->id.node_index == second_node_id.node_index + 1) { + const NodeId third_node_id = next_node_it->id; + const NodeSpec3D& third_node_data = next_node_it->data; + const common::Time third_time = third_node_data.time; + const common::Duration second_duration = third_time - second_time; + const common::Time first_center = first_time + first_duration / 2; + const common::Time second_center = second_time + second_duration / 2; + const IntegrateImuResult result_to_first_center = + IntegrateImu(imu_data, first_time, first_center, &imu_it2); + const IntegrateImuResult result_center_to_center = + IntegrateImu(imu_data, first_center, second_center, &imu_it2); + // 'delta_velocity' is the change in velocity from the point in time + // halfway between the first and second poses to halfway between + // second and third pose. It is computed from IMU data and still + // contains a delta due to gravity. The orientation of this vector is + // in the IMU frame at the second pose. + const Eigen::Vector3d delta_velocity = + (result.delta_rotation.inverse() * + result_to_first_center.delta_rotation) * + result_center_to_center.delta_velocity; + problem.AddResidualBlock( + AccelerationCostFunction3D::CreateAutoDiffCostFunction( + options_.acceleration_weight() / + common::ToSeconds(first_duration + second_duration), + delta_velocity, common::ToSeconds(first_duration), + common::ToSeconds(second_duration)), + nullptr /* loss function */, + C_nodes.at(second_node_id).rotation(), + C_nodes.at(first_node_id).translation(), + C_nodes.at(second_node_id).translation(), + C_nodes.at(third_node_id).translation(), + &trajectory_data.gravity_constant, + trajectory_data.imu_calibration.data()); + } + problem.AddResidualBlock( + RotationCostFunction3D::CreateAutoDiffCostFunction( + options_.rotation_weight() / common::ToSeconds(first_duration), + result.delta_rotation), + nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), + C_nodes.at(second_node_id).rotation(), + trajectory_data.imu_calibration.data()); + } + + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, 0.0); + } + } + + if (options_.fix_z_in_3d()) { + // Add penalties for violating odometry (if available) and changes between + // consecutive nodes. + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (frozen_trajectories.count(trajectory_id) != 0) { + node_it = trajectory_end; + continue; + } + + auto prev_node_it = node_it; + for (++node_it; node_it != trajectory_end; ++node_it) { + const NodeId first_node_id = prev_node_it->id; + const NodeSpec3D& first_node_data = prev_node_it->data; + prev_node_it = node_it; + const NodeId second_node_id = node_it->id; + const NodeSpec3D& second_node_data = node_it->data; + + if (second_node_id.node_index != first_node_id.node_index + 1) { + continue; + } + + // Add a relative pose constraint based on the odometry (if available). + const std::unique_ptr relative_odometry = + CalculateOdometryBetweenNodes(trajectory_id, first_node_data, + second_node_data); + if (relative_odometry != nullptr) { + problem.AddResidualBlock( + SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{ + *relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), + C_nodes.at(first_node_id).translation(), + C_nodes.at(second_node_id).rotation(), + C_nodes.at(second_node_id).translation()); + } + + // Add a relative pose constraint based on consecutive local SLAM poses. + const transform::Rigid3d relative_local_slam_pose = + first_node_data.local_pose.inverse() * second_node_data.local_pose; + problem.AddResidualBlock( + SpaCostFunction3D::CreateAutoDiffCostFunction( + Constraint::Pose{relative_local_slam_pose, + options_.local_slam_pose_translation_weight(), + options_.local_slam_pose_rotation_weight()}), + nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), + C_nodes.at(first_node_id).translation(), + C_nodes.at(second_node_id).rotation(), + C_nodes.at(second_node_id).translation()); + } + } + } + + // Add fixed frame pose constraints. + std::map C_fixed_frames; + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) { + node_it = trajectory_end; + continue; + } + + const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + bool fixed_frame_pose_initialized = false; + for (; node_it != trajectory_end; ++node_it) { + const NodeId node_id = node_it->id; + const NodeSpec3D& node_data = node_it->data; + + const std::unique_ptr fixed_frame_pose = + Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); + if (fixed_frame_pose == nullptr) { + continue; + } + + const Constraint::Pose constraint_pose{ + *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), + options_.fixed_frame_pose_rotation_weight()}; + + if (!fixed_frame_pose_initialized) { + transform::Rigid3d fixed_frame_pose_in_map; + if (trajectory_data.fixed_frame_origin_in_map.has_value()) { + fixed_frame_pose_in_map = + trajectory_data.fixed_frame_origin_in_map.value(); + } else { + fixed_frame_pose_in_map = + node_data.global_pose * constraint_pose.zbar_ij.inverse(); + } + C_fixed_frames.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple( + transform::Rigid3d( + fixed_frame_pose_in_map.translation(), + Eigen::AngleAxisd( + transform::GetYaw(fixed_frame_pose_in_map.rotation()), + Eigen::Vector3d::UnitZ())), + nullptr, + absl::make_unique>(), + &problem)); + fixed_frame_pose_initialized = true; + } + + problem.AddResidualBlock( + SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose), + options_.fixed_frame_pose_use_tolerant_loss() ? + new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) : nullptr, + C_fixed_frames.at(trajectory_id).rotation(), + C_fixed_frames.at(trajectory_id).translation(), + C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation()); + } + } + // Solve. + ceres::Solver::Summary summary; + ceres::Solve( + common::CreateCeresSolverOptions(options_.ceres_solver_options()), + &problem, &summary); + if (options_.log_solver_summary()) { + LOG(INFO) << summary.FullReport(); + for (const auto& trajectory_id_and_data : trajectory_data_) { + const int trajectory_id = trajectory_id_and_data.first; + const TrajectoryData& trajectory_data = trajectory_id_and_data.second; + if (trajectory_id != 0) { + LOG(INFO) << "Trajectory " << trajectory_id << ":"; + } + LOG(INFO) << "Gravity was: " << trajectory_data.gravity_constant; + const auto& imu_calibration = trajectory_data.imu_calibration; + LOG(INFO) << "IMU correction was: " + << common::RadToDeg(2. * + std::acos(std::abs(imu_calibration[0]))) + << " deg (" << imu_calibration[0] << ", " << imu_calibration[1] + << ", " << imu_calibration[2] << ", " << imu_calibration[3] + << ")"; + } + } + + // Store the result. + for (const auto& C_submap_id_data : C_submaps) { + submap_data_.at(C_submap_id_data.id).global_pose = + C_submap_id_data.data.ToRigid(); + } + for (const auto& C_node_id_data : C_nodes) { + node_data_.at(C_node_id_data.id).global_pose = + C_node_id_data.data.ToRigid(); + } + for (const auto& C_fixed_frame : C_fixed_frames) { + trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map = + C_fixed_frame.second.ToRigid(); + } + for (const auto& C_landmark : C_landmarks) { + landmark_data_[C_landmark.first] = C_landmark.second.ToRigid(); + } +} + +std::unique_ptr +OptimizationProblem3D::CalculateOdometryBetweenNodes( + const int trajectory_id, const NodeSpec3D& first_node_data, + const NodeSpec3D& second_node_data) const { + if (odometry_data_.HasTrajectory(trajectory_id)) { + const std::unique_ptr first_node_odometry = + Interpolate(odometry_data_, trajectory_id, first_node_data.time); + const std::unique_ptr second_node_odometry = + Interpolate(odometry_data_, trajectory_id, second_node_data.time); + if (first_node_odometry != nullptr && second_node_odometry != nullptr) { + const transform::Rigid3d relative_odometry = + first_node_odometry->inverse() * (*second_node_odometry); + return absl::make_unique(relative_odometry); + } + } + return nullptr; +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d.h new file mode 100644 index 0000000..721fde9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d.h @@ -0,0 +1,139 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_ + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/types/optional.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/pose_graph/optimization_problem_options.pb.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/map_by_time.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/transform_interpolation_buffer.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +struct NodeSpec3D { + common::Time time; + transform::Rigid3d local_pose; + transform::Rigid3d global_pose; +}; + +struct SubmapSpec3D { + transform::Rigid3d global_pose; +}; + +class OptimizationProblem3D + : public OptimizationProblemInterface { + public: + explicit OptimizationProblem3D( + const optimization::proto::OptimizationProblemOptions& options); + ~OptimizationProblem3D(); + + OptimizationProblem3D(const OptimizationProblem3D&) = delete; + OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete; + + void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override; + void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) override; + void AddTrajectoryNode(int trajectory_id, + const NodeSpec3D& node_data) override; + void InsertTrajectoryNode(const NodeId& node_id, + const NodeSpec3D& node_data) override; + void TrimTrajectoryNode(const NodeId& node_id) override; + void AddSubmap(int trajectory_id, + const transform::Rigid3d& global_submap_pose) override; + void InsertSubmap(const SubmapId& submap_id, + const transform::Rigid3d& global_submap_pose) override; + void TrimSubmap(const SubmapId& submap_id) override; + void SetMaxNumIterations(int32 max_num_iterations) override; + + void Solve( + const std::vector& constraints, + const std::map& + trajectories_state, + const std::map& landmark_nodes) override; + + const MapById& node_data() const override { + return node_data_; + } + const MapById& submap_data() const override { + return submap_data_; + } + const std::map& landmark_data() + const override { + return landmark_data_; + } + const sensor::MapByTime& imu_data() const override { + return imu_data_; + } + const sensor::MapByTime& odometry_data() + const override { + return odometry_data_; + } + + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); + void SetTrajectoryData( + int trajectory_id, + const PoseGraphInterface::TrajectoryData& trajectory_data); + const sensor::MapByTime& fixed_frame_pose_data() + const { + return fixed_frame_pose_data_; + } + const std::map& trajectory_data() + const { + return trajectory_data_; + } + + private: + // Computes the relative pose between two nodes based on odometry data. + std::unique_ptr CalculateOdometryBetweenNodes( + int trajectory_id, const NodeSpec3D& first_node_data, + const NodeSpec3D& second_node_data) const; + + optimization::proto::OptimizationProblemOptions options_; + MapById node_data_; + MapById submap_data_; + std::map landmark_data_; + sensor::MapByTime imu_data_; + sensor::MapByTime odometry_data_; + sensor::MapByTime fixed_frame_pose_data_; + std::map trajectory_data_; +}; + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc new file mode 100644 index 0000000..26f9685 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -0,0 +1,202 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/optimization/optimization_problem_3d.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_options.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +class OptimizationProblem3DTest : public ::testing::Test { + protected: + OptimizationProblem3DTest() + : optimization_problem_(CreateOptions()), rng_(45387) {} + + optimization::proto::OptimizationProblemOptions CreateOptions() { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + acceleration_weight = 2e-5, + rotation_weight = 1e-3, + huber_scale = 1., + local_slam_pose_translation_weight = 1e-2, + local_slam_pose_rotation_weight = 1e-2, + odometry_translation_weight = 1e-2, + odometry_rotation_weight = 1e-2, + fixed_frame_pose_translation_weight = 1e1, + fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, + log_solver_summary = true, + use_online_imu_extrinsics_in_3d = true, + fix_z_in_3d = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 200, + num_threads = 4, + }, + })text"); + return optimization::CreateOptimizationProblemOptions( + parameter_dictionary.get()); + } + + transform::Rigid3d RandomTransform(double translation_size, + double rotation_size) { + std::uniform_real_distribution translation_distribution( + -translation_size, translation_size); + const double x = translation_distribution(rng_); + const double y = translation_distribution(rng_); + const double z = translation_distribution(rng_); + std::uniform_real_distribution rotation_distribution(-rotation_size, + rotation_size); + const double rx = rotation_distribution(rng_); + const double ry = rotation_distribution(rng_); + const double rz = rotation_distribution(rng_); + return transform::Rigid3d(Eigen::Vector3d(x, y, z), + transform::AngleAxisVectorToRotationQuaternion( + Eigen::Vector3d(rx, ry, rz))); + } + + transform::Rigid3d RandomYawOnlyTransform(double translation_size, + double rotation_size) { + std::uniform_real_distribution translation_distribution( + -translation_size, translation_size); + const double x = translation_distribution(rng_); + const double y = translation_distribution(rng_); + const double z = translation_distribution(rng_); + std::uniform_real_distribution rotation_distribution(-rotation_size, + rotation_size); + const double rz = rotation_distribution(rng_); + return transform::Rigid3d(Eigen::Vector3d(x, y, z), + transform::AngleAxisVectorToRotationQuaternion( + Eigen::Vector3d(0., 0., rz))); + } + + OptimizationProblem3D optimization_problem_; + std::mt19937 rng_; +}; + +transform::Rigid3d AddNoise(const transform::Rigid3d& transform, + const transform::Rigid3d& noise) { + const Eigen::Quaterniond noisy_rotation(noise.rotation() * + transform.rotation()); + return transform::Rigid3d(transform.translation() + noise.translation(), + noisy_rotation); +} + +TEST_F(OptimizationProblem3DTest, ReducesNoise) { + constexpr int kNumNodes = 100; + const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity(); + const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation( + Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); + const int kTrajectoryId = 0; + + struct NoisyNode { + transform::Rigid3d ground_truth_pose; + transform::Rigid3d noise; + }; + std::vector test_data; + for (int j = 0; j != kNumNodes; ++j) { + test_data.push_back( + NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)}); + } + + common::Time now = common::FromUniversal(0); + for (const NoisyNode& node : test_data) { + const transform::Rigid3d pose = + AddNoise(node.ground_truth_pose, node.noise); + optimization_problem_.AddImuData( + kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81, + Eigen::Vector3d::Zero()}); + optimization_problem_.AddTrajectoryNode(kTrajectoryId, + NodeSpec3D{now, pose, pose}); + now += common::FromSeconds(0.1); + } + + std::vector constraints; + for (int j = 0; j != kNumNodes; ++j) { + constraints.push_back(OptimizationProblem3D::Constraint{ + SubmapId{kTrajectoryId, 0}, NodeId{kTrajectoryId, j}, + OptimizationProblem3D::Constraint::Pose{ + AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1., + 1.}}); + // We add an additional independent, but equally noisy observation. + constraints.push_back(OptimizationProblem3D::Constraint{ + SubmapId{kTrajectoryId, 1}, NodeId{kTrajectoryId, j}, + OptimizationProblem3D::Constraint::Pose{ + AddNoise(test_data[j].ground_truth_pose, + RandomYawOnlyTransform(0.2, 0.3)), + 1., 1.}}); + // We add very noisy data with a low weight to verify it is mostly ignored. + constraints.push_back(OptimizationProblem3D::Constraint{ + SubmapId{kTrajectoryId, 2}, NodeId{kTrajectoryId, j}, + OptimizationProblem3D::Constraint::Pose{ + kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * + RandomTransform(1e3, 3.), + 1e-9, 1e-9}}); + } + + double translation_error_before = 0.; + double rotation_error_before = 0.; + const auto& node_data = optimization_problem_.node_data(); + for (int j = 0; j != kNumNodes; ++j) { + translation_error_before += + (test_data[j].ground_truth_pose.translation() - + node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation()) + .norm(); + rotation_error_before += + transform::GetAngle(test_data[j].ground_truth_pose.inverse() * + node_data.at(NodeId{kTrajectoryId, j}).global_pose); + } + + optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); + optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); + optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform); + const std::map kTrajectoriesState = + {{kTrajectoryId, PoseGraphInterface::TrajectoryState::ACTIVE}}; + optimization_problem_.Solve(constraints, kTrajectoriesState, {}); + + double translation_error_after = 0.; + double rotation_error_after = 0.; + for (int j = 0; j != kNumNodes; ++j) { + translation_error_after += + (test_data[j].ground_truth_pose.translation() - + node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation()) + .norm(); + rotation_error_after += + transform::GetAngle(test_data[j].ground_truth_pose.inverse() * + node_data.at(NodeId{kTrajectoryId, j}).global_pose); + } + + EXPECT_GT(0.8 * translation_error_before, translation_error_after); + EXPECT_GT(0.8 * rotation_error_before, rotation_error_after); +} + +} // namespace +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_interface.h new file mode 100644 index 0000000..7e1c57c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_interface.h @@ -0,0 +1,89 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/time.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/map_by_time.h" +#include "cartographer/sensor/odometry_data.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +// Implements the SPA loop closure method. +template +class OptimizationProblemInterface { + public: + using Constraint = PoseGraphInterface::Constraint; + using LandmarkNode = PoseGraphInterface::LandmarkNode; + + OptimizationProblemInterface() {} + virtual ~OptimizationProblemInterface() {} + + OptimizationProblemInterface(const OptimizationProblemInterface&) = delete; + OptimizationProblemInterface& operator=(const OptimizationProblemInterface&) = + delete; + + virtual void AddImuData(int trajectory_id, + const sensor::ImuData& imu_data) = 0; + virtual void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) = 0; + virtual void AddTrajectoryNode(int trajectory_id, + const NodeDataType& node_data) = 0; + virtual void InsertTrajectoryNode(const NodeId& node_id, + const NodeDataType& node_data) = 0; + virtual void TrimTrajectoryNode(const NodeId& node_id) = 0; + virtual void AddSubmap(int trajectory_id, + const RigidTransformType& global_submap_pose) = 0; + virtual void InsertSubmap(const SubmapId& submap_id, + const RigidTransformType& global_submap_pose) = 0; + virtual void TrimSubmap(const SubmapId& submap_id) = 0; + virtual void SetMaxNumIterations(int32 max_num_iterations) = 0; + + // Optimizes the global poses. + virtual void Solve( + const std::vector& constraints, + const std::map& + trajectories_state, + const std::map& landmark_nodes) = 0; + + virtual const MapById& node_data() const = 0; + virtual const MapById& submap_data() const = 0; + virtual const std::map& landmark_data() + const = 0; + virtual const sensor::MapByTime& imu_data() const = 0; + virtual const sensor::MapByTime& odometry_data() + const = 0; +}; + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_options.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_options.cc new file mode 100644 index 0000000..39a0a56 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_options.cc @@ -0,0 +1,64 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/optimization/optimization_problem_options.h" + +#include "cartographer/common/internal/ceres_solver_options.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +proto::OptimizationProblemOptions CreateOptimizationProblemOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::OptimizationProblemOptions options; + options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale")); + options.set_acceleration_weight( + parameter_dictionary->GetDouble("acceleration_weight")); + options.set_rotation_weight( + parameter_dictionary->GetDouble("rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); + options.set_local_slam_pose_translation_weight( + parameter_dictionary->GetDouble("local_slam_pose_translation_weight")); + options.set_local_slam_pose_rotation_weight( + parameter_dictionary->GetDouble("local_slam_pose_rotation_weight")); + options.set_fixed_frame_pose_translation_weight( + parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight")); + options.set_fixed_frame_pose_rotation_weight( + parameter_dictionary->GetDouble("fixed_frame_pose_rotation_weight")); + options.set_fixed_frame_pose_use_tolerant_loss( + parameter_dictionary->GetBool("fixed_frame_pose_use_tolerant_loss")); + options.set_fixed_frame_pose_tolerant_loss_param_a( + parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_a")); + options.set_fixed_frame_pose_tolerant_loss_param_b( + parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_b")); + options.set_log_solver_summary( + parameter_dictionary->GetBool("log_solver_summary")); + options.set_use_online_imu_extrinsics_in_3d( + parameter_dictionary->GetBool("use_online_imu_extrinsics_in_3d")); + options.set_fix_z_in_3d(parameter_dictionary->GetBool("fix_z_in_3d")); + *options.mutable_ceres_solver_options() = + common::CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("ceres_solver_options").get()); + return options; +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_options.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_options.h new file mode 100644 index 0000000..5351e39 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/optimization/optimization_problem_options.h @@ -0,0 +1,34 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/pose_graph/optimization_problem_options.pb.h" + +namespace cartographer { +namespace mapping { +namespace optimization { + +proto::OptimizationProblemOptions CreateOptimizationProblemOptions( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace optimization +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/pose_graph_data.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/pose_graph_data.h new file mode 100644 index 0000000..666f942 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/pose_graph_data.h @@ -0,0 +1,92 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_ + +#include +#include +#include + +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" +#include "cartographer/mapping/internal/trajectory_connectivity_state.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/submaps.h" + +namespace cartographer { +namespace mapping { + +// The current state of the submap in the background threads. After this +// transitions to 'kFinished', all nodes are tried to match +// against this submap. Likewise, all new nodes are matched against submaps in +// that state. +enum class SubmapState { kNoConstraintSearch, kFinished }; + +struct InternalTrajectoryState { + enum class DeletionState { + NORMAL, + SCHEDULED_FOR_DELETION, + WAIT_FOR_DELETION + }; + + PoseGraphInterface::TrajectoryState state = + PoseGraphInterface::TrajectoryState::ACTIVE; + DeletionState deletion_state = DeletionState::NORMAL; +}; + +struct InternalSubmapData { + std::shared_ptr submap; + SubmapState state = SubmapState::kNoConstraintSearch; + + // IDs of the nodes that were inserted into this map together with + // constraints for them. They are not to be matched again when this submap + // becomes 'kFinished'. + std::set node_ids; +}; + +struct PoseGraphData { + // Submaps get assigned an ID and state as soon as they are seen, even + // before they take part in the background computations. + MapById submap_data; + + // Global submap poses currently used for displaying data. + MapById global_submap_poses_2d; + MapById global_submap_poses_3d; + + // Data that are currently being shown. + MapById trajectory_nodes; + + // Global landmark poses with all observations. + std::map + landmark_nodes; + + // How our various trajectories are related. + TrajectoryConnectivityState trajectory_connectivity_state; + int num_trajectory_nodes = 0; + std::map trajectories_state; + + // Set of all initial trajectory poses. + std::map initial_trajectory_poses; + + std::vector constraints; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator.cc new file mode 100644 index 0000000..18c9450 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator.cc @@ -0,0 +1,180 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/range_data_collator.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +constexpr float RangeDataCollator::kDefaultIntensityValue; + +/** + * @brief 多个雷达数据的时间同步 + * + * @param[in] sensor_id 雷达数据的话题 + * @param[in] timed_point_cloud_data 雷达数据 + * @return sensor::TimedPointCloudOriginData 根据时间处理之后的数据 + */ +sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( + const std::string& sensor_id, + sensor::TimedPointCloudData timed_point_cloud_data) { // 第一次拷贝 + CHECK_NE(expected_sensor_ids_.count(sensor_id), 0); + + // 从sensor_bridge传过来的数据的intensities为空 + //。。。timed_point_cloud_data.intensities.resize( + //。。。 timed_point_cloud_data.ranges.size(), kDefaultIntensityValue); + + // TODO(gaschler): These two cases can probably be one. + // 如果同话题的点云, 还有没处理的, 就先处同步没处理的点云, 将当前点云保存 + if (id_to_pending_data_.count(sensor_id) != 0) { + // current_end_为上一次时间同步的结束时间 + // current_start_为本次时间同步的开始时间 + current_start_ = current_end_; + // When we have two messages of the same sensor, move forward the older of + // the two (do not send out current). + // 本次时间同步的结束时间为这帧点云数据的结束时间 + current_end_ = id_to_pending_data_.at(sensor_id).time; + auto result = CropAndMerge(); + // 保存当前点云 + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); + return result; + } + + // 先将当前点云添加到 等待时间同步的map中 + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); + + // 等到range数据的话题都到来之后再进行处理 + if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { + return {}; + } + + current_start_ = current_end_; + // We have messages from all sensors, move forward to oldest. + common::Time oldest_timestamp = common::Time::max(); + // 找到所有传感器数据中最早的时间戳(点云最后一个点的时间) + for (const auto& pair : id_to_pending_data_) { + oldest_timestamp = std::min(oldest_timestamp, pair.second.time); + } + // current_end_是本次时间同步的结束时间 + // 是待时间同步map中的 所有点云中最早的时间戳 + current_end_ = oldest_timestamp; + return CropAndMerge(); +} + +// 对时间段内的数据进行截取与合并, 返回时间同步后的点云 +sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { + sensor::TimedPointCloudOriginData result{current_end_, {}, {}}; + bool warned_for_dropped_points = false; + // 遍历所有的传感器话题 + for (auto it = id_to_pending_data_.begin(); + it != id_to_pending_data_.end();) { + // 获取数据的引用 + sensor::TimedPointCloudData& data = it->second; + const sensor::TimedPointCloud& ranges = it->second.ranges; + const std::vector& intensities = it->second.intensities; + + // 找到点云中 最后一个时间戳小于current_start_的点的索引 + auto overlap_begin = ranges.begin(); + while (overlap_begin < ranges.end() && + data.time + common::FromSeconds((*overlap_begin).time) < + current_start_) { + ++overlap_begin; + } + + // 找到点云中 最后一个时间戳小于等于current_end_的点的索引 + auto overlap_end = overlap_begin; + while (overlap_end < ranges.end() && + data.time + common::FromSeconds((*overlap_end).time) <= + current_end_) { + ++overlap_end; + } + + // 丢弃点云中时间比起始时间早的点, 每执行一下CropAndMerge()打印一次log + if (ranges.begin() < overlap_begin && !warned_for_dropped_points) { + LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin) + << " earlier points."; + warned_for_dropped_points = true; + } + + // Copy overlapping range. + if (overlap_begin < overlap_end) { + // 获取下个点云的index, 即当前vector的个数 + std::size_t origin_index = result.origins.size(); + result.origins.push_back(data.origin); // 插入原点坐标 + + // 获取此传感器时间与集合时间戳的误差, + const float time_correction = + static_cast(common::ToSeconds(data.time - current_end_)); + + auto intensities_overlap_it = + intensities.begin() + (overlap_begin - ranges.begin()); + // reserve() 在预留空间改变时, 会将之前的数据拷贝到新的内存中 + result.ranges.reserve(result.ranges.size() + + std::distance(overlap_begin, overlap_end)); + + // 填充数据 + for (auto overlap_it = overlap_begin; overlap_it != overlap_end; + ++overlap_it, ++intensities_overlap_it) { + sensor::TimedPointCloudOriginData::RangeMeasurement point{ + *overlap_it, *intensities_overlap_it, origin_index}; + // current_end_ + point_time[3]_after == in_timestamp + + // point_time[3]_before + // 针对每个点时间戳进行修正, 让最后一个点的时间为0 + point.point_time.time += time_correction; + result.ranges.push_back(point); + } // end for + } // end if + + // Drop buffered points until overlap_end. + // 如果点云每个点都用了, 则可将这个数据进行删除 + if (overlap_end == ranges.end()) { + it = id_to_pending_data_.erase(it); + } + // 如果一个点都没用, 就先放这, 看下一个数据 + else if (overlap_end == ranges.begin()) { + ++it; + } + // 用了一部分的点 + else { + const auto intensities_overlap_end = + intensities.begin() + (overlap_end - ranges.begin()); + // 将用了的点删除, 这里的赋值是拷贝 + data = sensor::TimedPointCloudData{ + data.time, data.origin, + sensor::TimedPointCloud(overlap_end, ranges.end()), + std::vector(intensities_overlap_end, intensities.end())}; + ++it; + } + } // end for + + // 对各传感器的点云 按照每个点的时间从小到大进行排序 + std::sort(result.ranges.begin(), result.ranges.end(), + [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a, + const sensor::TimedPointCloudOriginData::RangeMeasurement& b) { + return a.point_time.time < b.point_time.time; + }); + return result; +} + +} // namespace mapping +} // namespace cartographer + diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator.h new file mode 100644 index 0000000..6bb6999 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator.h @@ -0,0 +1,61 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_RANGE_DATA_COLLATOR_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_RANGE_DATA_COLLATOR_H_ + +#include + +#include "absl/memory/memory.h" +#include "cartographer/sensor/timed_point_cloud_data.h" + +namespace cartographer { +namespace mapping { + +// Synchronizes TimedPointCloudData from different sensors. Input needs only be +// monotonous in 'TimedPointCloudData::time', output is monotonous in per-point +// timing. Up to one message per sensor is buffered, so a delay of the period of +// the slowest sensor may be introduced, which can be alleviated by passing +// subdivisions. +class RangeDataCollator { + public: + explicit RangeDataCollator( + const std::vector& expected_range_sensor_ids) + : expected_sensor_ids_(expected_range_sensor_ids.begin(), + expected_range_sensor_ids.end()) {} + + // If timed_point_cloud_data has incomplete intensity data, we will fill the + // missing intensities with kDefaultIntensityValue. + sensor::TimedPointCloudOriginData AddRangeData( + const std::string& sensor_id, + sensor::TimedPointCloudData timed_point_cloud_data); + + private: + sensor::TimedPointCloudOriginData CropAndMerge(); + + const std::set expected_sensor_ids_; + // Store at most one message for each sensor. + std::map id_to_pending_data_; //集合， + common::Time current_start_ = common::Time::min(); + common::Time current_end_ = common::Time::min(); + + constexpr static float kDefaultIntensityValue = 0.f; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_RANGE_DATA_COLLATOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator_test.cc new file mode 100644 index 0000000..43ceb00 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/range_data_collator_test.cc @@ -0,0 +1,205 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/range_data_collator.h" + +#include "cartographer/common/time.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +const int kNumSamples = 10; + +sensor::TimedPointCloudData CreateFakeRangeData(int from, int to, + bool fake_intensities) { + double duration = common::ToSeconds(common::FromUniversal(to) - + common::FromUniversal(from)); + sensor::TimedPointCloudData result{ + common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}, {}}; + result.ranges.reserve(kNumSamples); + for (int i = 0; i < kNumSamples; ++i) { + double fraction = static_cast(i) / (kNumSamples - 1); + float relative_time = (1. - fraction) * -duration; + result.ranges.push_back( + {Eigen::Vector3f{1., 2., static_cast(fraction)}, relative_time}); + if (fake_intensities) { + result.intensities.push_back(result.ranges.back().position.z()); + } + } + return result; +} + +bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) { + std::vector timestamps; + timestamps.reserve(data.ranges.size()); + for (const auto& range : data.ranges) { + timestamps.push_back(range.point_time.time); + } + return std::is_sorted(timestamps.begin(), timestamps.end()); +} + +void IntensitiesAreConsistent(const sensor::TimedPointCloudOriginData& data) { + for (const auto& range : data.ranges) { + EXPECT_NEAR(range.point_time.position.z(), range.intensity, 1e-6); + } +} + +TEST(RangeDataCollatorTest, SingleSensor) { + const std::string sensor_id = "single_sensor"; + RangeDataCollator collator({sensor_id}); + auto output_0 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300, false)); + EXPECT_EQ(common::ToUniversal(output_0.time), 300); + EXPECT_EQ(output_0.origins.size(), 1); + EXPECT_EQ(output_0.ranges.size(), kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_0)); + auto output_1 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); + EXPECT_EQ(common::ToUniversal(output_1.time), 500); + EXPECT_EQ(output_1.origins.size(), 1); + ASSERT_EQ(output_1.ranges.size(), kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_1)); + EXPECT_NEAR(common::ToUniversal( + output_1.time + + common::FromSeconds(output_1.ranges[0].point_time.time)), + 300, 2); + auto output_2 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); + EXPECT_EQ(common::ToUniversal(output_2.time), 510); + EXPECT_EQ(output_2.origins.size(), 1); + EXPECT_EQ(output_2.ranges.size(), 1); + EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); +} + +TEST(RangeDataCollatorTest, SingleSensorEmptyData) { + const std::string sensor_id = "single_sensor"; + RangeDataCollator collator({sensor_id}); + sensor::TimedPointCloudData empty_data{ + common::FromUniversal(300), {}, {}, {}}; + auto output_0 = collator.AddRangeData(sensor_id, empty_data); + EXPECT_EQ(output_0.time, empty_data.time); + EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size()); + EXPECT_TRUE(ArePointTimestampsSorted(output_0)); + auto output_1 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); + EXPECT_EQ(common::ToUniversal(output_1.time), 500); + EXPECT_EQ(output_1.origins.size(), 1); + ASSERT_EQ(output_1.ranges.size(), kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_1)); + EXPECT_NEAR(common::ToUniversal( + output_1.time + + common::FromSeconds(output_1.ranges[0].point_time.time)), + 300, 2); + auto output_2 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); + EXPECT_EQ(common::ToUniversal(output_2.time), 510); + EXPECT_EQ(output_2.origins.size(), 1); + EXPECT_EQ(output_2.ranges.size(), 1); + EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); +} + +TEST(RangeDataCollatorTest, TwoSensors) { + const std::string sensor_0 = "sensor_0"; + const std::string sensor_1 = "sensor_1"; + RangeDataCollator collator({sensor_0, sensor_1}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300, false)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310, false)); + EXPECT_EQ(output_1.origins.size(), 2); + EXPECT_EQ(common::ToUniversal(output_1.time), 300); + ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1); + EXPECT_NEAR(common::ToUniversal( + output_1.time + + common::FromSeconds(output_1.ranges[0].point_time.time)), + -1000, 2); + EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_1)); + auto output_2 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500, false)); + EXPECT_EQ(output_2.origins.size(), 2); + EXPECT_EQ(common::ToUniversal(output_2.time), 310); + ASSERT_EQ(output_2.ranges.size(), 2); + EXPECT_NEAR(common::ToUniversal( + output_2.time + + common::FromSeconds(output_2.ranges[0].point_time.time)), + 300, 2); + EXPECT_EQ(output_2.ranges.back().point_time.time, 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + // Sending the same sensor will flush everything before. + auto output_3 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700, false)); + EXPECT_EQ(common::ToUniversal(output_3.time), 500); + EXPECT_EQ( + output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(), + 3 * kNumSamples); + EXPECT_EQ(output_3.ranges.back().point_time.time, 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); +} + +TEST(RangeDataCollatorTest, ThreeSensors) { + const std::string sensor_0 = "sensor_0"; + const std::string sensor_1 = "sensor_1"; + const std::string sensor_2 = "sensor_2"; + RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, false)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, false)); + EXPECT_EQ(output_1.ranges.size(), 0); + auto output_2 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, false)); + EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + auto output_3 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, false)); + EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); +} + +TEST(RangeDataCollatorTest, ThreeSensorsWithIntensities) { + const std::string sensor_0 = "sensor_0"; + const std::string sensor_1 = "sensor_1"; + const std::string sensor_2 = "sensor_2"; + RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, true)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, true)); + EXPECT_EQ(output_1.ranges.size(), 0); + auto output_2 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, true)); + EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + IntensitiesAreConsistent(output_2); + auto output_3 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, true)); + EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); + IntensitiesAreConsistent(output_3); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.cc new file mode 100644 index 0000000..966ebe3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.cc @@ -0,0 +1,26 @@ +#include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::RealTimeCorrelativeScanMatcherOptions options; + options.set_linear_search_window( + parameter_dictionary->GetDouble("linear_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + options.set_translation_delta_cost_weight( + parameter_dictionary->GetDouble("translation_delta_cost_weight")); + options.set_rotation_delta_cost_weight( + parameter_dictionary->GetDouble("rotation_delta_cost_weight")); + CHECK_GE(options.translation_delta_cost_weight(), 0.); + CHECK_GE(options.rotation_delta_cost_weight(), 0.); + return options; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h new file mode 100644 index 0000000..47d3d01 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h @@ -0,0 +1,36 @@ + +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/submap_controller.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/submap_controller.cc new file mode 100644 index 0000000..03e4f8e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/submap_controller.cc @@ -0,0 +1,48 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/submap_controller.h" + +namespace cartographer { +namespace mapping { + +template <> +std::shared_ptr +SubmapController::CreateSubmap( + const mapping::proto::Submap& proto) { + if (proto.submap_2d().num_range_data() != 1) { + LOG(WARNING) << "Refusing to create partially filled submap: " + << proto.submap_2d().num_range_data(); + return nullptr; + } + return std::make_shared(proto.submap_2d(), + &conversion_tables_); +} + +template <> +std::shared_ptr +SubmapController::CreateSubmap( + const mapping::proto::Submap& proto) { + if (proto.submap_3d().num_range_data() != 1) { + LOG(INFO) << "Refusing to create partially filled submap: " + << proto.submap_3d().num_range_data(); + return nullptr; + } + return std::make_shared(proto.submap_3d()); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/submap_controller.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/submap_controller.h new file mode 100644 index 0000000..88922cd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/submap_controller.h @@ -0,0 +1,81 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H +#define CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H + +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/proto/serialization.pb.h" + +namespace cartographer { +namespace mapping { + +template +class SubmapController { + public: + std::shared_ptr UpdateSubmap( + const mapping::proto::Submap& proto) { + mapping::SubmapId submap_id{proto.submap_id().trajectory_id(), + proto.submap_id().submap_index()}; + std::shared_ptr submap_ptr; + auto submap_it = unfinished_submaps_.find(submap_id); + if (submap_it == unfinished_submaps_.end()) { + submap_ptr = CreateSubmap(proto); + if (submap_ptr) { + unfinished_submaps_.Insert(submap_id, submap_ptr); + } + return submap_ptr; + } + submap_ptr = submap_it->data; + CHECK(submap_ptr); + submap_ptr->UpdateFromProto(proto); + + // If the submap was just finished by the recent update, remove it from + // the list of unfinished submaps. + if (submap_ptr->insertion_finished()) { + unfinished_submaps_.Trim(submap_id); + } else { + // If the submap is unfinished set the 'num_range_data' to 0 since we + // haven't changed the HybridGrid. + submap_ptr->set_num_range_data(0); + } + return submap_ptr; + } + + private: + std::shared_ptr CreateSubmap(const mapping::proto::Submap& proto); + + mapping::MapById> + unfinished_submaps_; + + ValueConversionTables conversion_tables_; +}; + +template <> +std::shared_ptr +SubmapController::CreateSubmap( + const mapping::proto::Submap& proto); +template <> +std::shared_ptr +SubmapController::CreateSubmap( + const mapping::proto::Submap& proto); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/fake_trimmable.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/fake_trimmable.h new file mode 100644 index 0000000..aeb80f9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/fake_trimmable.h @@ -0,0 +1,117 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_TESTING_FAKE_TRIMMABLE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_FAKE_TRIMMABLE_H_ + +#include + +#include "cartographer/mapping/pose_graph_trimmer.h" + +namespace cartographer { +namespace mapping { +namespace testing { + +class FakeTrimmable : public Trimmable { + public: + FakeTrimmable() = default; + + // Populates dummy SubmapIDs. + FakeTrimmable(int trajectory_id, int num_submaps) { + for (int index = 0; index < num_submaps; ++index) { + submap_data_.Insert(SubmapId{trajectory_id, index}, {}); + } + } + ~FakeTrimmable() override {} + + int num_submaps(const int trajectory_id) const override { + return submap_data_.size() - trimmed_submaps_.size(); + } + + std::vector GetSubmapIds(int trajectory_id) const override { + std::vector submap_ids; + for (const auto& submap : submap_data_) { + submap_ids.push_back(submap.id); + } + return submap_ids; + } + + void set_submap_data( + const MapById& submap_data) { + submap_data_ = submap_data; + } + + MapById* mutable_submap_data() { + return &submap_data_; + } + + MapById GetOptimizedSubmapData() + const override { + return submap_data_; + } + + void set_trajectory_nodes( + const MapById& trajectory_nodes) { + trajectory_nodes_ = trajectory_nodes; + } + + MapById* mutable_trajectory_nodes() { + return &trajectory_nodes_; + } + + const MapById& GetTrajectoryNodes() const override { + return trajectory_nodes_; + } + + void set_constraints( + const std::vector& constraints) { + constraints_ = constraints; + } + + std::vector* mutable_constraints() { + return &constraints_; + } + + const std::vector& GetConstraints() + const override { + return constraints_; + } + + void TrimSubmap(const SubmapId& submap_id) override { + trimmed_submaps_.push_back(submap_id); + } + + bool IsFinished(const int trajectory_id) const override { return false; } + + void SetTrajectoryState( + int /*trajectory_id*/, + PoseGraphInterface::TrajectoryState /*state*/) override {} + + std::vector trimmed_submaps() { return trimmed_submaps_; } + + private: + std::vector trimmed_submaps_; + + std::vector constraints_; + MapById trajectory_nodes_; + MapById submap_data_; +}; + +} // namespace testing +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_TESTING_FAKE_TRIMMABLE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_map_builder.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_map_builder.h new file mode 100644 index 0000000..a61319a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_map_builder.h @@ -0,0 +1,68 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_MAP_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_MAP_BUILDER_H_ + +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using testing::_; + +namespace cartographer { +namespace mapping { +namespace testing { + +class MockMapBuilder : public mapping::MapBuilderInterface { + public: + MOCK_METHOD3( + AddTrajectoryBuilder, + int(const std::set &expected_sensor_ids, + const mapping::proto::TrajectoryBuilderOptions &trajectory_options, + mapping::MapBuilderInterface::LocalSlamResultCallback + local_slam_result_callback)); + MOCK_METHOD1(AddTrajectoryForDeserialization, + int(const mapping::proto::TrajectoryBuilderOptionsWithSensorIds + &options_with_sensor_ids_proto)); + MOCK_CONST_METHOD1(GetTrajectoryBuilder, + mapping::TrajectoryBuilderInterface *(int trajectory_id)); + MOCK_METHOD1(FinishTrajectory, void(int trajectory_id)); + MOCK_METHOD2(SubmapToProto, + std::string(const mapping::SubmapId &, + mapping::proto::SubmapQuery::Response *)); + MOCK_METHOD2(SerializeState, void(bool, io::ProtoStreamWriterInterface *)); + MOCK_METHOD2(SerializeStateToFile, bool(bool, const std::string &)); + MOCK_METHOD2(LoadState, + std::map(io::ProtoStreamReaderInterface *, bool)); + MOCK_METHOD2(LoadStateFromFile, + std::map(const std::string &, bool)); + MOCK_CONST_METHOD0(num_trajectory_builders, int()); + MOCK_METHOD0(pose_graph, mapping::PoseGraphInterface *()); + MOCK_CONST_METHOD0( + GetAllTrajectoryBuilderOptions, + const std::vector + &()); +}; + +} // namespace testing +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_MAP_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_pose_graph.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_pose_graph.h new file mode 100644 index 0000000..dbddfd1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -0,0 +1,70 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_POSE_GRAPH_H_ + +#include "cartographer/mapping/pose_graph_interface.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace testing { + +class MockPoseGraph : public mapping::PoseGraphInterface { + public: + MockPoseGraph() = default; + ~MockPoseGraph() override = default; + + MOCK_METHOD0(RunFinalOptimization, void()); + MOCK_CONST_METHOD0(GetAllSubmapData, + mapping::MapById()); + MOCK_CONST_METHOD1(GetSubmapData, SubmapData(const SubmapId&)); + MOCK_CONST_METHOD0(GetAllSubmapPoses, + mapping::MapById()); + MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int)); + MOCK_CONST_METHOD0( + GetTrajectoryNodes, + mapping::MapById()); + MOCK_CONST_METHOD0( + GetTrajectoryNodePoses, + mapping::MapById()); + MOCK_CONST_METHOD0( + GetTrajectoryStates, + std::map()); + MOCK_CONST_METHOD0(GetLandmarkPoses, + std::map()); + MOCK_METHOD3(SetLandmarkPose, + void(const std::string&, const transform::Rigid3d&, const bool)); + MOCK_METHOD1(DeleteTrajectory, void(int)); + MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int)); + MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int)); + MOCK_CONST_METHOD0( + GetTrajectoryData, + std::map()); + MOCK_CONST_METHOD0(constraints, std::vector()); + MOCK_CONST_METHOD1(ToProto, mapping::proto::PoseGraph(bool)); + MOCK_METHOD1(SetGlobalSlamOptimizationCallback, + void(GlobalSlamOptimizationCallback callback)); +}; + +} // namespace testing +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_POSE_GRAPH_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_trajectory_builder.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_trajectory_builder.h new file mode 100644 index 0000000..64de58c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/mock_trajectory_builder.h @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_TRAJECTORY_BUILDER_H_ + +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace testing { + +class MockTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { + public: + MockTrajectoryBuilder() = default; + ~MockTrajectoryBuilder() override = default; + + MOCK_METHOD2(AddSensorData, + void(const std::string &, const sensor::TimedPointCloudData &)); + MOCK_METHOD2(AddSensorData, + void(const std::string &, const sensor::ImuData &)); + MOCK_METHOD2(AddSensorData, + void(const std::string &, const sensor::OdometryData &)); + MOCK_METHOD2(AddSensorData, + void(const std::string &, const sensor::FixedFramePoseData &)); + MOCK_METHOD2(AddSensorData, + void(const std::string &, const sensor::LandmarkData &)); + + // Some of the platforms we run on may ship with a version of gmock which does + // not yet support move-only types. + MOCK_METHOD1(DoAddLocalSlamResultData, void(mapping::LocalSlamResultData *)); + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override { + DoAddLocalSlamResultData(local_slam_result_data.get()); + } +}; + +} // namespace testing +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_TRAJECTORY_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/test_helpers.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/test_helpers.cc new file mode 100644 index 0000000..53fcd3d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/test_helpers.cc @@ -0,0 +1,180 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/testing/test_helpers.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/config.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { +namespace testing { + +std::unique_ptr<::cartographer::common::LuaParameterDictionary> +ResolveLuaParameters(const std::string& lua_code) { + auto file_resolver = + absl::make_unique<::cartographer::common::ConfigurationFileResolver>( + std::vector{ + std::string(::cartographer::common::kSourceDirectory) + + "/configuration_files"}); + return absl::make_unique<::cartographer::common::LuaParameterDictionary>( + lua_code, std::move(file_resolver)); +} + +std::vector +GenerateFakeRangeMeasurements(double travel_distance, double duration, + double time_step) { + const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized(); + return GenerateFakeRangeMeasurements(kDirection * travel_distance, duration, + time_step, + transform::Rigid3f::Identity()); +} + +std::vector +GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation, + double duration, double time_step, + const transform::Rigid3f& local_to_global) { + std::vector measurements; + cartographer::sensor::TimedPointCloud point_cloud; + for (double angle = 0.; angle < M_PI; angle += 0.01) { + for (double height : {-0.4, -0.2, 0.0, 0.2, 0.4}) { + constexpr double kRadius = 5; + point_cloud.push_back({Eigen::Vector3d{kRadius * std::cos(angle), + kRadius * std::sin(angle), height} + .cast(), + 0.}); + } + } + const Eigen::Vector3f kVelocity = translation / duration; + for (double elapsed_time = 0.; elapsed_time < duration; + elapsed_time += time_step) { + cartographer::common::Time time = + cartographer::common::FromUniversal(123) + + cartographer::common::FromSeconds(elapsed_time); + cartographer::transform::Rigid3f global_pose = + local_to_global * + cartographer::transform::Rigid3f::Translation(elapsed_time * kVelocity); + cartographer::sensor::TimedPointCloud ranges = + cartographer::sensor::TransformTimedPointCloud(point_cloud, + global_pose.inverse()); + measurements.emplace_back(cartographer::sensor::TimedPointCloudData{ + time, Eigen::Vector3f::Zero(), ranges}); + } + return measurements; +} + +proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index, + bool finished) { + proto::Submap proto; + proto.mutable_submap_id()->set_trajectory_id(trajectory_id); + proto.mutable_submap_id()->set_submap_index(submap_index); + proto.mutable_submap_3d()->set_num_range_data(1); + *proto.mutable_submap_3d()->mutable_local_pose() = + transform::ToProto(transform::Rigid3d::Identity()); + proto.mutable_submap_3d()->set_finished(finished); + return proto; +} + +proto::Node CreateFakeNode(int trajectory_id, int node_index) { + proto::Node proto; + proto.mutable_node_id()->set_trajectory_id(trajectory_id); + proto.mutable_node_id()->set_node_index(node_index); + proto.mutable_node_data()->set_timestamp(42); + *proto.mutable_node_data()->mutable_local_pose() = + transform::ToProto(transform::Rigid3d::Identity()); + return proto; +} + +proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node, + const proto::Submap& submap) { + proto::PoseGraph::Constraint proto; + proto.mutable_submap_id()->set_submap_index( + submap.submap_id().submap_index()); + proto.mutable_submap_id()->set_trajectory_id( + submap.submap_id().trajectory_id()); + proto.mutable_node_id()->set_node_index(node.node_id().node_index()); + proto.mutable_node_id()->set_trajectory_id(node.node_id().trajectory_id()); + transform::Rigid3d pose( + Eigen::Vector3d(2., 3., 4.), + Eigen::AngleAxisd(M_PI / 8., Eigen::Vector3d::UnitX())); + *proto.mutable_relative_pose() = transform::ToProto(pose); + proto.set_translation_weight(0.2f); + proto.set_rotation_weight(0.1f); + proto.set_tag(proto::PoseGraph::Constraint::INTER_SUBMAP); + return proto; +} + +proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id, + proto::PoseGraph* pose_graph) { + for (int i = 0; i < pose_graph->trajectory_size(); ++i) { + proto::Trajectory* trajectory = pose_graph->mutable_trajectory(i); + if (trajectory->trajectory_id() == trajectory_id) { + return trajectory; + } + } + proto::Trajectory* trajectory = pose_graph->add_trajectory(); + trajectory->set_trajectory_id(trajectory_id); + return trajectory; +} + +proto::PoseGraph::LandmarkPose CreateFakeLandmark( + const std::string& landmark_id, const transform::Rigid3d& global_pose) { + proto::PoseGraph::LandmarkPose landmark; + landmark.set_landmark_id(landmark_id); + *landmark.mutable_global_pose() = transform::ToProto(global_pose); + return landmark; +} + +void AddToProtoGraph(const proto::Node& node_data, + proto::PoseGraph* pose_graph) { + auto* trajectory = + CreateTrajectoryIfNeeded(node_data.node_id().trajectory_id(), pose_graph); + auto* node = trajectory->add_node(); + node->set_timestamp(node_data.node_data().timestamp()); + node->set_node_index(node_data.node_id().node_index()); + *node->mutable_pose() = node_data.node_data().local_pose(); +} + +void AddToProtoGraph(const proto::Submap& submap_data, + proto::PoseGraph* pose_graph) { + auto* trajectory = CreateTrajectoryIfNeeded( + submap_data.submap_id().trajectory_id(), pose_graph); + auto* submap = trajectory->add_submap(); + submap->set_submap_index(submap_data.submap_id().submap_index()); + if (submap_data.has_submap_2d()) { + *submap->mutable_pose() = submap_data.submap_2d().local_pose(); + } else { + *submap->mutable_pose() = submap_data.submap_3d().local_pose(); + } +} + +void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint, + proto::PoseGraph* pose_graph) { + *pose_graph->add_constraint() = constraint; +} + +void AddToProtoGraph(const proto::PoseGraph::LandmarkPose& landmark, + proto::PoseGraph* pose_graph) { + *pose_graph->add_landmark_poses() = landmark; +} + +} // namespace testing +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/test_helpers.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/test_helpers.h new file mode 100644 index 0000000..98f2bed --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/testing/test_helpers.h @@ -0,0 +1,71 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_TESTING_TEST_HELPERS_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_TEST_HELPERS_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/sensor/timed_point_cloud_data.h" + +namespace cartographer { +namespace mapping { +namespace testing { + +std::unique_ptr<::cartographer::common::LuaParameterDictionary> +ResolveLuaParameters(const std::string& lua_code); + +std::vector +GenerateFakeRangeMeasurements(double travel_distance, double duration, + double time_step); + +std::vector +GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation, + double duration, double time_step, + const transform::Rigid3f& local_to_global); + +proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1, + bool finished = true); + +proto::Node CreateFakeNode(int trajectory_id = 1, int node_index = 1); + +proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node, + const proto::Submap& submap); + +proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id, + proto::PoseGraph* pose_graph); +proto::PoseGraph::LandmarkPose CreateFakeLandmark( + const std::string& landmark_id, const transform::Rigid3d& global_pose); + +void AddToProtoGraph(const proto::Node& node_data, + proto::PoseGraph* pose_graph); + +void AddToProtoGraph(const proto::Submap& submap_data, + proto::PoseGraph* pose_graph); + +void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint, + proto::PoseGraph* pose_graph); + +void AddToProtoGraph(const proto::PoseGraph::LandmarkPose& landmark_node, + proto::PoseGraph* pose_graph); + +} // namespace testing +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_TESTING_TEST_HELPERS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state.cc new file mode 100644 index 0000000..73ddf46 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state.cc @@ -0,0 +1,74 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/trajectory_connectivity_state.h" + +namespace cartographer { +namespace mapping { + +void TrajectoryConnectivityState::Add(const int trajectory_id) { + connected_components_.Add(trajectory_id); +} + +void TrajectoryConnectivityState::Connect(const int trajectory_id_a, + const int trajectory_id_b, + const common::Time time) { + if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) { + // The trajectories are transitively connected, i.e. they belong to the same + // connected component. In this case we only update the last connection time + // of those two trajectories. + auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); + if (last_connection_time_map_[sorted_pair] < time) { + last_connection_time_map_[sorted_pair] = time; + } + } else { + // The connection between these two trajectories is about to join to + // connected components. Here we update all bipartite trajectory pairs for + // the two connected components with the connection time. This is to quickly + // change to a more efficient loop closure search (by constraining the + // search window) when connected components are joined. + std::vector component_a = + connected_components_.GetComponent(trajectory_id_a); + std::vector component_b = + connected_components_.GetComponent(trajectory_id_b); + for (const auto id_a : component_a) { + for (const auto id_b : component_b) { + auto id_pair = std::minmax(id_a, id_b); + last_connection_time_map_[id_pair] = time; + } + } + } + connected_components_.Connect(trajectory_id_a, trajectory_id_b); +} + +bool TrajectoryConnectivityState::TransitivelyConnected( + const int trajectory_id_a, const int trajectory_id_b) const { + return connected_components_.TransitivelyConnected(trajectory_id_a, + trajectory_id_b); +} + +std::vector> TrajectoryConnectivityState::Components() const { + return connected_components_.Components(); +} + +common::Time TrajectoryConnectivityState::LastConnectionTime( + const int trajectory_id_a, const int trajectory_id_b) { + const auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); + return last_connection_time_map_[sorted_pair]; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state.h new file mode 100644 index 0000000..f7b2069 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state.h @@ -0,0 +1,77 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_INTERNAL_TRAJECTORY_CONNECTIVITY_STATE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_TRAJECTORY_CONNECTIVITY_STATE_H_ + +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/connected_components.h" + +namespace cartographer { +namespace mapping { + +// A class that tracks the connectivity state between trajectories. Compared to +// ConnectedComponents it tracks additionally the last time that a global +// constraint connected to trajectories. +// +// This class is thread-compatible. +class TrajectoryConnectivityState { + public: + TrajectoryConnectivityState() {} + + TrajectoryConnectivityState(const TrajectoryConnectivityState&) = delete; + TrajectoryConnectivityState& operator=(const TrajectoryConnectivityState&) = + delete; + + // Add a trajectory which is initially connected to only itself. + void Add(int trajectory_id); + + // 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 and update the last + // connected time. + void Connect(int trajectory_id_a, int trajectory_id_b, common::Time time); + + // Determines if two trajectories have been (transitively) connected. If + // either trajectory is not being tracked, returns false, except when it is + // the same trajectory, where it returns true. This function is invariant to + // the order of its arguments. + bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) const; + + // The trajectory IDs, grouped by connectivity. + std::vector> Components() const; + + // Return the last connection count between the two trajectories. If either of + // the trajectories is untracked or they have never been connected returns the + // beginning of time. + common::Time LastConnectionTime(int trajectory_id_a, int trajectory_id_b); + + private: + // ConnectedComponents are thread safe. + mutable ConnectedComponents connected_components_; + + // Tracks the last time a direct connection between two trajectories has + // been added. The exception is when a connection between two trajectories + // connects two formerly unconnected connected components. In this case all + // bipartite trajectories entries for these components are updated with the + // new connection time. + std::map, common::Time> last_connection_time_map_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_TRAJECTORY_CONNECTIVITY_STATE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state_test.cc new file mode 100644 index 0000000..f56e695 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/trajectory_connectivity_state_test.cc @@ -0,0 +1,91 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/internal/trajectory_connectivity_state.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { + +TEST(TrajectoryConnectivityStateTest, UnknownTrajectory) { + TrajectoryConnectivityState state; + state.Add(0); + + // Nothing is transitively connected to an unknown trajectory. + EXPECT_FALSE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time()); +} + +TEST(TrajectoryConnectivityStateTest, NotConnected) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + EXPECT_FALSE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time()); +} + +TEST(TrajectoryConnectivityStateTest, Connected) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + state.Connect(0, 1, common::FromUniversal(123456)); + EXPECT_TRUE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456)); +} + +TEST(TrajectoryConnectivityStateTest, UpdateConnectionTime) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + state.Connect(0, 1, common::FromUniversal(123456)); + state.Connect(0, 1, common::FromUniversal(234567)); + EXPECT_TRUE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567)); + + // Connections with an earlier connection time do not update the last + // connection time. + state.Connect(0, 1, common::FromUniversal(123456)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567)); +} + +TEST(TrajectoryConnectivityStateTest, JoinTwoComponents) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + state.Add(2); + state.Add(3); + state.Connect(0, 1, common::FromUniversal(123456)); + state.Connect(2, 3, common::FromUniversal(123456)); + + // Connect the two disjoint connected components. + state.Connect(0, 2, common::FromUniversal(234567)); + EXPECT_TRUE(state.TransitivelyConnected(0, 2)); + EXPECT_TRUE(state.TransitivelyConnected(1, 3)); + + // All bipartite trajectory pairs between the two connected components should + // have the updated connection time. + EXPECT_EQ(state.LastConnectionTime(0, 2), common::FromUniversal(234567)); + EXPECT_EQ(state.LastConnectionTime(0, 3), common::FromUniversal(234567)); + EXPECT_EQ(state.LastConnectionTime(1, 3), common::FromUniversal(234567)); + + // A pair of trajectory IDs belonging to the same connected component should + // be unaffected. + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456)); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/internal/work_queue.h b/carto_ws/src/cartographer-master/cartographer/mapping/internal/work_queue.h new file mode 100644 index 0000000..84b8863 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/internal/work_queue.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_WORK_QUEUE_H +#define CARTOGRAPHER_MAPPING_INTERNAL_WORK_QUEUE_H + +#include +#include +#include + +namespace cartographer { +namespace mapping { + +struct WorkItem { + enum class Result { + kDoNotRunOptimization, + kRunOptimization, + }; + + std::chrono::steady_clock::time_point time; + std::function task; +}; + +using WorkQueue = std::deque; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_WORK_QUEUE_H diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/map_builder.cc b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder.cc new file mode 100644 index 0000000..ba8a8b7 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder.cc @@ -0,0 +1,405 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/map_builder.h" + +#include "absl/memory/memory.h" +#include "absl/types/optional.h" +#include "cartographer/common/time.h" +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/io/serialization_format_migration.h" +#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" +#include "cartographer/mapping/internal/2d/pose_graph_2d.h" +#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" +#include "cartographer/mapping/internal/collated_trajectory_builder.h" +#include "cartographer/mapping/internal/global_trajectory_builder.h" +#include "cartographer/mapping/internal/motion_filter.h" +#include "cartographer/sensor/internal/collator.h" +#include "cartographer/sensor/internal/trajectory_collator.h" +#include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { +namespace { + +using mapping::proto::SerializedData; + +std::vector SelectRangeSensorIds( + const std::set& expected_sensor_ids) { + std::vector range_sensor_ids; + for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) { + if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) { + range_sensor_ids.push_back(sensor_id.id); + } + } + return range_sensor_ids; +} + +void MaybeAddPureLocalizationTrimmer( + const int trajectory_id, + const proto::TrajectoryBuilderOptions& trajectory_options, + PoseGraph* pose_graph) { + if (trajectory_options.pure_localization()) { + LOG(WARNING) + << "'TrajectoryBuilderOptions::pure_localization' field is deprecated. " + "Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead."; + pose_graph->AddTrimmer(absl::make_unique( + trajectory_id, 3 /* max_submaps_to_keep */)); + return; + } + if (trajectory_options.has_pure_localization_trimmer()) { + pose_graph->AddTrimmer(absl::make_unique( + trajectory_id, + trajectory_options.pure_localization_trimmer().max_submaps_to_keep())); + } +} + +} // namespace + +MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) + : options_(options), thread_pool_(options.num_background_threads()) { + CHECK(options.use_trajectory_builder_2d() ^ + options.use_trajectory_builder_3d()); + if (options.use_trajectory_builder_2d()) { + pose_graph_ = absl::make_unique( + options_.pose_graph_options(), + absl::make_unique( + options_.pose_graph_options().optimization_problem_options()), + &thread_pool_); + } + if (options.use_trajectory_builder_3d()) { + pose_graph_ = absl::make_unique( + options_.pose_graph_options(), + absl::make_unique( + options_.pose_graph_options().optimization_problem_options()), + &thread_pool_); + } + if (options.collate_by_trajectory()) { + sensor_collator_ = absl::make_unique(); + } else { + sensor_collator_ = absl::make_unique(); + } +} + +int MapBuilder::AddTrajectoryBuilder( + const std::set& expected_sensor_ids, + const proto::TrajectoryBuilderOptions& trajectory_options, + LocalSlamResultCallback local_slam_result_callback) { + const int trajectory_id = trajectory_builders_.size(); + + absl::optional pose_graph_odometry_motion_filter; + if (trajectory_options.has_pose_graph_odometry_motion_filter()) { + LOG(INFO) << "Using a motion filter for adding odometry to the pose graph."; + pose_graph_odometry_motion_filter.emplace( + MotionFilter(trajectory_options.pose_graph_odometry_motion_filter())); + } + + if (options_.use_trajectory_builder_3d()) { + std::unique_ptr local_trajectory_builder; + if (trajectory_options.has_trajectory_builder_3d_options()) { + local_trajectory_builder = absl::make_unique( + trajectory_options.trajectory_builder_3d_options(), + SelectRangeSensorIds(expected_sensor_ids)); + } + DCHECK(dynamic_cast(pose_graph_.get())); + trajectory_builders_.push_back(absl::make_unique( + trajectory_options, sensor_collator_.get(), trajectory_id, + expected_sensor_ids, + CreateGlobalTrajectoryBuilder3D( + std::move(local_trajectory_builder), trajectory_id, + static_cast(pose_graph_.get()), + local_slam_result_callback, pose_graph_odometry_motion_filter))); + } else { + std::unique_ptr local_trajectory_builder; + if (trajectory_options.has_trajectory_builder_2d_options()) { + local_trajectory_builder = absl::make_unique( + trajectory_options.trajectory_builder_2d_options(), + SelectRangeSensorIds(expected_sensor_ids)); + } + DCHECK(dynamic_cast(pose_graph_.get())); + trajectory_builders_.push_back(absl::make_unique( + trajectory_options, sensor_collator_.get(), trajectory_id, + expected_sensor_ids, + CreateGlobalTrajectoryBuilder2D( + std::move(local_trajectory_builder), trajectory_id, + static_cast(pose_graph_.get()), + local_slam_result_callback, pose_graph_odometry_motion_filter))); + } + MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, + pose_graph_.get()); + + if (trajectory_options.has_initial_trajectory_pose()) { + const auto& initial_trajectory_pose = + trajectory_options.initial_trajectory_pose(); + pose_graph_->SetInitialTrajectoryPose( + trajectory_id, initial_trajectory_pose.to_trajectory_id(), + transform::ToRigid3(initial_trajectory_pose.relative_pose()), + common::FromUniversal(initial_trajectory_pose.timestamp())); + } + proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto; + for (const auto& sensor_id : expected_sensor_ids) { + *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id); + } + *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = + trajectory_options; + all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size()); + return trajectory_id; +} + +int MapBuilder::AddTrajectoryForDeserialization( + const proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) { + const int trajectory_id = trajectory_builders_.size(); + trajectory_builders_.emplace_back(); + all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size()); + return trajectory_id; +} + +void MapBuilder::FinishTrajectory(const int trajectory_id) { + sensor_collator_->FinishTrajectory(trajectory_id); + pose_graph_->FinishTrajectory(trajectory_id); +} + +std::string MapBuilder::SubmapToProto( + const SubmapId& submap_id, proto::SubmapQuery::Response* const response) { + if (submap_id.trajectory_id < 0 || + submap_id.trajectory_id >= num_trajectory_builders()) { + return "Requested submap from trajectory " + + std::to_string(submap_id.trajectory_id) + " but there are only " + + std::to_string(num_trajectory_builders()) + " trajectories."; + } + + const auto submap_data = pose_graph_->GetSubmapData(submap_id); + if (submap_data.submap == nullptr) { + return "Requested submap " + std::to_string(submap_id.submap_index) + + " from trajectory " + std::to_string(submap_id.trajectory_id) + + " but it does not exist: maybe it has been trimmed."; + } + submap_data.submap->ToResponseProto(submap_data.pose, response); + return ""; +} + +void MapBuilder::SerializeState(bool include_unfinished_submaps, + io::ProtoStreamWriterInterface* const writer) { + io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer, + include_unfinished_submaps); +} + +bool MapBuilder::SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) { + io::ProtoStreamWriter writer(filename); + io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, &writer, + include_unfinished_submaps); + return (writer.Close()); +} + +std::map MapBuilder::LoadState( + io::ProtoStreamReaderInterface* const reader, bool load_frozen_state) { + io::ProtoStreamDeserializer deserializer(reader); + + // Create a copy of the pose_graph_proto, such that we can re-write the + // trajectory ids. + proto::PoseGraph pose_graph_proto = deserializer.pose_graph(); + const auto& all_builder_options_proto = + deserializer.all_trajectory_builder_options(); + + std::map trajectory_remapping; + for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) { + auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i); + const auto& options_with_sensor_ids_proto = + all_builder_options_proto.options_with_sensor_ids(i); + const int new_trajectory_id = + AddTrajectoryForDeserialization(options_with_sensor_ids_proto); + CHECK(trajectory_remapping + .emplace(trajectory_proto.trajectory_id(), new_trajectory_id) + .second) + << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id(); + trajectory_proto.set_trajectory_id(new_trajectory_id); + if (load_frozen_state) { + pose_graph_->FreezeTrajectory(new_trajectory_id); + } + } + + // Apply the calculated remapping to constraints in the pose graph proto. + for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) { + constraint_proto.mutable_submap_id()->set_trajectory_id( + trajectory_remapping.at(constraint_proto.submap_id().trajectory_id())); + constraint_proto.mutable_node_id()->set_trajectory_id( + trajectory_remapping.at(constraint_proto.node_id().trajectory_id())); + } + + MapById submap_poses; + for (const proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const proto::Trajectory::Submap& submap_proto : + trajectory_proto.submap()) { + submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(), + submap_proto.submap_index()}, + transform::ToRigid3(submap_proto.pose())); + } + } + + MapById node_poses; + for (const proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const proto::Trajectory::Node& node_proto : trajectory_proto.node()) { + node_poses.Insert( + NodeId{trajectory_proto.trajectory_id(), node_proto.node_index()}, + transform::ToRigid3(node_proto.pose())); + } + } + + // Set global poses of landmarks. + for (const auto& landmark : pose_graph_proto.landmark_poses()) { + pose_graph_->SetLandmarkPose(landmark.landmark_id(), + transform::ToRigid3(landmark.global_pose()), + true); + } + + if (options_.use_trajectory_builder_3d()) { + CHECK_NE(deserializer.header().format_version(), + io::kFormatVersionWithoutSubmapHistograms) + << "The pbstream file contains submaps without rotational histograms. " + "This can be converted with the 'pbstream migrate' tool, see the " + "Cartographer documentation for details. "; + } + + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + switch (proto.data_case()) { + case SerializedData::kPoseGraph: + LOG(ERROR) << "Found multiple serialized `PoseGraph`. Serialized " + "stream likely corrupt!."; + break; + case SerializedData::kAllTrajectoryBuilderOptions: + LOG(ERROR) << "Found multiple serialized " + "`AllTrajectoryBuilderOptions`. Serialized stream likely " + "corrupt!."; + break; + case SerializedData::kSubmap: { + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + trajectory_remapping.at( + proto.submap().submap_id().trajectory_id())); + const SubmapId submap_id(proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()); + pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id), + proto.submap()); + break; + } + case SerializedData::kNode: { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + trajectory_remapping.at(proto.node().node_id().trajectory_id())); + const NodeId node_id(proto.node().node_id().trajectory_id(), + proto.node().node_id().node_index()); + const transform::Rigid3d& node_pose = node_poses.at(node_id); + pose_graph_->AddNodeFromProto(node_pose, proto.node()); + break; + } + case SerializedData::kTrajectoryData: { + proto.mutable_trajectory_data()->set_trajectory_id( + trajectory_remapping.at(proto.trajectory_data().trajectory_id())); + pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data()); + break; + } + case SerializedData::kImuData: { + if (load_frozen_state) break; + pose_graph_->AddImuData( + trajectory_remapping.at(proto.imu_data().trajectory_id()), + sensor::FromProto(proto.imu_data().imu_data())); + break; + } + case SerializedData::kOdometryData: { + if (load_frozen_state) break; + pose_graph_->AddOdometryData( + trajectory_remapping.at(proto.odometry_data().trajectory_id()), + sensor::FromProto(proto.odometry_data().odometry_data())); + break; + } + case SerializedData::kFixedFramePoseData: { + if (load_frozen_state) break; + pose_graph_->AddFixedFramePoseData( + trajectory_remapping.at( + proto.fixed_frame_pose_data().trajectory_id()), + sensor::FromProto( + proto.fixed_frame_pose_data().fixed_frame_pose_data())); + break; + } + case SerializedData::kLandmarkData: { + if (load_frozen_state) break; + pose_graph_->AddLandmarkData( + trajectory_remapping.at(proto.landmark_data().trajectory_id()), + sensor::FromProto(proto.landmark_data().landmark_data())); + break; + } + default: + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); + } + } + + if (load_frozen_state) { + // Add information about which nodes belong to which submap. + // This is required, even without constraints. + for (const proto::PoseGraph::Constraint& constraint_proto : + pose_graph_proto.constraint()) { + if (constraint_proto.tag() != + proto::PoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + pose_graph_->AddNodeToSubmap( + NodeId{constraint_proto.node_id().trajectory_id(), + constraint_proto.node_id().node_index()}, + SubmapId{constraint_proto.submap_id().trajectory_id(), + constraint_proto.submap_id().submap_index()}); + } + } else { + // When loading unfrozen trajectories, 'AddSerializedConstraints' will + // take care of adding information about which nodes belong to which + // submap. + pose_graph_->AddSerializedConstraints( + FromProto(pose_graph_proto.constraint())); + } + CHECK(reader->eof()); + return trajectory_remapping; +} + +std::map MapBuilder::LoadStateFromFile( + const std::string& state_filename, const bool load_frozen_state) { + const std::string suffix = ".pbstream"; + if (state_filename.substr( + std::max(state_filename.size() - suffix.size(), 0)) != suffix) { + LOG(WARNING) << "The file containing the state should be a " + ".pbstream file."; + } + LOG(INFO) << "Loading saved state '" << state_filename << "'..."; + io::ProtoStreamReader stream(state_filename); + return LoadState(&stream, load_frozen_state); +} + +std::unique_ptr CreateMapBuilder( + const proto::MapBuilderOptions& options) { + return absl::make_unique(options); +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/map_builder.h b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder.h new file mode 100644 index 0000000..ee885a0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder.h @@ -0,0 +1,104 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_MAP_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ + +#include + +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" +#include "cartographer/sensor/collator_interface.h" + +namespace cartographer { +namespace mapping { + +// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) +// and a PoseGraph for loop closure. +class MapBuilder : public MapBuilderInterface { + public: + explicit MapBuilder(const proto::MapBuilderOptions &options); + ~MapBuilder() override {} + + MapBuilder(const MapBuilder &) = delete; + MapBuilder &operator=(const MapBuilder &) = delete; + + int AddTrajectoryBuilder( + const std::set &expected_sensor_ids, + const proto::TrajectoryBuilderOptions &trajectory_options, + LocalSlamResultCallback local_slam_result_callback) override; + + int AddTrajectoryForDeserialization( + const proto::TrajectoryBuilderOptionsWithSensorIds + &options_with_sensor_ids_proto) override; + + void FinishTrajectory(int trajectory_id) override; + + std::string SubmapToProto(const SubmapId &submap_id, + proto::SubmapQuery::Response *response) override; + + void SerializeState(bool include_unfinished_submaps, + io::ProtoStreamWriterInterface *writer) override; + + bool SerializeStateToFile(bool include_unfinished_submaps, + const std::string &filename) override; + + std::map LoadState(io::ProtoStreamReaderInterface *reader, + bool load_frozen_state) override; + + std::map LoadStateFromFile(const std::string &filename, + const bool load_frozen_state) override; + + mapping::PoseGraphInterface *pose_graph() override { + return pose_graph_.get(); + } + + int num_trajectory_builders() const override { + return trajectory_builders_.size(); + } + + mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder( + int trajectory_id) const override { + return trajectory_builders_.at(trajectory_id).get(); + } + + const std::vector + &GetAllTrajectoryBuilderOptions() const override { + return all_trajectory_builder_options_; + } + + private: + const proto::MapBuilderOptions options_; + common::ThreadPool thread_pool_; + + std::unique_ptr pose_graph_; + + std::unique_ptr sensor_collator_; + std::vector> + trajectory_builders_; + std::vector + all_trajectory_builder_options_; +}; + +std::unique_ptr CreateMapBuilder( + const proto::MapBuilderOptions& options); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_interface.cc b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_interface.cc new file mode 100644 index 0000000..af75f6c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_interface.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/map_builder_interface.h" + +#include "cartographer/mapping/pose_graph.h" + +namespace cartographer { +namespace mapping { + +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::MapBuilderOptions options; + options.set_use_trajectory_builder_2d( + parameter_dictionary->GetBool("use_trajectory_builder_2d")); + options.set_use_trajectory_builder_3d( + parameter_dictionary->GetBool("use_trajectory_builder_3d")); + options.set_num_background_threads( + parameter_dictionary->GetNonNegativeInt("num_background_threads")); + options.set_collate_by_trajectory( + parameter_dictionary->GetBool("collate_by_trajectory")); + *options.mutable_pose_graph_options() = CreatePoseGraphOptions( + parameter_dictionary->GetDictionary("pose_graph").get()); + CHECK_NE(options.use_trajectory_builder_2d(), + options.use_trajectory_builder_3d()); + return options; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_interface.h new file mode 100644 index 0000000..76062fb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_interface.h @@ -0,0 +1,120 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_MAP_BUILDER_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_ + +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/io/proto_stream_interface.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_builder_interface.h" + +namespace cartographer { +namespace mapping { + +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +// This interface is used for both library and RPC implementations. +// Implementations wire up the complete SLAM stack. +class MapBuilderInterface { + public: + using LocalSlamResultCallback = + TrajectoryBuilderInterface::LocalSlamResultCallback; + + using SensorId = TrajectoryBuilderInterface::SensorId; + + MapBuilderInterface() {} + virtual ~MapBuilderInterface() {} + + MapBuilderInterface(const MapBuilderInterface&) = delete; + MapBuilderInterface& operator=(const MapBuilderInterface&) = delete; + + // Creates a new trajectory builder and returns its index. + virtual int AddTrajectoryBuilder( + const std::set& expected_sensor_ids, + const proto::TrajectoryBuilderOptions& trajectory_options, + LocalSlamResultCallback local_slam_result_callback) = 0; + + // Creates a new trajectory and returns its index. Querying the trajectory + // builder for it will return 'nullptr'. + virtual int AddTrajectoryForDeserialization( + const proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) = 0; + + // Returns the 'TrajectoryBuilderInterface' corresponding to the specified + // 'trajectory_id' or 'nullptr' if the trajectory has no corresponding + // builder. + virtual mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( + int trajectory_id) const = 0; + + // Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished, + // i.e. no further sensor data is expected. + virtual void FinishTrajectory(int trajectory_id) = 0; + + // Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an + // error string on failure, or an empty string on success. + virtual std::string SubmapToProto(const SubmapId& submap_id, + proto::SubmapQuery::Response* response) = 0; + + // Serializes the current state to a proto stream. If + // 'include_unfinished_submaps' is set to true, unfinished submaps, i.e. + // submaps that have not yet received all rangefinder data insertions, will + // be included in the serialized state. + virtual void SerializeState(bool include_unfinished_submaps, + io::ProtoStreamWriterInterface* writer) = 0; + + // Serializes the current state to a proto stream file on the host system. If + // 'include_unfinished_submaps' is set to true, unfinished submaps, i.e. + // submaps that have not yet received all rangefinder data insertions, will + // be included in the serialized state. + // Returns true if the file was successfully written. + virtual bool SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) = 0; + + // Loads the SLAM state from a proto stream. Returns the remapping of new + // trajectory_ids. + virtual std::map + LoadState(io::ProtoStreamReaderInterface* reader, bool load_frozen_state) = 0; + + // Loads the SLAM state from a pbstream file. Returns the remapping of new + // trajectory_ids. + virtual std::map + LoadStateFromFile(const std::string& filename, bool load_frozen_state) = 0; + + virtual int num_trajectory_builders() const = 0; + + virtual mapping::PoseGraphInterface* pose_graph() = 0; + + virtual const std::vector& + GetAllTrajectoryBuilderOptions() const = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_test.cc new file mode 100644 index 0000000..4f205c1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/map_builder_test.cc @@ -0,0 +1,479 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/map_builder.h" + +#include "cartographer/common/config.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; + +const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"}; +const SensorId kIMUSensorId{SensorId::SensorType::IMU, "imu"}; +constexpr double kDuration = 4.; // Seconds. +constexpr double kTimeStep = 0.1; // Seconds. +constexpr double kTravelDistance = 1.2; // Meters. + +template +class MapBuilderTestBase : public T { + protected: + void SetUp() override { + // Global SLAM optimization is not executed. + const std::string kMapBuilderLua = R"text( + include "map_builder.lua" + MAP_BUILDER.use_trajectory_builder_2d = true + MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0 + MAP_BUILDER.pose_graph.global_sampling_ratio = 0.05 + MAP_BUILDER.pose_graph.global_constraint_search_after_n_seconds = 0 + return MAP_BUILDER)text"; + auto map_builder_parameters = testing::ResolveLuaParameters(kMapBuilderLua); + map_builder_options_ = + CreateMapBuilderOptions(map_builder_parameters.get()); + // Multiple submaps are created because of a small 'num_range_data'. + const std::string kTrajectoryBuilderLua = R"text( + include "trajectory_builder.lua" + TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false + TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 4 + TRAJECTORY_BUILDER.trajectory_builder_3d.submaps.num_range_data = 4 + return TRAJECTORY_BUILDER)text"; + auto trajectory_builder_parameters = + testing::ResolveLuaParameters(kTrajectoryBuilderLua); + trajectory_builder_options_ = + CreateTrajectoryBuilderOptions(trajectory_builder_parameters.get()); + } + + void BuildMapBuilder() { + map_builder_ = CreateMapBuilder(map_builder_options_); + } + + void SetOptionsTo3D() { + map_builder_options_.set_use_trajectory_builder_2d(false); + map_builder_options_.set_use_trajectory_builder_3d(true); + } + + void SetOptionsToTSDF2D() { + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_range_data_inserter_options() + ->set_range_data_inserter_type( + proto::RangeDataInserterOptions::TSDF_INSERTER_2D); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_grid_options_2d() + ->set_grid_type(proto::GridOptions2D::TSDF); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(10.0); + map_builder_options_.mutable_pose_graph_options() + ->mutable_constraint_builder_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(50.0); + } + + void SetOptionsEnableGlobalOptimization() { + map_builder_options_.mutable_pose_graph_options() + ->set_optimize_every_n_nodes(3); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_motion_filter_options() + ->set_max_distance_meters(0); + } + + MapBuilderInterface::LocalSlamResultCallback GetLocalSlamResultCallback() { + return [=](const int trajectory_id, const ::cartographer::common::Time time, + const ::cartographer::transform::Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local, + const std::unique_ptr< + const cartographer::mapping::TrajectoryBuilderInterface:: + InsertionResult>) { + local_slam_result_poses_.push_back(local_pose); + }; + } + + int CreateTrajectoryWithFakeData(double timestamps_add_duration = 0.) { + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (auto& measurement : measurements) { + measurement.time += common::FromSeconds(timestamps_add_duration); + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + } + map_builder_->FinishTrajectory(trajectory_id); + return trajectory_id; + } + + std::unique_ptr map_builder_; + proto::MapBuilderOptions map_builder_options_; + proto::TrajectoryBuilderOptions trajectory_builder_options_; + std::vector<::cartographer::transform::Rigid3d> local_slam_result_poses_; +}; + +class MapBuilderTest : public MapBuilderTestBase<::testing::Test> {}; +class MapBuilderTestByGridType + : public MapBuilderTestBase<::testing::TestWithParam> {}; +class MapBuilderTestByGridTypeAndDimensions + : public MapBuilderTestBase< + ::testing::TestWithParam>> { +}; +INSTANTIATE_TEST_CASE_P(MapBuilderTestByGridType, MapBuilderTestByGridType, + ::testing::Values(GridType::PROBABILITY_GRID, + GridType::TSDF)); +INSTANTIATE_TEST_CASE_P( + MapBuilderTestByGridTypeAndDimensions, + MapBuilderTestByGridTypeAndDimensions, + ::testing::Values(std::make_pair(GridType::PROBABILITY_GRID, 2), + std::make_pair(GridType::PROBABILITY_GRID, 3), + std::make_pair(GridType::TSDF, 2))); + +TEST_P(MapBuilderTestByGridTypeAndDimensions, TrajectoryAddFinish) { + if (GetParam().second == 3) SetOptionsTo3D(); + if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D(); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + nullptr /* GetLocalSlamResultCallbackForSubscriptions */); + EXPECT_EQ(1, map_builder_->num_trajectory_builders()); + EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr); + EXPECT_TRUE(map_builder_->pose_graph() != nullptr); + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id)); +} + +TEST_P(MapBuilderTestByGridType, LocalSlam2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); +} + +TEST_F(MapBuilderTest, LocalSlam3D) { + SetOptionsTo3D(); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + trajectory_builder->AddSensorData( + kIMUSensorId.id, + sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); +} + +TEST_P(MapBuilderTestByGridType, GlobalSlam2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); + SetOptionsEnableGlobalOptimization(); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50); + EXPECT_THAT(map_builder_->pose_graph()->constraints(), + ::testing::Contains(::testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))); + const auto trajectory_nodes = + map_builder_->pose_graph()->GetTrajectoryNodes(); + EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 20); + const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData(); + EXPECT_GE(submap_data.SizeOfTrajectoryOrZero(trajectory_id), 5); + const transform::Rigid3d final_pose = + map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) * + local_slam_result_poses_.back(); + EXPECT_NEAR(kTravelDistance, final_pose.translation().norm(), + 0.1 * kTravelDistance); +} + +TEST_F(MapBuilderTest, GlobalSlam3D) { + SetOptionsTo3D(); + SetOptionsEnableGlobalOptimization(); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + trajectory_builder->AddSensorData( + kIMUSensorId.id, + sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 10); + EXPECT_THAT(map_builder_->pose_graph()->constraints(), + ::testing::Contains(::testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))); + const auto trajectory_nodes = + map_builder_->pose_graph()->GetTrajectoryNodes(); + EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 5); + const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData(); + EXPECT_GE(submap_data.SizeOfTrajectoryOrZero(trajectory_id), 2); + const transform::Rigid3d final_pose = + map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) * + local_slam_result_poses_.back(); + EXPECT_NEAR(kTravelDistance, final_pose.translation().norm(), + 0.1 * kTravelDistance); +} + +TEST_P(MapBuilderTestByGridType, DeleteFinishedTrajectory2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); + SetOptionsEnableGlobalOptimization(); + BuildMapBuilder(); + int trajectory_id = CreateTrajectoryWithFakeData(); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id)); + EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50); + EXPECT_GE( + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + trajectory_id), + 20); + EXPECT_GE( + map_builder_->pose_graph()->GetAllSubmapData().SizeOfTrajectoryOrZero( + trajectory_id), + 5); + map_builder_->pose_graph()->DeleteTrajectory(trajectory_id); + int another_trajectory_id = CreateTrajectoryWithFakeData(100.); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_TRUE( + map_builder_->pose_graph()->IsTrajectoryFinished(another_trajectory_id)); + EXPECT_EQ( + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + trajectory_id), + 0); + EXPECT_EQ( + map_builder_->pose_graph()->GetAllSubmapData().SizeOfTrajectoryOrZero( + trajectory_id), + 0); + map_builder_->pose_graph()->DeleteTrajectory(another_trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(map_builder_->pose_graph()->constraints().size(), 0); + EXPECT_EQ( + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + another_trajectory_id), + 0); + EXPECT_EQ( + map_builder_->pose_graph()->GetAllSubmapData().SizeOfTrajectoryOrZero( + another_trajectory_id), + 0); +} + +TEST_P(MapBuilderTestByGridTypeAndDimensions, SaveLoadState) { + if (GetParam().second == 3) SetOptionsTo3D(); + if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D(); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->set_use_imu_data(true); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + trajectory_builder->AddSensorData( + kIMUSensorId.id, + sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + int num_constraints = map_builder_->pose_graph()->constraints().size(); + int num_nodes = + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + trajectory_id); + EXPECT_GT(num_constraints, 0); + EXPECT_GT(num_nodes, 0); + // TODO(gaschler): Consider using in-memory to avoid side effects. + const std::string filename = "temp-SaveLoadState.pbstream"; + io::ProtoStreamWriter writer(filename); + map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer); + writer.Close(); + + // Reset 'map_builder_'. + BuildMapBuilder(); + io::ProtoStreamReader reader(filename); + auto trajectory_remapping = + map_builder_->LoadState(&reader, false /* load_frozen_state */); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(num_constraints, map_builder_->pose_graph()->constraints().size()); + ASSERT_EQ(trajectory_remapping.size(), 1); + int new_trajectory_id = trajectory_remapping.begin()->second; + EXPECT_EQ( + num_nodes, + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + new_trajectory_id)); +} + +TEST_P(MapBuilderTestByGridType, LocalizationOnFrozenTrajectory2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); + BuildMapBuilder(); + int temp_trajectory_id = CreateTrajectoryWithFakeData(); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_GT(map_builder_->pose_graph()->constraints().size(), 0); + EXPECT_GT( + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + temp_trajectory_id), + 0); + const std::string filename = "temp-LocalizationOnFrozenTrajectory2D.pbstream"; + io::ProtoStreamWriter writer(filename); + map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer); + writer.Close(); + + // Reset 'map_builder_'. + local_slam_result_poses_.clear(); + SetOptionsEnableGlobalOptimization(); + BuildMapBuilder(); + io::ProtoStreamReader reader(filename); + map_builder_->LoadState(&reader, true /* load_frozen_state */); + map_builder_->pose_graph()->RunFinalOptimization(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + transform::Rigid3d frozen_trajectory_to_global( + Eigen::Vector3d(0.5, 0.4, 0), + Eigen::Quaterniond(Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitZ()))); + Eigen::Vector3d travel_translation = + Eigen::Vector3d(2., 1., 0.).normalized() * kTravelDistance; + auto measurements = testing::GenerateFakeRangeMeasurements( + travel_translation.cast(), kDuration, kTimeStep, + frozen_trajectory_to_global.cast()); + for (auto& measurement : measurements) { + measurement.time += common::FromSeconds(100.); + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.15 * kTravelDistance); + EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50); + auto constraints = map_builder_->pose_graph()->constraints(); + int num_cross_trajectory_constraints = 0; + for (const auto& constraint : constraints) { + if (constraint.node_id.trajectory_id != + constraint.submap_id.trajectory_id) { + ++num_cross_trajectory_constraints; + } + } + EXPECT_GE(num_cross_trajectory_constraints, 3); + // TODO(gaschler): Subscribe global slam callback, verify that all nodes are + // optimized. + EXPECT_THAT(constraints, ::testing::Contains(::testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))); + const auto trajectory_nodes = + map_builder_->pose_graph()->GetTrajectoryNodes(); + EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 20); + const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData(); + EXPECT_GE(submap_data.SizeOfTrajectoryOrZero(trajectory_id), 5); + const transform::Rigid3d global_pose = + map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) * + local_slam_result_poses_.back(); + EXPECT_NEAR(frozen_trajectory_to_global.translation().norm(), + map_builder_->pose_graph() + ->GetLocalToGlobalTransform(trajectory_id) + .translation() + .norm(), + 0.1); + const transform::Rigid3d expected_global_pose = + frozen_trajectory_to_global * + transform::Rigid3d::Translation(travel_translation); + EXPECT_NEAR( + 0., + (global_pose.translation() - expected_global_pose.translation()).norm(), + 0.3) + << "global_pose: " << global_pose + << "expected_global_pose: " << expected_global_pose; +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator.cc new file mode 100644 index 0000000..5bed706 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator.cc @@ -0,0 +1,263 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/pose_extrapolator.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration, + double imu_gravity_time_constant) + : pose_queue_duration_(pose_queue_duration), + gravity_time_constant_(imu_gravity_time_constant), + cached_extrapolated_pose_{common::Time::min(), + transform::Rigid3d::Identity()} {} + +std::unique_ptr PoseExtrapolator::InitializeWithImu( + const common::Duration pose_queue_duration, + const double imu_gravity_time_constant, const sensor::ImuData& imu_data) { + auto extrapolator = absl::make_unique( + pose_queue_duration, imu_gravity_time_constant); + extrapolator->AddImuData(imu_data); + extrapolator->imu_tracker_ = + absl::make_unique(imu_gravity_time_constant, imu_data.time); + extrapolator->imu_tracker_->AddImuLinearAccelerationObservation( + imu_data.linear_acceleration); + extrapolator->imu_tracker_->AddImuAngularVelocityObservation( + imu_data.angular_velocity); + extrapolator->imu_tracker_->Advance(imu_data.time); + extrapolator->AddPose( + imu_data.time, + transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation())); + return extrapolator; +} + +common::Time PoseExtrapolator::GetLastPoseTime() const { + if (timed_pose_queue_.empty()) { + return common::Time::min(); + } + return timed_pose_queue_.back().time; +} + +common::Time PoseExtrapolator::GetLastExtrapolatedTime() const { + if (!extrapolation_imu_tracker_) { + return common::Time::min(); + } + return extrapolation_imu_tracker_->time(); +} + +void PoseExtrapolator::AddPose(const common::Time time, + const transform::Rigid3d& pose) { + if (imu_tracker_ == nullptr) { + common::Time tracker_start = time; + if (!imu_data_.empty()) { + tracker_start = std::min(tracker_start, imu_data_.front().time); + } + imu_tracker_ = + absl::make_unique(gravity_time_constant_, tracker_start); + } + timed_pose_queue_.push_back(TimedPose{time, pose}); + while (timed_pose_queue_.size() > 2 && + timed_pose_queue_[1].time <= time - pose_queue_duration_) { + timed_pose_queue_.pop_front(); + } + UpdateVelocitiesFromPoses(); + AdvanceImuTracker(time, imu_tracker_.get()); + TrimImuData(); + TrimOdometryData(); + odometry_imu_tracker_ = absl::make_unique(*imu_tracker_); + extrapolation_imu_tracker_ = absl::make_unique(*imu_tracker_); +} + +void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) { + CHECK(timed_pose_queue_.empty() || + imu_data.time >= timed_pose_queue_.back().time); + imu_data_.push_back(imu_data); + TrimImuData(); +} + +void PoseExtrapolator::AddOdometryData( + const sensor::OdometryData& odometry_data) { + CHECK(timed_pose_queue_.empty() || + odometry_data.time >= timed_pose_queue_.back().time); + odometry_data_.push_back(odometry_data); + TrimOdometryData(); + if (odometry_data_.size() < 2) { + return; + } + // TODO(whess): Improve by using more than just the last two odometry poses. + // Compute extrapolation in the tracking frame. + const sensor::OdometryData& odometry_data_oldest = odometry_data_.front(); + const sensor::OdometryData& odometry_data_newest = odometry_data_.back(); + const double odometry_time_delta = + common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time); + const transform::Rigid3d odometry_pose_delta = + odometry_data_newest.pose.inverse() * odometry_data_oldest.pose; + angular_velocity_from_odometry_ = + transform::RotationQuaternionToAngleAxisVector( + odometry_pose_delta.rotation()) / + odometry_time_delta; + if (timed_pose_queue_.empty()) { + return; + } + const Eigen::Vector3d + linear_velocity_in_tracking_frame_at_newest_odometry_time = + odometry_pose_delta.translation() / odometry_time_delta; + const Eigen::Quaterniond orientation_at_newest_odometry_time = + timed_pose_queue_.back().pose.rotation() * + ExtrapolateRotation(odometry_data_newest.time, + odometry_imu_tracker_.get()); + linear_velocity_from_odometry_ = + orientation_at_newest_odometry_time * + linear_velocity_in_tracking_frame_at_newest_odometry_time; +} + +transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) { + const TimedPose& newest_timed_pose = timed_pose_queue_.back(); + CHECK_GE(time, newest_timed_pose.time); + if (cached_extrapolated_pose_.time != time) { + const Eigen::Vector3d translation = + ExtrapolateTranslation(time) + newest_timed_pose.pose.translation(); + + const Eigen::Quaterniond rotation = + newest_timed_pose.pose.rotation() * + ExtrapolateRotation(time, extrapolation_imu_tracker_.get()); + cached_extrapolated_pose_ = + TimedPose{time, transform::Rigid3d{translation, rotation}}; + } + return cached_extrapolated_pose_.pose; +} + +Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation( + const common::Time time) { + ImuTracker imu_tracker = *imu_tracker_; + AdvanceImuTracker(time, &imu_tracker); + return imu_tracker.orientation(); +} + +void PoseExtrapolator::UpdateVelocitiesFromPoses() { + if (timed_pose_queue_.size() < 2) { + // We need two poses to estimate velocities. + return; + } + CHECK(!timed_pose_queue_.empty()); + const TimedPose& newest_timed_pose = timed_pose_queue_.back(); + const auto newest_time = newest_timed_pose.time; + const TimedPose& oldest_timed_pose = timed_pose_queue_.front(); + const auto oldest_time = oldest_timed_pose.time; + const double queue_delta = common::ToSeconds(newest_time - oldest_time); + if (queue_delta < common::ToSeconds(pose_queue_duration_)) { + LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: " + << queue_delta << " s"; + return; + } + const transform::Rigid3d& newest_pose = newest_timed_pose.pose; + const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose; + linear_velocity_from_poses_ = + (newest_pose.translation() - oldest_pose.translation()) / queue_delta; + angular_velocity_from_poses_ = + transform::RotationQuaternionToAngleAxisVector( + oldest_pose.rotation().inverse() * newest_pose.rotation()) / + queue_delta; +} + +void PoseExtrapolator::TrimImuData() { + while (imu_data_.size() > 1 && !timed_pose_queue_.empty() && + imu_data_[1].time <= timed_pose_queue_.back().time) { + imu_data_.pop_front(); + } +} + +void PoseExtrapolator::TrimOdometryData() { + while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() && + odometry_data_[1].time <= timed_pose_queue_.back().time) { + odometry_data_.pop_front(); + } +} + +void PoseExtrapolator::AdvanceImuTracker(const common::Time time, + ImuTracker* const imu_tracker) const { + CHECK_GE(time, imu_tracker->time()); + if (imu_data_.empty() || time < imu_data_.front().time) { + // There is no IMU data until 'time', so we advance the ImuTracker and use + // the angular velocities from poses and fake gravity to help 2D stability. + imu_tracker->Advance(time); + imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ()); + imu_tracker->AddImuAngularVelocityObservation( + odometry_data_.size() < 2 ? angular_velocity_from_poses_ + : angular_velocity_from_odometry_); + return; + } + if (imu_tracker->time() < imu_data_.front().time) { + // Advance to the beginning of 'imu_data_'. + imu_tracker->Advance(imu_data_.front().time); + } + auto it = std::lower_bound( + imu_data_.begin(), imu_data_.end(), imu_tracker->time(), + [](const sensor::ImuData& imu_data, const common::Time& time) { + return imu_data.time < time; + }); + while (it != imu_data_.end() && it->time < time) { + imu_tracker->Advance(it->time); + imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration); + imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity); + ++it; + } + imu_tracker->Advance(time); +} + +Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation( + const common::Time time, ImuTracker* const imu_tracker) const { + CHECK_GE(time, imu_tracker->time()); + AdvanceImuTracker(time, imu_tracker); + const Eigen::Quaterniond last_orientation = imu_tracker_->orientation(); + return last_orientation.inverse() * imu_tracker->orientation(); +} + +Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { + const TimedPose& newest_timed_pose = timed_pose_queue_.back(); + const double extrapolation_delta = + common::ToSeconds(time - newest_timed_pose.time); + if (odometry_data_.size() < 2) { + return extrapolation_delta * linear_velocity_from_poses_; + } + return extrapolation_delta * linear_velocity_from_odometry_; +} + +PoseExtrapolator::ExtrapolationResult +PoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + std::vector poses; + for (auto it = times.begin(); it != std::prev(times.end()); ++it) { + poses.push_back(ExtrapolatePose(*it).cast()); + } + + const Eigen::Vector3d current_velocity = odometry_data_.size() < 2 + ? linear_velocity_from_poses_ + : linear_velocity_from_odometry_; + return ExtrapolationResult{poses, ExtrapolatePose(times.back()), + current_velocity, + EstimateGravityOrientation(times.back())}; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator.h b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator.h new file mode 100644 index 0000000..1c6885d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator.h @@ -0,0 +1,98 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_POSE_EXTRAPOLATOR_H_ +#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/imu_tracker.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { + +// Keep poses for a certain duration to estimate linear and angular velocity. +// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if +// available to improve the extrapolation. +class PoseExtrapolator : public PoseExtrapolatorInterface { + public: + explicit PoseExtrapolator(common::Duration pose_queue_duration, + double imu_gravity_time_constant); + + PoseExtrapolator(const PoseExtrapolator&) = delete; + PoseExtrapolator& operator=(const PoseExtrapolator&) = delete; + + static std::unique_ptr InitializeWithImu( + common::Duration pose_queue_duration, double imu_gravity_time_constant, + const sensor::ImuData& imu_data); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; + + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; + + // Returns the current gravity alignment estimate as a rotation from + // the tracking frame into a gravity aligned frame. + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; + + private: + void UpdateVelocitiesFromPoses(); + void TrimImuData(); + void TrimOdometryData(); + void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const; + Eigen::Quaterniond ExtrapolateRotation(common::Time time, + ImuTracker* imu_tracker) const; + Eigen::Vector3d ExtrapolateTranslation(common::Time time); + + const common::Duration pose_queue_duration_; + struct TimedPose { + common::Time time; + transform::Rigid3d pose; + }; + std::deque timed_pose_queue_; + Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero(); + + const double gravity_time_constant_; + std::deque imu_data_; + std::unique_ptr imu_tracker_; + std::unique_ptr odometry_imu_tracker_; + std::unique_ptr extrapolation_imu_tracker_; + TimedPose cached_extrapolated_pose_; + + std::deque odometry_data_; + Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero(); +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_interface.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_interface.cc new file mode 100644 index 0000000..e408d3a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_interface.cc @@ -0,0 +1,99 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_extrapolator_interface.h" + +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" +#include "cartographer/mapping/pose_extrapolator.h" + +namespace cartographer { +namespace mapping { + +namespace { + +proto::ConstantVelocityPoseExtrapolatorOptions +CreateConstantVelocityPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ConstantVelocityPoseExtrapolatorOptions options; + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + return options; +} + +proto::ImuBasedPoseExtrapolatorOptions CreateImuBasedPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ImuBasedPoseExtrapolatorOptions options; + + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_gravity_constant( + parameter_dictionary->GetDouble("gravity_constant")); + options.set_pose_translation_weight( + parameter_dictionary->GetDouble("pose_translation_weight")); + options.set_pose_rotation_weight( + parameter_dictionary->GetDouble("pose_rotation_weight")); + options.set_imu_acceleration_weight( + parameter_dictionary->GetDouble("imu_acceleration_weight")); + options.set_imu_rotation_weight( + parameter_dictionary->GetDouble("imu_rotation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + *options.mutable_solver_options() = CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("solver_options").get()); + + return options; +} + +} // namespace + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::PoseExtrapolatorOptions options; + options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based")); + *options.mutable_constant_velocity() = + CreateConstantVelocityPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("constant_velocity").get()); + *options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("imu_based").get()); + + return options; +} + +std::unique_ptr +PoseExtrapolatorInterface::CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses) { + CHECK(!imu_data.empty()); + if (options.use_imu_based()) { + return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(), + imu_data, initial_poses); + } else { + return PoseExtrapolator::InitializeWithImu( + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), + imu_data.back()); + } +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_interface.h new file mode 100644 index 0000000..bf258ce --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_interface.h @@ -0,0 +1,82 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/pose_extrapolator_options.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class PoseExtrapolatorInterface { + public: + struct ExtrapolationResult { + // The poses for the requested times at index 0 to N-1. + std::vector previous_poses; + // The pose for the requested time at index N. + transform::Rigid3d current_pose; + Eigen::Vector3d current_velocity; + Eigen::Quaterniond gravity_from_tracking; + }; + + PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; + PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = + delete; + virtual ~PoseExtrapolatorInterface() {} + + // TODO: Remove dependency cycle. + static std::unique_ptr CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + virtual common::Time GetLastPoseTime() const = 0; + virtual common::Time GetLastExtrapolatedTime() const = 0; + + virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0; + virtual void AddImuData(const sensor::ImuData& imu_data) = 0; + virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0; + virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0; + + virtual ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) = 0; + + // Returns the current gravity alignment estimate as a rotation from + // the tracking frame into a gravity aligned frame. + virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0; + + protected: + PoseExtrapolatorInterface() {} +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_test.cc new file mode 100644 index 0000000..00ef86f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_extrapolator_test.cc @@ -0,0 +1,181 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_extrapolator.h" + +#include "Eigen/Geometry" +#include "absl/memory/memory.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +constexpr double kPoseQueueDuration = 0.5f; +constexpr double kGravityTimeConstant = 0.1f; +constexpr double kExtrapolateDuration = 0.1f; +constexpr double kPrecision = 1e-8; +constexpr double kExtrapolatePrecision = 1e-2; + +TEST(PoseExtrapolatorDeathTest, IncompleteInitialization) { + PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration), + kGravityTimeConstant); + common::Time time = common::FromUniversal(123); + EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), ""); + EXPECT_DEATH(extrapolator.ExtrapolatePose(time), ""); + Eigen::Vector3d acceleration(0, 0, 9); + Eigen::Vector3d angular_velocity(0, 0, 0); + extrapolator.AddImuData( + sensor::ImuData{time, acceleration, angular_velocity}); + EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), ""); + EXPECT_DEATH(extrapolator.ExtrapolatePose(time), ""); +} + +TEST(PoseExtrapolatorDeathTest, ExtrapolateInPast) { + PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration), + kGravityTimeConstant); + common::Time time_present = common::FromUniversal(123); + transform::Rigid3d pose = + transform::Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.3)); + extrapolator.AddPose(time_present, pose); + extrapolator.EstimateGravityOrientation(time_present); + EXPECT_THAT(extrapolator.ExtrapolatePose(time_present), + transform::IsNearly(pose, kPrecision)); + common::Time time_in_past = time_present - common::FromSeconds(10); + EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time_in_past), ""); + EXPECT_DEATH(extrapolator.ExtrapolatePose(time_in_past), ""); + common::Time time_in_future = time_present + common::FromSeconds(20); + extrapolator.ExtrapolatePose(time_in_future); + EXPECT_DEATH(extrapolator.ExtrapolatePose(time_present), ""); +} + +TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { + Eigen::Vector3d initial_gravity_acceleration(1.6, 2.0, 8.0); + Eigen::Vector3d angular_velocity(0, 0, 0); + common::Time current_time = common::FromUniversal(123); + sensor::ImuData imu_data{current_time, initial_gravity_acceleration, + angular_velocity}; + auto extrapolator = PoseExtrapolator::InitializeWithImu( + common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data); + Eigen::Quaterniond expected_orientation = + FromTwoVectors(initial_gravity_acceleration, Eigen::Vector3d::UnitZ()); + EXPECT_NEAR(0., + extrapolator->EstimateGravityOrientation(current_time) + .angularDistance(expected_orientation), + kPrecision); + Eigen::Vector3d gravity_acceleration(1.6, 2.0, 8.0); + for (int i = 0; i < 10; ++i) { + current_time += common::FromSeconds(kGravityTimeConstant); + extrapolator->AddImuData( + sensor::ImuData{current_time, gravity_acceleration, angular_velocity}); + } + expected_orientation = + FromTwoVectors(gravity_acceleration, Eigen::Vector3d::UnitZ()); + EXPECT_NEAR(0., + extrapolator->EstimateGravityOrientation(current_time) + .angularDistance(expected_orientation), + kPrecision); +} + +TEST(PoseExtrapolatorTest, ExtrapolateWithPoses) { + PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration), + kGravityTimeConstant); + common::Time current_time = common::FromUniversal(123); + transform::Rigid3d current_pose = + transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2)); + Eigen::Vector3d velocity(0, 0.1, 0); + Eigen::Vector3d angular_velocity(0.01, 0, 0.1); + transform::Rigid3d motion_per_second( + velocity, Eigen::AngleAxisd(angular_velocity.norm(), + angular_velocity.normalized())); + extrapolator.AddPose(current_time, current_pose); + EXPECT_EQ(common::ToUniversal(extrapolator.GetLastPoseTime()), + common::ToUniversal(current_time)); + EXPECT_THAT(extrapolator.ExtrapolatePose(current_time), + transform::IsNearly(current_pose, kPrecision)); + for (int i = 0; i < 5; ++i) { + current_time += common::FromSeconds(1); + current_pose = current_pose * motion_per_second; + extrapolator.AddPose(current_time, current_pose); + EXPECT_THAT(extrapolator.ExtrapolatePose(current_time), + transform::IsNearly(current_pose, kPrecision)); + transform::Rigid3d expected_pose = + current_pose * + transform::Rigid3d( + kExtrapolateDuration * velocity, + Eigen::AngleAxisd(kExtrapolateDuration * angular_velocity.norm(), + angular_velocity.normalized())); + EXPECT_THAT(extrapolator.ExtrapolatePose( + current_time + common::FromSeconds(kExtrapolateDuration)), + transform::IsNearly(expected_pose, kExtrapolatePrecision)); + EXPECT_EQ(common::ToUniversal(extrapolator.GetLastExtrapolatedTime()), + common::ToUniversal(current_time + + common::FromSeconds(kExtrapolateDuration))); + } + Eigen::AngleAxisd gravity_axis( + extrapolator.EstimateGravityOrientation(current_time)); + EXPECT_NEAR( + 0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(), + kExtrapolatePrecision); +} + +TEST(PoseExtrapolatorTest, ExtrapolateWithIMU) { + Eigen::Vector3d initial_gravity_acceleration(0, 0, 9.8); + Eigen::Vector3d initial_angular_velocity(0, 0, 0); + common::Time current_time = common::FromUniversal(123); + sensor::ImuData imu_data{current_time, initial_gravity_acceleration, + initial_angular_velocity}; + auto extrapolator = PoseExtrapolator::InitializeWithImu( + common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data); + transform::Rigid3d current_pose = + transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2)); + // Let velocity estimation come to rest. + for (int i = 0; i < kPoseQueueDuration + 2; ++i) { + current_time += common::FromSeconds(1); + extrapolator->AddImuData(sensor::ImuData{ + current_time, initial_gravity_acceleration, initial_angular_velocity}); + extrapolator->AddPose(current_time, current_pose); + } + + Eigen::Vector3d angular_velocity_yaw(0, 0, 0.1); + transform::Rigid3d expected_pose = current_pose; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 10; ++j) { + current_time += common::FromSeconds(kGravityTimeConstant); + extrapolator->AddImuData(sensor::ImuData{ + current_time, initial_gravity_acceleration, angular_velocity_yaw}); + Eigen::Quaterniond expected_rotation( + Eigen::AngleAxisd(kGravityTimeConstant * angular_velocity_yaw.norm(), + angular_velocity_yaw.normalized())); + expected_pose = + expected_pose * transform::Rigid3d::Rotation(expected_rotation); + } + EXPECT_THAT(extrapolator->ExtrapolatePose(current_time), + transform::IsNearly(expected_pose, kExtrapolatePrecision)); + extrapolator->AddPose(current_time, expected_pose); + } + Eigen::AngleAxisd gravity_axis( + extrapolator->EstimateGravityOrientation(current_time)); + EXPECT_NEAR( + 0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(), + kPrecision); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph.cc new file mode 100644 index 0000000..097e8db --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph.cc @@ -0,0 +1,213 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/pose_graph.h" + +#include "cartographer/mapping/internal/constraints/constraint_builder.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_options.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +proto::PoseGraph::Constraint::Tag ToProto( + const PoseGraph::Constraint::Tag& tag) { + switch (tag) { + case PoseGraph::Constraint::Tag::INTRA_SUBMAP: + return proto::PoseGraph::Constraint::INTRA_SUBMAP; + case PoseGraph::Constraint::Tag::INTER_SUBMAP: + return proto::PoseGraph::Constraint::INTER_SUBMAP; + } + LOG(FATAL) << "Unsupported tag."; +} + +PoseGraph::Constraint::Tag FromProto( + const proto::PoseGraph::Constraint::Tag& proto) { + switch (proto) { + case proto::PoseGraph::Constraint::INTRA_SUBMAP: + return PoseGraph::Constraint::Tag::INTRA_SUBMAP; + case proto::PoseGraph::Constraint::INTER_SUBMAP: + return PoseGraph::Constraint::Tag::INTER_SUBMAP; + case ::google::protobuf::kint32max: + case ::google::protobuf::kint32min: { + } + } + LOG(FATAL) << "Unsupported tag."; +} + +std::vector FromProto( + const ::google::protobuf::RepeatedPtrField& + constraint_protos) { + std::vector constraints; + for (const auto& constraint_proto : constraint_protos) { + const mapping::SubmapId submap_id{ + constraint_proto.submap_id().trajectory_id(), + constraint_proto.submap_id().submap_index()}; + const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(), + constraint_proto.node_id().node_index()}; + const PoseGraph::Constraint::Pose pose{ + transform::ToRigid3(constraint_proto.relative_pose()), + constraint_proto.translation_weight(), + constraint_proto.rotation_weight()}; + const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag()); + constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag}); + } + return constraints; +} + +void PopulateOverlappingSubmapsTrimmerOptions2D( + proto::PoseGraphOptions* const pose_graph_options, + common::LuaParameterDictionary* const parameter_dictionary) { + constexpr char kDictionaryKey[] = "overlapping_submaps_trimmer_2d"; + if (!parameter_dictionary->HasKey(kDictionaryKey)) return; + + auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey); + auto* options = pose_graph_options->mutable_overlapping_submaps_trimmer_2d(); + options->set_fresh_submaps_count( + options_dictionary->GetInt("fresh_submaps_count")); + options->set_min_covered_area( + options_dictionary->GetDouble("min_covered_area")); + options->set_min_added_submaps_count( + options_dictionary->GetInt("min_added_submaps_count")); +} + +proto::PoseGraphOptions CreatePoseGraphOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::PoseGraphOptions options; + options.set_optimize_every_n_nodes( + parameter_dictionary->GetInt("optimize_every_n_nodes")); + *options.mutable_constraint_builder_options() = + constraints::CreateConstraintBuilderOptions( + parameter_dictionary->GetDictionary("constraint_builder").get()); + options.set_matcher_translation_weight( + parameter_dictionary->GetDouble("matcher_translation_weight")); + options.set_matcher_rotation_weight( + parameter_dictionary->GetDouble("matcher_rotation_weight")); + *options.mutable_optimization_problem_options() = + optimization::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")); + options.set_log_residual_histograms( + parameter_dictionary->GetBool("log_residual_histograms")); + options.set_global_constraint_search_after_n_seconds( + parameter_dictionary->GetDouble( + "global_constraint_search_after_n_seconds")); + PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary); + return options; +} + +proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) { + proto::PoseGraph::Constraint constraint_proto; + *constraint_proto.mutable_relative_pose() = + transform::ToProto(constraint.pose.zbar_ij); + constraint_proto.set_translation_weight(constraint.pose.translation_weight); + constraint_proto.set_rotation_weight(constraint.pose.rotation_weight); + constraint_proto.mutable_submap_id()->set_trajectory_id( + constraint.submap_id.trajectory_id); + constraint_proto.mutable_submap_id()->set_submap_index( + constraint.submap_id.submap_index); + constraint_proto.mutable_node_id()->set_trajectory_id( + constraint.node_id.trajectory_id); + constraint_proto.mutable_node_id()->set_node_index( + constraint.node_id.node_index); + constraint_proto.set_tag(mapping::ToProto(constraint.tag)); + return constraint_proto; +} + +proto::PoseGraph PoseGraph::ToProto(bool include_unfinished_submaps) const { + proto::PoseGraph proto; + + std::map trajectory_protos; + const auto trajectory = [&proto, &trajectory_protos]( + const int trajectory_id) -> proto::Trajectory* { + if (trajectory_protos.count(trajectory_id) == 0) { + auto* const trajectory_proto = proto.add_trajectory(); + trajectory_proto->set_trajectory_id(trajectory_id); + CHECK(trajectory_protos.emplace(trajectory_id, trajectory_proto).second); + } + return trajectory_protos.at(trajectory_id); + }; + + std::set unfinished_submaps; + for (const auto& submap_id_data : GetAllSubmapData()) { + proto::Trajectory* trajectory_proto = + trajectory(submap_id_data.id.trajectory_id); + if (!include_unfinished_submaps && + !submap_id_data.data.submap->insertion_finished()) { + // Collect IDs of all unfinished submaps and skip them. + unfinished_submaps.insert(submap_id_data.id); + continue; + } + CHECK(submap_id_data.data.submap != nullptr); + auto* const submap_proto = trajectory_proto->add_submap(); + submap_proto->set_submap_index(submap_id_data.id.submap_index); + *submap_proto->mutable_pose() = + transform::ToProto(submap_id_data.data.pose); + } + + auto constraints_copy = constraints(); + std::set orphaned_nodes; + proto.mutable_constraint()->Reserve(constraints_copy.size()); + for (auto it = constraints_copy.begin(); it != constraints_copy.end();) { + if (!include_unfinished_submaps && + unfinished_submaps.count(it->submap_id) > 0) { + // Skip all those constraints that refer to unfinished submaps and + // remember the corresponding trajectory nodes as potentially orphaned. + orphaned_nodes.insert(it->node_id); + it = constraints_copy.erase(it); + continue; + } + *proto.add_constraint() = cartographer::mapping::ToProto(*it); + ++it; + } + + if (!include_unfinished_submaps) { + // Iterate over all constraints and remove trajectory nodes from + // 'orphaned_nodes' that are not actually orphaned. + for (const auto& constraint : constraints_copy) { + orphaned_nodes.erase(constraint.node_id); + } + } + + for (const auto& node_id_data : GetTrajectoryNodes()) { + proto::Trajectory* trajectory_proto = + trajectory(node_id_data.id.trajectory_id); + CHECK(node_id_data.data.constant_data != nullptr); + auto* const node_proto = trajectory_proto->add_node(); + node_proto->set_node_index(node_id_data.id.node_index); + node_proto->set_timestamp( + common::ToUniversal(node_id_data.data.constant_data->time)); + *node_proto->mutable_pose() = + transform::ToProto(node_id_data.data.global_pose); + } + + auto landmarks_copy = GetLandmarkPoses(); + proto.mutable_landmark_poses()->Reserve(landmarks_copy.size()); + for (const auto& id_pose : landmarks_copy) { + auto* landmark_proto = proto.add_landmark_poses(); + landmark_proto->set_landmark_id(id_pose.first); + *landmark_proto->mutable_global_pose() = transform::ToProto(id_pose.second); + } + return proto; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph.h b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph.h new file mode 100644 index 0000000..b4dfd3b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph.h @@ -0,0 +1,148 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_H_ + +#include +#include +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/pose_graph_trimmer.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/pose_graph_options.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer/sensor/map_by_time.h" +#include "cartographer/sensor/odometry_data.h" + +namespace cartographer { +namespace mapping { + +proto::PoseGraphOptions CreatePoseGraphOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class PoseGraph : public PoseGraphInterface { + public: + struct InitialTrajectoryPose { + int to_trajectory_id; + transform::Rigid3d relative_pose; + common::Time time; + }; + + PoseGraph() {} + ~PoseGraph() override {} + + PoseGraph(const PoseGraph&) = delete; + PoseGraph& operator=(const PoseGraph&) = delete; + + // Inserts an IMU measurement. + virtual void AddImuData(int trajectory_id, + const sensor::ImuData& imu_data) = 0; + + // Inserts an odometry measurement. + virtual void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) = 0; + + // Inserts a fixed frame pose measurement. + virtual void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) = 0; + + // Inserts landmarks observations. + virtual void AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) = 0; + + // Finishes the given trajectory. + virtual void FinishTrajectory(int trajectory_id) = 0; + + // Freezes a trajectory. Poses in this trajectory will not be optimized. + virtual void FreezeTrajectory(int trajectory_id) = 0; + + // Adds a 'submap' from a proto with the given 'global_pose' to the + // appropriate trajectory. + virtual void AddSubmapFromProto(const transform::Rigid3d& global_pose, + const proto::Submap& submap) = 0; + + // Adds a 'node' from a proto with the given 'global_pose' to the + // appropriate trajectory. + virtual void AddNodeFromProto(const transform::Rigid3d& global_pose, + const proto::Node& node) = 0; + + // Sets the trajectory data from a proto. + virtual void SetTrajectoryDataFromProto( + const mapping::proto::TrajectoryData& data) = 0; + + // Adds information that 'node_id' was inserted into 'submap_id'. The submap + // has to be deserialized first. + virtual void AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) = 0; + + // Adds serialized constraints. The corresponding trajectory nodes and submaps + // have to be deserialized before calling this function. + virtual void AddSerializedConstraints( + const std::vector& constraints) = 0; + + // Adds a 'trimmer'. It will be used after all data added before it has been + // included in the pose graph. + virtual void AddTrimmer(std::unique_ptr trimmer) = 0; + + // Gets the current trajectory clusters. + virtual std::vector> GetConnectedTrajectories() const = 0; + + proto::PoseGraph ToProto(bool include_unfinished_submaps) const override; + + // Returns the IMU data. + virtual sensor::MapByTime GetImuData() const = 0; + + // Returns the odometry data. + virtual sensor::MapByTime GetOdometryData() const = 0; + + // Returns the fixed frame pose data. + virtual sensor::MapByTime GetFixedFramePoseData() + const = 0; + + // Returns the landmark data. + virtual std::map + GetLandmarkNodes() const = 0; + + // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with + // respect to 'to_trajectory_id' at time 'time'. + virtual void SetInitialTrajectoryPose(int from_trajectory_id, + int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) = 0; +}; + +std::vector FromProto( + const ::google::protobuf::RepeatedPtrField< + ::cartographer::mapping::proto::PoseGraph::Constraint>& + constraint_protos); +proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_interface.h new file mode 100644 index 0000000..68551f1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_interface.h @@ -0,0 +1,165 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_POSE_GRAPH_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ + +#include +#include + +#include "absl/types/optional.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { + +class PoseGraphInterface { + 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 Constraint { + struct Pose { + transform::Rigid3d zbar_ij; + double translation_weight; + double rotation_weight; + }; + + SubmapId submap_id; // 'i' in the paper. + NodeId node_id; // 'j' in the paper. + + // Pose of the node 'j' relative to submap 'i'. + Pose pose; + + // Differentiates between intra-submap (where node 'j' was inserted into + // submap 'i') and inter-submap constraints (where node 'j' was not inserted + // into submap 'i'). + enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; + }; + + struct LandmarkNode { + struct LandmarkObservation { + int trajectory_id; + common::Time time; + transform::Rigid3d landmark_to_tracking_transform; + double translation_weight; + double rotation_weight; + }; + std::vector landmark_observations; + absl::optional global_landmark_pose; + bool frozen = false; + }; + + struct SubmapPose { + int version; + transform::Rigid3d pose; + }; + + struct SubmapData { + std::shared_ptr submap; + transform::Rigid3d pose; + }; + + struct TrajectoryData { + double gravity_constant = 9.8; + std::array imu_calibration{{1., 0., 0., 0.}}; + absl::optional fixed_frame_origin_in_map; + }; + + enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED }; + + using GlobalSlamOptimizationCallback = + std::function&, + const std::map&)>; + + PoseGraphInterface() {} + virtual ~PoseGraphInterface() {} + + PoseGraphInterface(const PoseGraphInterface&) = delete; + PoseGraphInterface& operator=(const PoseGraphInterface&) = delete; + + // Waits for all computations to finish and computes optimized poses. + virtual void RunFinalOptimization() = 0; + + // Returns data for all submaps. + virtual MapById GetAllSubmapData() const = 0; + + // Returns the current optimized transform and submap itself for the given + // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does + // not exist (anymore). + virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; + + // Returns the global poses for all submaps. + virtual MapById GetAllSubmapPoses() const = 0; + + // Returns the transform converting data in the local map frame (i.e. the + // continuous, non-loop-closed frame) into the global map frame (i.e. the + // discontinuous, loop-closed frame). + virtual transform::Rigid3d GetLocalToGlobalTransform( + int trajectory_id) const = 0; + + // Returns the current optimized trajectories. + virtual MapById GetTrajectoryNodes() const = 0; + + // Returns the current optimized trajectory poses. + virtual MapById GetTrajectoryNodePoses() + const = 0; + + // Returns the states of trajectories. + virtual std::map GetTrajectoryStates() const = 0; + + // Returns the current optimized landmark poses. + virtual std::map GetLandmarkPoses() + const = 0; + + // Sets global pose of landmark 'landmark_id' to given 'global_pose'. + virtual void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose, + const bool frozen = false) = 0; + + // Deletes a trajectory asynchronously. + virtual void DeleteTrajectory(int trajectory_id) = 0; + + // Checks if the given trajectory is finished. + virtual bool IsTrajectoryFinished(int trajectory_id) const = 0; + + // Checks if the given trajectory is frozen. + virtual bool IsTrajectoryFrozen(int trajectory_id) const = 0; + + // Returns the trajectory data. + virtual std::map GetTrajectoryData() const = 0; + + // Returns the collection of constraints. + virtual std::vector constraints() const = 0; + + // Serializes the constraints and trajectories. If + // 'include_unfinished_submaps' is set to 'true', unfinished submaps, i.e. + // submaps that have not yet received all rangefinder data insertions, will + // be included, otherwise not. + virtual proto::PoseGraph ToProto(bool include_unfinished_submaps) const = 0; + + // Sets the callback function that is invoked whenever the global optimization + // problem is solved. + virtual void SetGlobalSlamOptimizationCallback( + GlobalSlamOptimizationCallback callback) = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_test.cc new file mode 100644 index 0000000..7aa95d1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_test.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_graph.h" + +#include "cartographer/mapping/internal/testing/test_helpers.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/transform/transform.h" +#include "google/protobuf/util/message_differencer.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(PoseGraph, SerializeConstraint) { + proto::PoseGraph::Constraint expected_constraint = + testing::CreateFakeConstraint(testing::CreateFakeNode(1, 2), + testing::CreateFakeSubmap3D(2, 3)); + ::google::protobuf::RepeatedPtrField + constraint_protos; + *constraint_protos.Add() = expected_constraint; + PoseGraph::Constraint constraint = FromProto(constraint_protos).front(); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + expected_constraint, ToProto(constraint))); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer.cc new file mode 100644 index 0000000..8604444 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer.cc @@ -0,0 +1,50 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_graph_trimmer.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id, + const int num_submaps_to_keep) + : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) { + CHECK_GE(num_submaps_to_keep, 2) << "Cannot trim with less than 2 submaps"; +} + +void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { + if (pose_graph->IsFinished(trajectory_id_)) { + num_submaps_to_keep_ = 0; + } + + auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); + for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) { + pose_graph->TrimSubmap(submap_ids.at(i)); + } + + if (num_submaps_to_keep_ == 0) { + finished_ = true; + pose_graph->SetTrajectoryState( + trajectory_id_, PoseGraphInterface::TrajectoryState::DELETED); + } +} + +bool PureLocalizationTrimmer::IsFinished() { return finished_; } + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer.h b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer.h new file mode 100644 index 0000000..87c2cc8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer.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. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_ + +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph_interface.h" + +namespace cartographer { +namespace mapping { + +// Implemented by the pose graph to provide thread-safe access to functions for +// trimming the graph. +class Trimmable { + public: + virtual ~Trimmable() {} + + virtual int num_submaps(int trajectory_id) const = 0; + + virtual std::vector GetSubmapIds(int trajectory_id) const = 0; + // Returns finished submaps with optimized poses only. + virtual MapById + GetOptimizedSubmapData() const = 0; + virtual const MapById& GetTrajectoryNodes() const = 0; + virtual const std::vector& GetConstraints() + const = 0; + + // Trim 'submap_id' and corresponding intra-submap nodes. They + // will no longer take part in scan matching, loop closure, visualization. + // The numbering remains unchanged. + virtual void TrimSubmap(const SubmapId& submap_id) = 0; + + // Checks if the given trajectory is finished or not. + virtual bool IsFinished(int trajectory_id) const = 0; + + // Sets the state for a specific trajectory. + virtual void SetTrajectoryState( + int trajectory_id, PoseGraphInterface::TrajectoryState state) = 0; +}; + +// An interface to implement algorithms that choose how to trim the pose graph. +class PoseGraphTrimmer { + public: + virtual ~PoseGraphTrimmer() {} + + // Called once after each pose graph optimization. + virtual void Trim(Trimmable* pose_graph) = 0; + + // Checks if this trimmer is in a terminatable state. + virtual bool IsFinished() = 0; +}; + +// Keeps the last 'num_submaps_to_keep' of the trajectory with 'trajectory_id' +// to implement localization without mapping. +class PureLocalizationTrimmer : public PoseGraphTrimmer { + public: + PureLocalizationTrimmer(int trajectory_id, int num_submaps_to_keep); + ~PureLocalizationTrimmer() override {} + + void Trim(Trimmable* pose_graph) override; + bool IsFinished() override; + + private: + const int trajectory_id_; + int num_submaps_to_keep_; + bool finished_ = false; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer_test.cc new file mode 100644 index 0000000..daad1bc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/pose_graph_trimmer_test.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_graph_trimmer.h" + +#include + +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/testing/fake_trimmable.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(PureLocalizationTrimmerTest, MarksSubmapsAsExpected) { + const int kTrajectoryId = 42; + PureLocalizationTrimmer trimmer(kTrajectoryId, 15); + testing::FakeTrimmable fake_pose_graph(kTrajectoryId, 17); + trimmer.Trim(&fake_pose_graph); + + const auto trimmed_submaps = fake_pose_graph.trimmed_submaps(); + ASSERT_EQ(2, trimmed_submaps.size()); + EXPECT_EQ((SubmapId{kTrajectoryId, 0}), trimmed_submaps[0]); + EXPECT_EQ((SubmapId{kTrajectoryId, 1}), trimmed_submaps[1]); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/probability_values.cc b/carto_ws/src/cartographer-master/cartographer/mapping/probability_values.cc new file mode 100644 index 0000000..b0b05b5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/probability_values.cc @@ -0,0 +1,108 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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 "absl/memory/memory.h" + +namespace cartographer { +namespace mapping { + +namespace { + +constexpr int kValueCount = 32768; + +// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound]. +float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value, + const float unknown_result, + const float lower_bound, + const float upper_bound) { + CHECK_LT(value, kValueCount); + if (value == unknown_value) return unknown_result; + const float kScale = (upper_bound - lower_bound) / (kValueCount - 2.f); + return value * kScale + (lower_bound - kScale); +} + +std::unique_ptr> PrecomputeValueToBoundedFloat( + const uint16 unknown_value, const float unknown_result, + const float lower_bound, const float upper_bound) { + auto result = absl::make_unique>(); + // Repeat two times, so that both values with and without the update marker + // can be converted to a probability. + constexpr int kRepetitionCount = 2; + result->reserve(kRepetitionCount * kValueCount); + for (int repeat = 0; repeat != kRepetitionCount; ++repeat) { + for (int value = 0; value != kValueCount; ++value) { + result->push_back(SlowValueToBoundedFloat( + value, unknown_value, unknown_result, lower_bound, upper_bound)); + } + } + return result; +} + +std::unique_ptr> PrecomputeValueToProbability() { + return PrecomputeValueToBoundedFloat(kUnknownProbabilityValue, + kMinProbability, kMinProbability, + kMaxProbability); +} + +std::unique_ptr> PrecomputeValueToCorrespondenceCost() { + return PrecomputeValueToBoundedFloat( + kUnknownCorrespondenceValue, kMaxCorrespondenceCost, + kMinCorrespondenceCost, kMaxCorrespondenceCost); +} + +} // namespace + +const std::vector* const kValueToProbability = + PrecomputeValueToProbability().release(); + +const std::vector* const kValueToCorrespondenceCost = + PrecomputeValueToCorrespondenceCost().release(); + +std::vector ComputeLookupTableToApplyOdds(const float odds) { + std::vector result; + result.reserve(kValueCount); + result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) + + kUpdateMarker); + for (int cell = 1; cell != kValueCount; ++cell) { + result.push_back(ProbabilityToValue(ProbabilityFromOdds( + odds * Odds((*kValueToProbability)[cell]))) + + kUpdateMarker); + } + return result; +} + +std::vector ComputeLookupTableToApplyCorrespondenceCostOdds( + float odds) { + std::vector result; + result.reserve(kValueCount); + result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost( + ProbabilityFromOdds(odds))) + + kUpdateMarker); + for (int cell = 1; cell != kValueCount; ++cell) { + result.push_back( + CorrespondenceCostToValue( + ProbabilityToCorrespondenceCost(ProbabilityFromOdds( + odds * Odds(CorrespondenceCostToProbability( + (*kValueToCorrespondenceCost)[cell]))))) + + kUpdateMarker); + } + return result; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/probability_values.h b/carto_ws/src/cartographer-master/cartographer/mapping/probability_values.h new file mode 100644 index 0000000..2ab57aa --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/probability_values.h @@ -0,0 +1,148 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ +#define CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ + +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +namespace { + +inline uint16 BoundedFloatToValue(const float float_value, + const float lower_bound, + const float upper_bound) { + const int value = + common::RoundToInt( + (common::Clamp(float_value, lower_bound, upper_bound) - lower_bound) * + (32766.f / (upper_bound - lower_bound))) + + 1; + // DCHECK for performance. + DCHECK_GE(value, 1); + DCHECK_LE(value, 32767); + return value; +} + +} // namespace + +inline float Odds(float probability) { + return probability / (1.f - probability); +} + +inline float ProbabilityFromOdds(const float odds) { + return odds / (odds + 1.f); +} + +inline float ProbabilityToCorrespondenceCost(const float probability) { + return 1.f - probability; +} + +inline float CorrespondenceCostToProbability(const float correspondence_cost) { + return 1.f - correspondence_cost; +} + +constexpr float kMinProbability = 0.1f; +constexpr float kMaxProbability = 1.f - kMinProbability; +constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; +constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; + +// Clamps probability to be in the range [kMinProbability, kMaxProbability]. +inline float ClampProbability(const float probability) { + return common::Clamp(probability, kMinProbability, kMaxProbability); +} +// Clamps correspondece cost to be in the range [kMinCorrespondenceCost, +// kMaxCorrespondenceCost]. +inline float ClampCorrespondenceCost(const float correspondence_cost) { + return common::Clamp(correspondence_cost, kMinCorrespondenceCost, + kMaxCorrespondenceCost); +} + +constexpr uint16 kUnknownProbabilityValue = 0; +constexpr uint16 kUnknownCorrespondenceValue = kUnknownProbabilityValue; +constexpr uint16 kUpdateMarker = 1u << 15; + +// Converts a correspondence_cost to a uint16 in the [1, 32767] range. +inline uint16 CorrespondenceCostToValue(const float correspondence_cost) { + return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost, + kMaxCorrespondenceCost); +} + +// Converts a probability to a uint16 in the [1, 32767] range. +inline uint16 ProbabilityToValue(const float probability) { + return BoundedFloatToValue(probability, kMinProbability, kMaxProbability); +} + +extern const std::vector* const kValueToProbability; +extern const std::vector* const kValueToCorrespondenceCost; + +// 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]; +} + +// Converts a uint16 (which may or may not have the update marker set) to a +// correspondence cost in the range [kMinCorrespondenceCost, +// kMaxCorrespondenceCost]. +inline float ValueToCorrespondenceCost(const uint16 value) { + return (*kValueToCorrespondenceCost)[value]; +} + +inline uint16 ProbabilityValueToCorrespondenceCostValue( + uint16 probability_value) { + if (probability_value == kUnknownProbabilityValue) { + return kUnknownCorrespondenceValue; + } + bool update_carry = false; + if (probability_value > kUpdateMarker) { + probability_value -= kUpdateMarker; + update_carry = true; + } + uint16 result = CorrespondenceCostToValue( + ProbabilityToCorrespondenceCost(ValueToProbability(probability_value))); + if (update_carry) result += kUpdateMarker; + return result; +} + +inline uint16 CorrespondenceCostValueToProbabilityValue( + uint16 correspondence_cost_value) { + if (correspondence_cost_value == kUnknownCorrespondenceValue) + return kUnknownProbabilityValue; + bool update_carry = false; + if (correspondence_cost_value > kUpdateMarker) { + correspondence_cost_value -= kUpdateMarker; + update_carry = true; + } + uint16 result = ProbabilityToValue(CorrespondenceCostToProbability( + ValueToCorrespondenceCost(correspondence_cost_value))); + if (update_carry) result += kUpdateMarker; + return result; +} + +std::vector ComputeLookupTableToApplyOdds(float odds); +std::vector ComputeLookupTableToApplyCorrespondenceCostOdds(float odds); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/probability_values_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/probability_values_test.cc new file mode 100644 index 0000000..328456a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/probability_values_test.cc @@ -0,0 +1,173 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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); +} + +TEST(ProbabilityValuesTest, OddsConversionsCorrespondenceCost) { + EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(Odds( + CorrespondenceCostToProbability(kMaxCorrespondenceCost)))), + kMaxCorrespondenceCost, 1e-6); + EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(Odds( + CorrespondenceCostToProbability(kMinCorrespondenceCost)))), + kMinCorrespondenceCost, 1e-6); + EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds( + Odds(CorrespondenceCostToProbability(0.5)))), + 0.5, 1e-6); +} + +TEST(ProbabilityValuesTest, + ProbabilityValueToCorrespondenceCostValueConversions) { + for (uint16 i = 0; i < 32768; ++i) { + EXPECT_EQ(ProbabilityValueToCorrespondenceCostValue( + CorrespondenceCostValueToProbabilityValue(i)), + i); + EXPECT_EQ(CorrespondenceCostValueToProbabilityValue( + ProbabilityValueToCorrespondenceCostValue(i)), + i); + } +} + +TEST(ProbabilityValuesTest, + ProbabilityValueToCorrespondenceCostValueConversionsWithUpdateMarker) { + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_EQ(ProbabilityValueToCorrespondenceCostValue( + CorrespondenceCostValueToProbabilityValue(i + kUpdateMarker)), + i + kUpdateMarker); + EXPECT_EQ(CorrespondenceCostValueToProbabilityValue( + ProbabilityValueToCorrespondenceCostValue(i + kUpdateMarker)), + i + kUpdateMarker); + } +} + +TEST(ProbabilityValuesTest, ConversionLookUpTable) { + EXPECT_NEAR(ValueToProbability(0), 1.f - ValueToCorrespondenceCost(0), 1e-6); + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_NEAR(ValueToProbability(i), ValueToCorrespondenceCost(i), 1e-6) + << " i " << i; + } +} + +TEST(ProbabilityValuesTest, CellUpdate) { + std::vector probability_table = + ComputeLookupTableToApplyOdds(Odds(0.9f)); + std::vector correspondence_table = + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9f)); + uint16 cell_pg_pre_update = 0; + uint16 cell_cg_pre_update = 0; + uint16 cell_pg_post_update = probability_table[cell_pg_pre_update]; + uint16 cell_cg_post_update = correspondence_table[cell_cg_pre_update]; + float p_post = ValueToProbability(cell_pg_post_update); + float c_post = ValueToCorrespondenceCost(cell_cg_post_update); + EXPECT_NEAR(p_post, 1.f - c_post, 1e-6); + int num_evaluations = 5000; + for (int i_probability = 0; i_probability < num_evaluations; + ++i_probability) { + float p = (static_cast(i_probability) / + static_cast(num_evaluations)) * + (kMaxProbability - kMinProbability) + + kMinProbability; + cell_pg_pre_update = ProbabilityToValue(p); + cell_cg_pre_update = + CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(p)); + float p_value = + (common::Clamp(p, kMinProbability, kMaxProbability) - kMinProbability) * + (32766.f / (kMaxProbability - kMinProbability)); + float cvalue = (common::Clamp(ProbabilityToCorrespondenceCost(p), + kMinProbability, kMaxProbability) - + kMinProbability) * + (32766.f / (kMaxProbability - kMinProbability)); + + EXPECT_NEAR(cell_pg_pre_update, 32768 - cell_cg_pre_update, 1) + << "p " << p << " p_value " << p_value << " cvalue " << cvalue + << " p_valuei " << common::RoundToInt(p_value) << " cvaluei " + << common::RoundToInt(cvalue); + cell_pg_post_update = probability_table[cell_pg_pre_update]; + cell_cg_post_update = correspondence_table[cell_cg_pre_update]; + p_post = ValueToProbability(cell_pg_post_update); + c_post = ValueToCorrespondenceCost(cell_cg_post_update); + EXPECT_NEAR(p_post, 1.f - c_post, 5e-5) + << "p " << p << " " << p_post - 1.f + c_post; + } +} + +TEST(ProbabilityValuesTest, MultipleCellUpdate) { + std::vector probability_table = + ComputeLookupTableToApplyOdds(Odds(0.55f)); + std::vector correspondence_table = + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.55f)); + uint16 cell_pg_post_update = probability_table[0]; + uint16 cell_cg_post_update = correspondence_table[0]; + float p_post = ValueToProbability(cell_pg_post_update); + float c_post = ValueToCorrespondenceCost(cell_cg_post_update); + EXPECT_NEAR(p_post, 1.f - c_post, 1e-6); + int num_evaluations = 5000; + for (int i_probability = 0; i_probability < num_evaluations; + ++i_probability) { + float p = (static_cast(i_probability) / + static_cast(num_evaluations)) * + (kMaxProbability - kMinProbability) + + kMinProbability; + cell_pg_post_update = ProbabilityToValue(p) + kUpdateMarker; + cell_cg_post_update = + CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(p)) + + kUpdateMarker; + for (int i_update = 0; i_update < 20; ++i_update) { + cell_pg_post_update = + probability_table[cell_pg_post_update - kUpdateMarker]; + cell_cg_post_update = + correspondence_table[cell_cg_post_update - kUpdateMarker]; + } + p_post = ValueToProbability(cell_pg_post_update); + c_post = ValueToCorrespondenceCost(cell_cg_post_update); + EXPECT_NEAR(p_post, 1.f - c_post, 5e-5) + << "p " << p << " p_post " << p_post << " " << p_post - 1.f + c_post; + } +} + +TEST(ProbabilityValuesTest, EqualityLookupTableToApplyOdds) { + std::vector probability_table = ComputeLookupTableToApplyOdds(0.3); + std::vector correspondence_table = + ComputeLookupTableToApplyCorrespondenceCostOdds(0.3); + for (int i = 0; i < 32768; ++i) { + EXPECT_NEAR( + probability_table[i], + CorrespondenceCostValueToProbabilityValue( + correspondence_table[ProbabilityValueToCorrespondenceCostValue(i)]), + 1); + EXPECT_NEAR( + ProbabilityValueToCorrespondenceCostValue( + probability_table[CorrespondenceCostValueToProbabilityValue(i)]), + correspondence_table[i], 1); + } +} + +} // namespace +} // namespace mapping +} // namespace cartographer \ No newline at end of file diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/cell_limits_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/cell_limits_2d.proto new file mode 100644 index 0000000..6cee237 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/cell_limits_2d.proto @@ -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. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message CellLimits { + int32 num_x_cells = 1; + int32 num_y_cells = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/connected_components.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/connected_components.proto new file mode 100644 index 0000000..49e5006 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/connected_components.proto @@ -0,0 +1,30 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +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 = "ConnectedComponentsOuterClass"; + +// Connectivity structure between trajectories. +message ConnectedComponents { + message ConnectedComponent { + repeated int32 trajectory_id = 1; + } + + repeated ConnectedComponent connected_component = 1; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/grid_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/grid_2d.proto new file mode 100644 index 0000000..010d5b6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/grid_2d.proto @@ -0,0 +1,42 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/map_limits.proto"; +import "cartographer/mapping/proto/probability_grid.proto"; +import "cartographer/mapping/proto/tsdf_2d.proto"; + +message Grid2D { + message CellBox { + int32 max_x = 1; + int32 max_y = 2; + int32 min_x = 3; + int32 min_y = 4; + } + + MapLimits limits = 1; + // These values are actually int16s, but protos don't have a native int16 + // type. + repeated int32 cells = 2; + CellBox known_cells_box = 3; + oneof grid { + ProbabilityGrid probability_grid_2d = 4; + TSDF2D tsdf_2d = 5; + } + float min_correspondence_cost = 6; + float max_correspondence_cost = 7; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/grid_2d_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/grid_2d_options.proto new file mode 100644 index 0000000..4dcc506 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/grid_2d_options.proto @@ -0,0 +1,28 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message GridOptions2D { + enum GridType { + INVALID_GRID = 0; + PROBABILITY_GRID = 1; + TSDF = 2; + } + + GridType grid_type = 1; + float resolution = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/hybrid_grid.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/hybrid_grid.proto new file mode 100644 index 0000000..b53f366 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/hybrid_grid.proto @@ -0,0 +1,28 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message HybridGrid { + float resolution = 1; + // '{x, y, z}_indices[i]' is the index of 'values[i]'. + repeated sint32 x_indices = 3; + repeated sint32 y_indices = 4; + repeated sint32 z_indices = 5; + // The entries in 'values' should be uint16s, not int32s, but protos don't + // have a uint16 type. + repeated int32 values = 6; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto new file mode 100644 index 0000000..6add120 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto @@ -0,0 +1,77 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; +import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; +import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto"; +import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/proto/submaps_options_2d.proto"; + +// NEXT ID: 22 +message LocalTrajectoryBuilderOptions2D { + // Rangefinder points outside these ranges will be dropped. + float min_range = 14; + float max_range = 15; + float min_z = 1; + float max_z = 2; + + // Points beyond 'max_range' will be inserted with this length as empty space. + float missing_data_ray_length = 16; + + // Number of range data to accumulate into one unwarped, combined range data + // to use for scan matching. + int32 num_accumulated_range_data = 19; + + // Voxel filter that gets applied to the range data immediately after + // cropping. + float voxel_filter_size = 3; + + // Voxel filter used to compute a sparser point cloud for matching. + sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 6; + + // Voxel filter used to compute a sparser point cloud for finding loop + // closures. + sensor.proto.AdaptiveVoxelFilterOptions + loop_closure_adaptive_voxel_filter_options = 20; + + // Whether to solve the online scan matching first using the correlative scan + // matcher to generate a good starting point for Ceres. + bool use_online_correlative_scan_matching = 5; + cartographer.mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions + real_time_correlative_scan_matcher_options = 7; + cartographer.mapping.scan_matching.proto.CeresScanMatcherOptions2D + ceres_scan_matcher_options = 8; + MotionFilterOptions motion_filter_options = 13; + + // Time constant in seconds for the orientation moving average based on + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. + double imu_gravity_time_constant = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21; + + SubmapsOptions2D submaps_options = 11; + + // True if IMU data should be expected and used. + bool use_imu_data = 12; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto new file mode 100644 index 0000000..2ff762a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto @@ -0,0 +1,79 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; +import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; +import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/proto/submaps_options_3d.proto"; +import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/timestamped_transform.proto"; + +// NEXT ID: 22 +message LocalTrajectoryBuilderOptions3D { + // Rangefinder points outside these ranges will be dropped. + float min_range = 1; + float max_range = 2; + + // Number of range data to accumulate into one unwarped, combined range data + // to use for scan matching. + int32 num_accumulated_range_data = 3; + + // Voxel filter that gets applied to the range data immediately after + // cropping. + float voxel_filter_size = 4; + + // Voxel filter used to compute a sparser point cloud for matching. + sensor.proto.AdaptiveVoxelFilterOptions + high_resolution_adaptive_voxel_filter_options = 5; + sensor.proto.AdaptiveVoxelFilterOptions + low_resolution_adaptive_voxel_filter_options = 12; + + // Whether to solve the online scan matching first using the correlative scan + // matcher to generate a good starting point for Ceres. + bool use_online_correlative_scan_matching = 13; + mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions + real_time_correlative_scan_matcher_options = 14; + mapping.scan_matching.proto.CeresScanMatcherOptions3D + ceres_scan_matcher_options = 6; + mapping.proto.MotionFilterOptions motion_filter_options = 7; + + // Time constant in seconds for the orientation moving average based on + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. + double imu_gravity_time_constant = 15; + + // Number of histogram buckets for the rotational scan matcher. + int32 rotational_histogram_size = 17; + + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; + + repeated transform.proto.TimestampedTransform initial_poses = 19; + repeated sensor.proto.ImuData initial_imu_data = 20; + + SubmapsOptions3D submaps_options = 8; + + // Whether to use Lidar intensities in Ceres Scan Matcher. + bool use_intensities = 21; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/map_builder_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/map_builder_options.proto new file mode 100644 index 0000000..48f1ef3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/map_builder_options.proto @@ -0,0 +1,30 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/mapping/proto/pose_graph_options.proto"; + +package cartographer.mapping.proto; + +message MapBuilderOptions { + bool use_trajectory_builder_2d = 1; + bool use_trajectory_builder_3d = 2; + + // Number of threads to use for background computations. + int32 num_background_threads = 3; + PoseGraphOptions pose_graph_options = 4; + // Sort sensor input independently for each trajectory. + bool collate_by_trajectory = 5; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/map_limits.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/map_limits.proto new file mode 100644 index 0000000..53044e6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/map_limits.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 = "proto3"; + +import "cartographer/mapping/proto/cell_limits_2d.proto"; +import "cartographer/transform/proto/transform.proto"; + +package cartographer.mapping.proto; + +message MapLimits { + double resolution = 1; + cartographer.transform.proto.Vector2d max = 2; + CellLimits cell_limits = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/motion_filter_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/motion_filter_options.proto new file mode 100644 index 0000000..ed5b61f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/motion_filter_options.proto @@ -0,0 +1,28 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message MotionFilterOptions { + // Threshold above which range data is inserted based on time. + double max_time_seconds = 1; + + // Threshold above which range data is inserted based on linear motion. + double max_distance_meters = 2; + + // Threshold above which range data is inserted based on rotational motion. + double max_angle_radians = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/normal_estimation_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/normal_estimation_options_2d.proto new file mode 100644 index 0000000..dc88368 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/normal_estimation_options_2d.proto @@ -0,0 +1,22 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message NormalEstimationOptions2D { + int32 num_normal_samples = 1; + float sample_radius = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_extrapolator_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_extrapolator_options.proto new file mode 100644 index 0000000..ff6a453 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_extrapolator_options.proto @@ -0,0 +1,48 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +package cartographer.mapping.proto; + +message ConstantVelocityPoseExtrapolatorOptions { + // Time constant in seconds for the orientation moving average based on + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + double imu_gravity_time_constant = 1; + double pose_queue_duration = 2; +} + +message ImuBasedPoseExtrapolatorOptions { + double pose_queue_duration = 1; + double gravity_constant = 2; + double pose_translation_weight = 3; + double pose_rotation_weight = 4; + double imu_acceleration_weight = 5; + double imu_rotation_weight = 6; + cartographer.common.proto.CeresSolverOptions solver_options = 7; + double odometry_translation_weight = 8; + double odometry_rotation_weight = 9; +} + +message PoseExtrapolatorOptions { + bool use_imu_based = 1; + ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2; + ImuBasedPoseExtrapolatorOptions imu_based = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph.proto new file mode 100644 index 0000000..76c10b8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph.proto @@ -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. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/trajectory.proto"; +import "cartographer/transform/proto/transform.proto"; + +message SubmapId { + int32 trajectory_id = 1; + int32 submap_index = 2; // Submap index in the given trajectory. +} + +message NodeId { + int32 trajectory_id = 1; + int32 node_index = 2; // Node index in the given trajectory. +} + +message PoseGraph { + message Constraint { + // Differentiates between intra-submap (where the range data was inserted + // into the submap) and inter-submap constraints (where the range data was + // not inserted into the submap). + enum Tag { + INTRA_SUBMAP = 0; + INTER_SUBMAP = 1; + } + + SubmapId submap_id = 1; // Submap ID. + NodeId node_id = 2; // Node ID. + // Pose of the node relative to submap, i.e. taking data from the node frame + // into the submap frame. + transform.proto.Rigid3d relative_pose = 3; + // Weight of the translational part of the constraint. + double translation_weight = 6; + // Weight of the rotational part of the constraint. + double rotation_weight = 7; + Tag tag = 5; + } + + message LandmarkPose { + string landmark_id = 1; + transform.proto.Rigid3d global_pose = 2; + } + + repeated Constraint constraint = 2; + repeated Trajectory trajectory = 4; + repeated LandmarkPose landmark_poses = 5; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto new file mode 100644 index 0000000..8447cba --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto @@ -0,0 +1,59 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +package cartographer.mapping.constraints.proto; + +import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto"; +import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; +import "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.proto"; +import "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.proto"; + +message ConstraintBuilderOptions { + // A constraint will be added if the proportion of added constraints to + // potential constraints drops below this number. + double sampling_ratio = 1; + + // Threshold for poses to be considered near a submap. + double max_constraint_distance = 2; + + // 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. + double min_score = 4; + + // Threshold below which global localizations are not trusted. + double global_localization_min_score = 5; + + // Weight used in the optimization problem for the translational component of + // loop closure constraints. + double loop_closure_translation_weight = 13; + + // Weight used in the optimization problem for the rotational component of + // loop closure constraints. + double loop_closure_rotation_weight = 14; + + // If enabled, logs information of loop-closing constraints for debugging. + bool log_matches = 8; + + // Options for the internally used scan matchers. + mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions2D + fast_correlative_scan_matcher_options = 9; + mapping.scan_matching.proto.CeresScanMatcherOptions2D + ceres_scan_matcher_options = 11; + mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions3D + fast_correlative_scan_matcher_options_3d = 10; + mapping.scan_matching.proto.CeresScanMatcherOptions3D + ceres_scan_matcher_options_3d = 12; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto new file mode 100644 index 0000000..efc61c1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto @@ -0,0 +1,73 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.optimization.proto; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +// NEXT ID: 26 +message OptimizationProblemOptions { + reserved 20 to 22; // For visual constraints. + // Scaling parameter for Huber loss function. + double huber_scale = 1; + + // Scaling parameter for the IMU acceleration term. + double acceleration_weight = 8; + + // Scaling parameter for the IMU rotation term. + double rotation_weight = 9; + + // Scaling parameter for translation between consecutive nodes based on the + // local SLAM pose. + double local_slam_pose_translation_weight = 14; + + // Scaling parameter for rotation between consecutive nodes based on the + // local SLAM pose. + double local_slam_pose_rotation_weight = 15; + + // Scaling parameter for translation between consecutive nodes based on the + // odometry. + double odometry_translation_weight = 16; + + // Scaling parameter for rotation between consecutive nodes based on the + // odometry. + double odometry_rotation_weight = 17; + + // Scaling parameter for the FixedFramePose translation. Unit: 1/meters. + double fixed_frame_pose_translation_weight = 11; + + // Scaling parameter for the FixedFramePose rotation. + double fixed_frame_pose_rotation_weight = 12; + + bool fixed_frame_pose_use_tolerant_loss = 23; + // The following parameters are used only if fixed_frame_pose_use_tolerant_loss is true. + // See http://ceres-solver.org/nnls_modeling.html. + // For large values of s, the tolerant loss function approaches a null loss + // with fixed_frame_pose_translation_weight. + double fixed_frame_pose_tolerant_loss_param_a = 24; + double fixed_frame_pose_tolerant_loss_param_b = 25; + + // 3D only: fix Z. + bool fix_z_in_3d = 13; + + // 3D only: activate online IMU extrinsics. + bool use_online_imu_extrinsics_in_3d = 18; + + // If true, the Ceres solver summary will be logged for every optimization. + bool log_solver_summary = 5; + + common.proto.CeresSolverOptions ceres_solver_options = 7; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph_options.proto new file mode 100644 index 0000000..de2c8a3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/pose_graph_options.proto @@ -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. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/pose_graph/constraint_builder_options.proto"; +import "cartographer/mapping/proto/pose_graph/optimization_problem_options.proto"; + +message PoseGraphOptions { + // Online loop closure: If positive, will run the loop closure while the map + // is built. + int32 optimize_every_n_nodes = 1; + + // Options for the constraint builder. + mapping.constraints.proto.ConstraintBuilderOptions + constraint_builder_options = 3; + + // Weight used in the optimization problem for the translational component of + // non-loop-closure scan matcher constraints. + double matcher_translation_weight = 7; + + // Weight used in the optimization problem for the rotational component of + // non-loop-closure scan matcher constraints. + double matcher_rotation_weight = 8; + + // Options for the optimization problem. + mapping.optimization.proto.OptimizationProblemOptions + optimization_problem_options = 4; + + // Number of iterations to use in 'optimization_problem_options' for the final + // optimization. + int32 max_num_final_iterations = 6; + + // Rate at which we sample a single trajectory's nodes for global + // localization. + double global_sampling_ratio = 5; + + // Whether to output histograms for the pose residuals. + bool log_residual_histograms = 9; + + // If for the duration specified by this option no global contraint has been + // added between two trajectories, loop closure searches will be performed + // globally rather than in a smaller search window. + double global_constraint_search_after_n_seconds = 10; + + message OverlappingSubmapsTrimmerOptions2D { + int32 fresh_submaps_count = 1; + double min_covered_area = 2; + int32 min_added_submaps_count = 3; + } + + // Instantiates the 'OverlappingSubmapsTrimmer2d' which trims submaps from the + // pose graph based on the area of overlap. + OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 11; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/probability_grid.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/probability_grid.proto new file mode 100644 index 0000000..2a2f1b6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/probability_grid.proto @@ -0,0 +1,20 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message ProbabilityGrid { +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto new file mode 100644 index 0000000..c5140ec --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto @@ -0,0 +1,31 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message ProbabilityGridRangeDataInserterOptions2D { + // Probability change for a hit (this will be converted to odds and therefore + // must be greater than 0.5). + double hit_probability = 1; + + // Probability change for a miss (this will be converted to odds and therefore + // must be less than 0.5). + double miss_probability = 2; + + // If 'false', free space will not change the probabilities in the occupancy + // grid. + bool insert_free_space = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/range_data_inserter_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/range_data_inserter_options.proto new file mode 100644 index 0000000..9470f2c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/range_data_inserter_options.proto @@ -0,0 +1,33 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto"; + +package cartographer.mapping.proto; + +message RangeDataInserterOptions { + enum RangeDataInserterType { + INVALID_INSERTER = 0; + PROBABILITY_GRID_INSERTER_2D = 1; + TSDF_INSERTER_2D = 2; + } + + RangeDataInserterType range_data_inserter_type = 1; + ProbabilityGridRangeDataInserterOptions2D + probability_grid_range_data_inserter_options_2d = 2; + TSDFRangeDataInserterOptions2D tsdf_range_data_inserter_options_2d = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/range_data_inserter_options_3d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/range_data_inserter_options_3d.proto new file mode 100644 index 0000000..fd5a44a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/range_data_inserter_options_3d.proto @@ -0,0 +1,34 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message RangeDataInserterOptions3D { + // Probability change for a hit (this will be converted to odds and therefore + // must be greater than 0.5). + double hit_probability = 1; + + // Probability change for a miss (this will be converted to odds and therefore + // must be less than 0.5). + double miss_probability = 2; + + // Up to how many free space voxels are updated for scan matching. + // 0 disables free space. + int32 num_free_space_voxels = 3; + + // Do not insert intensities above this threshold into IntensityHybridGrid. + float intensity_threshold = 4; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto new file mode 100644 index 0000000..f1bcf48 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto @@ -0,0 +1,31 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.scan_matching.proto; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +// NEXT ID: 10 +message CeresScanMatcherOptions2D { + // Scaling parameters for each cost functor. + double occupied_space_weight = 1; + double translation_weight = 2; + double rotation_weight = 3; + + // Configure the Ceres solver. See the Ceres documentation for more + // information: https://code.google.com/p/ceres-solver/ + common.proto.CeresSolverOptions ceres_solver_options = 9; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto new file mode 100644 index 0000000..0f80184 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto @@ -0,0 +1,44 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.scan_matching.proto; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +message IntensityCostFunctionOptions { + double weight = 1; + double huber_scale = 2; + // Ignore ranges with intensity above this threshold. + float intensity_threshold = 3; +} + +// NEXT ID: 8 +message CeresScanMatcherOptions3D { + // Scaling parameters for each occupied space cost functor. + repeated double occupied_space_weight = 1; + double translation_weight = 2; + double rotation_weight = 3; + + // Whether only to allow changes to yaw, keeping roll/pitch constant. + bool only_optimize_yaw = 5; + + // Configure the Ceres solver. See the Ceres documentation for more + // information: https://code.google.com/p/ceres-solver/ + common.proto.CeresSolverOptions ceres_solver_options = 6; + + // Scaling parameters for each intensity cost functor. + repeated IntensityCostFunctionOptions intensity_cost_function_options = 7; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.proto new file mode 100644 index 0000000..3e5fe37 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.proto @@ -0,0 +1,30 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.scan_matching.proto; + +message FastCorrelativeScanMatcherOptions2D { + // Minimum linear search window in which the best possible scan alignment + // will be found. + double linear_search_window = 3; + + // Minimum angular search window in which the best possible scan alignment + // will be found. + double angular_search_window = 4; + + // Number of precomputed grids to use. + int32 branch_and_bound_depth = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.proto new file mode 100644 index 0000000..b6330f1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.proto @@ -0,0 +1,45 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.scan_matching.proto; + +message FastCorrelativeScanMatcherOptions3D { + // Number of precomputed grids to use. + int32 branch_and_bound_depth = 2; + + // Number of full resolution grids to use, additional grids will reduce the + // resolution by half each. + int32 full_resolution_depth = 8; + + // Minimum score for the rotational scan matcher. + double min_rotational_score = 4; + + // Threshold for the score of the low resolution grid below which a match is + // not considered. Only used for 3D. + double min_low_resolution_score = 9; + + // Linear search window in the plane orthogonal to gravity in which the best + // possible scan alignment will be found. + double linear_xy_search_window = 5; + + // Linear search window in the gravity direction in which the best possible + // scan alignment will be found. + double linear_z_search_window = 6; + + // Minimum angular search window in which the best possible scan alignment + // will be found. + double angular_search_window = 7; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto new file mode 100644 index 0000000..4ddaef3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto @@ -0,0 +1,31 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.scan_matching.proto; + +message RealTimeCorrelativeScanMatcherOptions { + // Minimum linear search window in which the best possible scan alignment + // will be found. + double linear_search_window = 1; + + // Minimum angular search window in which the best possible scan alignment + // will be found. + double angular_search_window = 2; + + // Weights applied to each part of the score. + double translation_delta_cost_weight = 3; + double rotation_delta_cost_weight = 4; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/serialization.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/serialization.proto new file mode 100644 index 0000000..aea7a0e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/serialization.proto @@ -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. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/pose_graph.proto"; +import "cartographer/mapping/proto/submap.proto"; +import "cartographer/mapping/proto/trajectory_node_data.proto"; +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/mapping/proto/trajectory_builder_options.proto"; +import "cartographer/transform/proto/transform.proto"; + +message Submap { + SubmapId submap_id = 1; + Submap2D submap_2d = 2; + Submap3D submap_3d = 3; +} + +message Node { + NodeId node_id = 1; + TrajectoryNodeData node_data = 5; +} + +message ImuData { + int32 trajectory_id = 1; + sensor.proto.ImuData imu_data = 2; +} + +message OdometryData { + int32 trajectory_id = 1; + sensor.proto.OdometryData odometry_data = 2; +} + +message FixedFramePoseData { + int32 trajectory_id = 1; + sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2; +} + +message LandmarkData { + int32 trajectory_id = 1; + sensor.proto.LandmarkData landmark_data = 2; +} + +message TrajectoryData { + int32 trajectory_id = 1; + double gravity_constant = 2; + transform.proto.Quaterniond imu_calibration = 3; + transform.proto.Rigid3d fixed_frame_origin_in_map = 4; +} + +message LocalSlamResultData { + int64 timestamp = 1; + TrajectoryNodeData node_data = 2; + repeated Submap submaps = 3; +} + +// Header of the serialization format. At the moment it only contains the +// version of the format. +message SerializationHeader { + uint32 format_version = 1; +} + +message SerializedData { + oneof data { + PoseGraph pose_graph = 1; + AllTrajectoryBuilderOptions all_trajectory_builder_options = 2; + Submap submap = 3; + Node node = 4; + TrajectoryData trajectory_data = 5; + ImuData imu_data = 6; + OdometryData odometry_data = 7; + FixedFramePoseData fixed_frame_pose_data = 8; + LandmarkData landmark_data = 9; + } +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/submap.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submap.proto new file mode 100644 index 0000000..5033127 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submap.proto @@ -0,0 +1,39 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/grid_2d.proto"; +import "cartographer/mapping/proto/hybrid_grid.proto"; +import "cartographer/transform/proto/transform.proto"; + +// Serialized state of a Submap2D. +message Submap2D { + transform.proto.Rigid3d local_pose = 1; + int32 num_range_data = 2; + bool finished = 3; + Grid2D grid = 4; +} + +// Serialized state of a Submap3D. +message Submap3D { + transform.proto.Rigid3d local_pose = 1; + int32 num_range_data = 2; + bool finished = 3; + HybridGrid high_resolution_hybrid_grid = 4; + HybridGrid low_resolution_hybrid_grid = 5; + repeated float rotational_scan_matcher_histogram = 6; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/submap_visualization.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submap_visualization.proto new file mode 100644 index 0000000..4cd9498 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submap_visualization.proto @@ -0,0 +1,73 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/transform/proto/transform.proto"; + +package cartographer.mapping.proto; + +message SubmapList { + message SubmapEntry { + int32 submap_version = 1; + 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'. + int32 submap_index = 1; + // Index into 'TrajectoryList.trajectory'. + int32 trajectory_id = 2; + } + + message Response { + // Version of the given submap, higher means newer. + int32 submap_version = 2; + + // Texture that visualizes a grid of a submap. + message SubmapTexture { + // GZipped map data, in row-major order, starting with (0,0). Each cell + // consists of two bytes: value (premultiplied by alpha) and alpha. + bytes cells = 1; + + // Dimensions of the grid in cells. + int32 width = 2; + int32 height = 3; + + // Size of one cell in meters. + double resolution = 4; + + // Pose of the resolution*width x resolution*height rectangle in the + // submap frame. + transform.proto.Rigid3d slice_pose = 5; + } + + // When multiple textures are present, high resolution comes first. + repeated SubmapTexture textures = 10; + + // Error message in response to malformed requests. + string error_message = 8; + } + + Request request = 1; + Response response = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/submaps_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submaps_options_2d.proto new file mode 100644 index 0000000..65838da --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submaps_options_2d.proto @@ -0,0 +1,29 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/mapping/proto/grid_2d_options.proto"; +import "cartographer/mapping/proto/range_data_inserter_options.proto"; + +package cartographer.mapping.proto; + +message SubmapsOptions2D { + // Number of range data before adding a new submap. Each submap will get twice + // the number of range data inserted: First for initialization without being + // matched against, then while being matched. + int32 num_range_data = 1; + GridOptions2D grid_options_2d = 2; + RangeDataInserterOptions range_data_inserter_options = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/submaps_options_3d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submaps_options_3d.proto new file mode 100644 index 0000000..822c6d9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/submaps_options_3d.proto @@ -0,0 +1,40 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/mapping/proto/range_data_inserter_options_3d.proto"; + +package cartographer.mapping.proto; + +message SubmapsOptions3D { + // Resolution of the 'high_resolution' map in meters used for local SLAM and + // loop closure. + double high_resolution = 1; + + // Maximum range to filter the point cloud to before insertion into the + // 'high_resolution' map. + double high_resolution_max_range = 4; + + // Resolution of the 'low_resolution' version of the map in meters used for + // local SLAM only. + double low_resolution = 5; + + // Number of range data before adding a new submap. Each submap will get twice + // the number of range data inserted: First for initialization without being + // matched against, then while being matched. + int32 num_range_data = 2; + + RangeDataInserterOptions3D range_data_inserter_options = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory.proto new file mode 100644 index 0000000..8cd7d57 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory.proto @@ -0,0 +1,51 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/transform/proto/transform.proto"; + +option java_outer_classname = "TrajectoryOuterClass"; + +message Trajectory { + // NEXT_ID: 8 + message Node { + // Index of this node within its trajectory. + int32 node_index = 7; + + int64 timestamp = 1; + + // Transform from tracking to global map frame. + transform.proto.Rigid3d pose = 5; + } + + message Submap { + // Index of this submap within its trajectory. + int32 submap_index = 2; + + // Transform from submap to global map frame. + transform.proto.Rigid3d pose = 1; + } + + // ID of this trajectory. + int32 trajectory_id = 3; + + // Time-ordered sequence of Nodes. + repeated Node node = 1; + + // Submaps associated with the trajectory. + repeated Submap submap = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory_builder_options.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory_builder_options.proto new file mode 100644 index 0000000..13f0344 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory_builder_options.proto @@ -0,0 +1,70 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/transform/proto/transform.proto"; +import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/local_trajectory_builder_options_2d.proto"; +import "cartographer/mapping/proto/local_trajectory_builder_options_3d.proto"; + +package cartographer.mapping.proto; + +message InitialTrajectoryPose { + transform.proto.Rigid3d relative_pose = 1; + int32 to_trajectory_id = 2; + int64 timestamp = 3; +} + +message TrajectoryBuilderOptions { + LocalTrajectoryBuilderOptions2D trajectory_builder_2d_options = 1; + LocalTrajectoryBuilderOptions3D trajectory_builder_3d_options = 2; + InitialTrajectoryPose initial_trajectory_pose = 4; + + reserved 5; + + bool pure_localization = 3 [deprecated = true]; + message PureLocalizationTrimmerOptions { + int32 max_submaps_to_keep = 1; + } + PureLocalizationTrimmerOptions pure_localization_trimmer = 6; + + bool collate_fixed_frame = 7; + bool collate_landmarks = 8; + + MotionFilterOptions pose_graph_odometry_motion_filter = 9; +} + +message SensorId { + enum SensorType { + RANGE = 0; + IMU = 1; + ODOMETRY = 2; + FIXED_FRAME_POSE = 3; + LANDMARK = 4; + LOCAL_SLAM_RESULT = 5; + } + + SensorType type = 1; + string id = 2; +} + +message TrajectoryBuilderOptionsWithSensorIds { + repeated SensorId sensor_id = 1; + TrajectoryBuilderOptions trajectory_builder_options = 2; +} + +message AllTrajectoryBuilderOptions { + repeated TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids = 1; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory_node_data.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory_node_data.proto new file mode 100644 index 0000000..d8cb0b1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/trajectory_node_data.proto @@ -0,0 +1,32 @@ +// Copyright 2017 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/transform.proto"; + +// Serialized state of a mapping::TrajectoryNode::Data. +message TrajectoryNodeData { + int64 timestamp = 1; + transform.proto.Quaterniond gravity_alignment = 2; + sensor.proto.CompressedPointCloud + filtered_gravity_aligned_point_cloud = 3; + sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4; + sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5; + repeated float rotational_scan_matcher_histogram = 6; + transform.proto.Rigid3d local_pose = 7; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/tsdf_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/tsdf_2d.proto new file mode 100644 index 0000000..bfbb44d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/tsdf_2d.proto @@ -0,0 +1,23 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message TSDF2D { + float truncation_distance = 1; + float max_weight = 2; + repeated int32 weight_cells = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto b/carto_ws/src/cartographer-master/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto new file mode 100644 index 0000000..d3ef1d3 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto @@ -0,0 +1,47 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/proto/normal_estimation_options_2d.proto"; + +message TSDFRangeDataInserterOptions2D { + // Distance to the surface within the signed distance function is evaluated. + double truncation_distance = 1; + // Maximum weight that can be stored in a cell. + double maximum_weight = 2; + + // Enables updating cells between the sensor origin and the range observation + // as free space. + bool update_free_space = 3; + + NormalEstimationOptions2D normal_estimation_options = 4; + + // Project the distance between the updated cell und the range observation to + // the estimated scan normal. + bool project_sdf_distance_to_scan_normal = 5; + + // Update weight is scaled with 1/distance(origin,hit)^range_exponent. + int32 update_weight_range_exponent = 6; + + // Kernel bandwidth of the weight factor based on the angle between. + // scan normal and ray + double update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 7; + + // Kernel bandwidth of the weight factor based on the distance between + // cell and scan observation. + double update_weight_distance_cell_to_hit_kernel_bandwidth = 8; +} diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/range_data_inserter_interface.cc b/carto_ws/src/cartographer-master/cartographer/mapping/range_data_inserter_interface.cc new file mode 100644 index 0000000..571987c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/range_data_inserter_interface.cc @@ -0,0 +1,49 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/range_data_inserter_interface.h" + +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" + +namespace cartographer { +namespace mapping { + +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::RangeDataInserterOptions options; + const std::string range_data_inserter_type_string = + parameter_dictionary->GetString("range_data_inserter_type"); + proto::RangeDataInserterOptions_RangeDataInserterType + range_data_inserter_type; + CHECK(proto::RangeDataInserterOptions_RangeDataInserterType_Parse( + range_data_inserter_type_string, &range_data_inserter_type)) + << "Unknown RangeDataInserterOptions_RangeDataInserterType kind: " + << range_data_inserter_type_string; + options.set_range_data_inserter_type(range_data_inserter_type); + *options.mutable_probability_grid_range_data_inserter_options_2d() = + CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary + ->GetDictionary("probability_grid_range_data_inserter") + .get()); + *options.mutable_tsdf_range_data_inserter_options_2d() = + CreateTSDFRangeDataInserterOptions2D( + parameter_dictionary->GetDictionary("tsdf_range_data_inserter") + .get()); + return options; +} +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/range_data_inserter_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/range_data_inserter_interface.h new file mode 100644 index 0000000..a3207af --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/range_data_inserter_interface.h @@ -0,0 +1,48 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_ +#define CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_ + +#include +#include + +#include "cartographer/mapping/grid_interface.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class RangeDataInserterInterface { + public: + virtual ~RangeDataInserterInterface() {} + + // Inserts 'range_data' into 'grid'. + virtual void Insert(const sensor::RangeData& range_data, + GridInterface* grid) const = 0; + virtual void Insert_new(const sensor::RangeData& range_data, + GridInterface* grid, + const transform::Rigid3d local_pose_) const = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/submaps.h b/carto_ws/src/cartographer-master/cartographer/mapping/submaps.h new file mode 100644 index 0000000..368feb4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/submaps.h @@ -0,0 +1,97 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_ +#define CARTOGRAPHER_MAPPING_SUBMAPS_H_ + +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/trajectory_node.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 a 'local_pose' in the local map frame, keeps +// track of how many range data were inserted into it, and sets +// 'insertion_finished' when the map no longer changes and is ready for loop +// closing. +class Submap { + public: + Submap(const transform::Rigid3d& local_submap_pose) + : local_pose_(local_submap_pose) {} + virtual ~Submap() {} + + virtual proto::Submap ToProto(bool include_grid_data) const = 0; + virtual void UpdateFromProto(const proto::Submap& proto) = 0; + + // Fills data into the 'response'. + virtual void ToResponseProto( + const transform::Rigid3d& global_submap_pose, + proto::SubmapQuery::Response* response) const = 0; + + // Pose of this submap in the local map frame. + + transform::Rigid3d local_pose() const { return local_pose_; } + + // Number of RangeData inserted. + int num_range_data() const { return num_range_data_; } + void set_num_range_data(const int num_range_data) { + num_range_data_ = num_range_data; + } + + bool insertion_finished() const { return insertion_finished_; } + void set_insertion_finished(bool insertion_finished) { + insertion_finished_ = insertion_finished; + } + + private: + const transform::Rigid3d local_pose_; + int num_range_data_ = 0; + bool insertion_finished_ = false; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_SUBMAPS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/submaps_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/submaps_test.cc new file mode 100644 index 0000000..dd2e1eb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/submaps_test.cc @@ -0,0 +1,42 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/submaps.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +// Converts the given log odds to a probability. This function is known to be +// very slow, because expf is incredibly slow. +inline float Expit(float log_odds) { + const float exp_log_odds = std::exp(log_odds); + return exp_log_odds / (1.f + exp_log_odds); +} + +TEST(SubmapsTest, LogOddsConversions) { + EXPECT_NEAR(Expit(Logit(kMinProbability)), kMinProbability, 1e-6); + EXPECT_NEAR(Expit(Logit(kMaxProbability)), kMaxProbability, 1e-6); + EXPECT_NEAR(Expit(Logit(0.5)), 0.5, 1e-6); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_builder_interface.cc b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_builder_interface.cc new file mode 100644 index 0000000..b81eed5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_builder_interface.cc @@ -0,0 +1,139 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_builder_interface.h" + +#include "cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h" +#include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" + +namespace cartographer { +namespace mapping { +namespace { + +void PopulatePureLocalizationTrimmerOptions( + proto::TrajectoryBuilderOptions* const trajectory_builder_options, + common::LuaParameterDictionary* const parameter_dictionary) { + constexpr char kDictionaryKey[] = "pure_localization_trimmer"; + if (!parameter_dictionary->HasKey(kDictionaryKey)) return; + + auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey); + auto* options = + trajectory_builder_options->mutable_pure_localization_trimmer(); + options->set_max_submaps_to_keep( + options_dictionary->GetInt("max_submaps_to_keep")); +} + +void PopulatePoseGraphOdometryMotionFilterOptions( + proto::TrajectoryBuilderOptions* const trajectory_builder_options, + common::LuaParameterDictionary* const parameter_dictionary) { + constexpr char kDictionaryKey[] = "pose_graph_odometry_motion_filter"; + if (!parameter_dictionary->HasKey(kDictionaryKey)) return; + + auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey); + auto* options = + trajectory_builder_options->mutable_pose_graph_odometry_motion_filter(); + options->set_max_time_seconds( + options_dictionary->GetDouble("max_time_seconds")); + options->set_max_distance_meters( + options_dictionary->GetDouble("max_distance_meters")); + options->set_max_angle_radians( + options_dictionary->GetDouble("max_angle_radians")); +} + +} // namespace + +proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::TrajectoryBuilderOptions options; + *options.mutable_trajectory_builder_2d_options() = + CreateLocalTrajectoryBuilderOptions2D( + parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); + *options.mutable_trajectory_builder_3d_options() = + CreateLocalTrajectoryBuilderOptions3D( + parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); + options.set_collate_fixed_frame( + parameter_dictionary->GetBool("collate_fixed_frame")); + options.set_collate_landmarks( + parameter_dictionary->GetBool("collate_landmarks")); + PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary); + PopulatePoseGraphOdometryMotionFilterOptions(&options, parameter_dictionary); + return options; +} + +proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId& sensor_id) { + proto::SensorId sensor_id_proto; + switch (sensor_id.type) { + case TrajectoryBuilderInterface::SensorId::SensorType::RANGE: + sensor_id_proto.set_type(proto::SensorId::RANGE); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::IMU: + sensor_id_proto.set_type(proto::SensorId::IMU); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY: + sensor_id_proto.set_type(proto::SensorId::ODOMETRY); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::FIXED_FRAME_POSE: + sensor_id_proto.set_type(proto::SensorId::FIXED_FRAME_POSE); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::LANDMARK: + sensor_id_proto.set_type(proto::SensorId::LANDMARK); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT: + sensor_id_proto.set_type(proto::SensorId::LOCAL_SLAM_RESULT); + break; + default: + LOG(FATAL) << "Unsupported sensor type."; + } + sensor_id_proto.set_id(sensor_id.id); + return sensor_id_proto; +} + +TrajectoryBuilderInterface::SensorId FromProto( + const proto::SensorId& sensor_id_proto) { + TrajectoryBuilderInterface::SensorId sensor_id; + switch (sensor_id_proto.type()) { + case proto::SensorId::RANGE: + sensor_id.type = TrajectoryBuilderInterface::SensorId::SensorType::RANGE; + break; + case proto::SensorId::IMU: + sensor_id.type = TrajectoryBuilderInterface::SensorId::SensorType::IMU; + break; + case proto::SensorId::ODOMETRY: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY; + break; + case proto::SensorId::FIXED_FRAME_POSE: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::FIXED_FRAME_POSE; + break; + case proto::SensorId::LANDMARK: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::LANDMARK; + break; + case proto::SensorId::LOCAL_SLAM_RESULT: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT; + break; + default: + LOG(FATAL) << "Unsupported sensor type."; + } + sensor_id.id = sensor_id_proto.id(); + return sensor_id; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_builder_interface.h b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_builder_interface.h new file mode 100644 index 0000000..1173d8e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_builder_interface.h @@ -0,0 +1,122 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_ + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" + +namespace cartographer { +namespace mapping { + +proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class LocalSlamResultData; + +// 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 TrajectoryBuilderInterface { + public: + struct InsertionResult { + NodeId node_id; + std::shared_ptr constant_data; + std::vector> insertion_submaps; + }; + + // A callback which is called after local SLAM processes an accumulated + // 'sensor::RangeData'. If the data was inserted into a submap, reports the + // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out. + using LocalSlamResultCallback = + std::function)>; + + struct SensorId { + enum class SensorType { + RANGE = 0, + IMU, + ODOMETRY, + FIXED_FRAME_POSE, + LANDMARK, + LOCAL_SLAM_RESULT + }; + + SensorType type; + std::string id; + + bool operator==(const SensorId& other) const { + return std::forward_as_tuple(type, id) == + std::forward_as_tuple(other.type, other.id); + } + + bool operator<(const SensorId& other) const { + return std::forward_as_tuple(type, id) < + std::forward_as_tuple(other.type, other.id); + } + }; + + TrajectoryBuilderInterface() {} + virtual ~TrajectoryBuilderInterface() {} + + TrajectoryBuilderInterface(const TrajectoryBuilderInterface&) = delete; + TrajectoryBuilderInterface& operator=(const TrajectoryBuilderInterface&) = + delete; + + virtual void AddSensorData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data) = 0; + virtual void AddSensorData(const std::string& sensor_id, + const sensor::ImuData& imu_data) = 0; + virtual void AddSensorData(const std::string& sensor_id, + const sensor::OdometryData& odometry_data) = 0; + virtual void AddSensorData( + const std::string& sensor_id, + const sensor::FixedFramePoseData& fixed_frame_pose) = 0; + virtual void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) = 0; + // Allows to directly add local SLAM results to the 'PoseGraph'. Note that it + // is invalid to add local SLAM results for a trajectory that has a + // 'LocalTrajectoryBuilder2D/3D'. + virtual void AddLocalSlamResultData( + std::unique_ptr local_slam_result_data) = 0; +}; + +proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId& sensor_id); +TrajectoryBuilderInterface::SensorId FromProto( + const proto::SensorId& sensor_id_proto); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node.cc b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node.cc new file mode 100644 index 0000000..368de8a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node.cc @@ -0,0 +1,72 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_node.h" + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) { + proto::TrajectoryNodeData proto; + proto.set_timestamp(common::ToUniversal(constant_data.time)); + *proto.mutable_gravity_alignment() = + transform::ToProto(constant_data.gravity_alignment); + *proto.mutable_filtered_gravity_aligned_point_cloud() = + sensor::CompressedPointCloud( + constant_data.filtered_gravity_aligned_point_cloud) + .ToProto(); + *proto.mutable_high_resolution_point_cloud() = + sensor::CompressedPointCloud(constant_data.high_resolution_point_cloud) + .ToProto(); + *proto.mutable_low_resolution_point_cloud() = + sensor::CompressedPointCloud(constant_data.low_resolution_point_cloud) + .ToProto(); + for (Eigen::VectorXf::Index i = 0; + i != constant_data.rotational_scan_matcher_histogram.size(); ++i) { + proto.add_rotational_scan_matcher_histogram( + constant_data.rotational_scan_matcher_histogram(i)); + } + *proto.mutable_local_pose() = transform::ToProto(constant_data.local_pose); + return proto; +} + +TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) { + Eigen::VectorXf rotational_scan_matcher_histogram( + proto.rotational_scan_matcher_histogram_size()); + for (int i = 0; i != proto.rotational_scan_matcher_histogram_size(); ++i) { + rotational_scan_matcher_histogram(i) = + proto.rotational_scan_matcher_histogram(i); + } + return TrajectoryNode::Data{ + common::FromUniversal(proto.timestamp()), + transform::ToEigen(proto.gravity_alignment()), + sensor::CompressedPointCloud(proto.filtered_gravity_aligned_point_cloud()) + .Decompress(), + sensor::CompressedPointCloud(proto.high_resolution_point_cloud()) + .Decompress(), + sensor::CompressedPointCloud(proto.low_resolution_point_cloud()) + .Decompress(), + rotational_scan_matcher_histogram, + transform::ToRigid3(proto.local_pose())}; +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node.h b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node.h new file mode 100644 index 0000000..9238cfc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node.h @@ -0,0 +1,81 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ +#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ + +#include +#include + +#include "Eigen/Core" +#include "absl/types/optional.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory_node_data.pb.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping { + +struct TrajectoryNodePose { + struct ConstantPoseData { + common::Time time; + transform::Rigid3d local_pose; + }; + // The node pose in the global SLAM frame. + transform::Rigid3d global_pose; + + absl::optional constant_pose_data; +}; + +struct TrajectoryNode { + struct Data { + common::Time time; + + // Transform to approximately gravity align the tracking frame as + // determined by local SLAM. + Eigen::Quaterniond gravity_alignment; + + // Used for loop closure in 2D: voxel filtered returns in the + // 'gravity_alignment' frame. + sensor::PointCloud filtered_gravity_aligned_point_cloud; + + // Used for loop closure in 3D. + sensor::PointCloud high_resolution_point_cloud; + sensor::PointCloud low_resolution_point_cloud; + Eigen::VectorXf rotational_scan_matcher_histogram; + + // The node pose in the local SLAM frame. + transform::Rigid3d local_pose; + }; + + common::Time time() const { return constant_data->time; } + + // This must be a shared_ptr. If the data is used for visualization while the + // node is being trimmed, it must survive until all use finishes. + std::shared_ptr constant_data; + + // The node pose in the global SLAM frame. + transform::Rigid3d global_pose; +}; + +proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data); +TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node_test.cc new file mode 100644 index 0000000..c927a8d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/trajectory_node_test.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_node.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory_node_data.pb.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(TrajectoryNodeTest, ToAndFromProto) { + const TrajectoryNode::Data expected{ + common::FromUniversal(42), + Eigen::Quaterniond(1., 2., -3., -4.), + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{1.f, 2.f, 0.f}}, + {Eigen::Vector3f{0.f, 0.f, 1.f}}})) + .Decompress(), + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}})) + .Decompress(), + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}})) + .Decompress(), + Eigen::VectorXf::Unit(20, 4), + transform::Rigid3d({1., 2., 3.}, + Eigen::Quaterniond(4., 5., -6., -7.).normalized())}; + const proto::TrajectoryNodeData proto = ToProto(expected); + const TrajectoryNode::Data actual = FromProto(proto); + EXPECT_EQ(expected.time, actual.time); + EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment)); + EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud.points(), + actual.filtered_gravity_aligned_point_cloud.points()); + EXPECT_EQ(expected.high_resolution_point_cloud.points(), + actual.high_resolution_point_cloud.points()); + EXPECT_EQ(expected.low_resolution_point_cloud.points(), + actual.low_resolution_point_cloud.points()); + EXPECT_EQ(expected.rotational_scan_matcher_histogram, + actual.rotational_scan_matcher_histogram); + EXPECT_THAT(actual.local_pose, + transform::IsNearly(expected.local_pose, 1e-9)); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables.cc b/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables.cc new file mode 100644 index 0000000..9800015 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables.cc @@ -0,0 +1,70 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/value_conversion_tables.h" + +#include "absl/memory/memory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace { + +constexpr uint16 kUpdateMarker = 1u << 15; + +// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound]. +float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value, + const float unknown_result, + const float lower_bound, + const float upper_bound) { + CHECK_LE(value, 32767); + if (value == unknown_value) return unknown_result; + const float kScale = (upper_bound - lower_bound) / 32766.f; + return value * kScale + (lower_bound - kScale); +} + +std::unique_ptr> PrecomputeValueToBoundedFloat( + const uint16 unknown_value, const float unknown_result, + const float lower_bound, const float upper_bound) { + auto result = absl::make_unique>(); + size_t num_values = std::numeric_limits::max() + 1; + result->reserve(num_values); + for (size_t value = 0; value != num_values; ++value) { + result->push_back(SlowValueToBoundedFloat( + static_cast(value) & ~kUpdateMarker, unknown_value, + unknown_result, lower_bound, upper_bound)); + } + return result; +} +} // namespace + +const std::vector* ValueConversionTables::GetConversionTable( + float unknown_result, float lower_bound, float upper_bound) { + std::tuple bounds = + std::make_tuple(unknown_result, lower_bound, upper_bound); + auto lookup_table_iterator = bounds_to_lookup_table_.find(bounds); + if (lookup_table_iterator == bounds_to_lookup_table_.end()) { + auto insertion_result = bounds_to_lookup_table_.emplace( + bounds, PrecomputeValueToBoundedFloat(0, unknown_result, lower_bound, + upper_bound)); + return insertion_result.first->second.get(); + } else { + return lookup_table_iterator->second.get(); + } +} + +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables.h b/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables.h new file mode 100644 index 0000000..56924f0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables.h @@ -0,0 +1,48 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_VALUE_CONVERSION_TABLES_H_ +#define CARTOGRAPHER_MAPPING_VALUE_CONVERSION_TABLES_H_ + +#include +#include + +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +// Performs lazy computations of lookup tables for mapping from a uint16 value +// to a float in ['lower_bound', 'upper_bound']. The first element of the table +// is set to 'unknown_result'. +class ValueConversionTables { + public: + const std::vector* GetConversionTable(float unknown_result, + float lower_bound, + float upper_bound); + + private: + std::map, + std::unique_ptr>> + bounds_to_lookup_table_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_VALUE_CONVERSION_TABLES_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables_test.cc b/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables_test.cc new file mode 100644 index 0000000..3009ecd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/mapping/value_conversion_tables_test.cc @@ -0,0 +1,68 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/value_conversion_tables.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(ValueConversionTablesTest, EqualTables) { + ValueConversionTables value_conversion_tables; + const std::vector* reference_table = + value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f); + const std::vector* test_table = + value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f); + EXPECT_EQ(reference_table, test_table); +} + +TEST(ValueConversionTablesTest, InequalTables) { + ValueConversionTables value_conversion_tables; + const std::vector* reference_table = + value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f); + const std::vector* test_table = + value_conversion_tables.GetConversionTable(0.1f, 0.4f, 0.5f); + EXPECT_FALSE(reference_table == test_table); +} + +TEST(ValueConversionTablesTest, ValueConversion) { + ValueConversionTables value_conversion_tables; + std::mt19937 rng(42); + std::uniform_real_distribution bound_distribution(-10.f, 10.f); + for (size_t sample_index = 0; sample_index < 100; ++sample_index) { + const float bound_sample_0 = bound_distribution(rng); + const float bound_sample_1 = bound_distribution(rng); + const float lower_bound = std::min(bound_sample_0, bound_sample_1); + const float upper_bound = std::max(bound_sample_0, bound_sample_1); + const float undefined_value = bound_distribution(rng); + const std::vector* conversion_table = + value_conversion_tables.GetConversionTable(undefined_value, lower_bound, + upper_bound); + EXPECT_EQ((*conversion_table)[0], undefined_value); + const float scale = (upper_bound - lower_bound) / 32766.f; + for (uint16 i = 1; i < 32768; ++i) { + EXPECT_EQ((*conversion_table)[i], i * scale + (lower_bound - scale)); + } + } +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/counter.cc b/carto_ws/src/cartographer-master/cartographer/metrics/counter.cc new file mode 100644 index 0000000..8fae37d --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/counter.cc @@ -0,0 +1,39 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/metrics/counter.h" + +namespace cartographer { +namespace metrics { + +namespace { + +// Implementation of counter that does nothing. +class NullCounter : public Counter { + public: + void Increment() override{}; + void Increment(double) override{}; +}; + +} // namespace + +Counter* Counter::Null() { + static NullCounter null_counter; + return &null_counter; +} + +} // namespace metrics +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/counter.h b/carto_ws/src/cartographer-master/cartographer/metrics/counter.h new file mode 100644 index 0000000..781d5e4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/counter.h @@ -0,0 +1,39 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_METRICS_COUNTER_H_ +#define CARTOGRAPHER_METRICS_COUNTER_H_ + +#include +#include + +namespace cartographer { +namespace metrics { + +class Counter { + public: + // Counter instance that does nothing. Safe for use in static initializers. + static Counter* Null(); + + virtual ~Counter() = default; + virtual void Increment() = 0; + virtual void Increment(double by_value) = 0; +}; + +} // namespace metrics +} // namespace cartographer + +#endif // CARTOGRAPHER_METRICS_COUNTER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/family_factory.h b/carto_ws/src/cartographer-master/cartographer/metrics/family_factory.h new file mode 100644 index 0000000..0db3492 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/family_factory.h @@ -0,0 +1,71 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_METRICS_FAMILY_FACTORY_H_ +#define CARTOGRAPHER_METRICS_FAMILY_FACTORY_H_ + +#include +#include + +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer/metrics/histogram.h" + +namespace cartographer { +namespace metrics { + +template +class NullFamily; + +template +class Family { + public: // Family instance that does nothing. Safe for use in static + // initializers. + static Family* Null() { + static NullFamily null_family; + return &null_family; + } + + virtual ~Family() = default; + + virtual MetricType* Add(const std::map& labels) = 0; +}; + +template +class NullFamily : public Family { + public: + MetricType* Add(const std::map& labels) override { + return MetricType::Null(); + } +}; + +class FamilyFactory { + public: + virtual ~FamilyFactory() = default; + + virtual Family* NewCounterFamily(const std::string& name, + const std::string& description) = 0; + virtual Family* NewGaugeFamily(const std::string& name, + const std::string& description) = 0; + virtual Family* NewHistogramFamily( + const std::string& name, const std::string& description, + const Histogram::BucketBoundaries& boundaries) = 0; +}; + +} // namespace metrics +} // namespace cartographer + +#endif // CARTOGRAPHER_METRICS_FAMILY_FACTORY_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/gauge.cc b/carto_ws/src/cartographer-master/cartographer/metrics/gauge.cc new file mode 100644 index 0000000..6af2b0a --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/gauge.cc @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/metrics/gauge.h" + +namespace cartographer { +namespace metrics { + +namespace { + +// Implementation of gauge that does nothing. +class NullGauge : public Gauge { + public: + void Increment() override{}; + void Increment(double) override{}; + void Decrement() override{}; + void Decrement(double) override{}; + void Set(double) override{}; +}; + +} // namespace + +Gauge* Gauge::Null() { + static NullGauge null_gauge; + return &null_gauge; +} + +} // namespace metrics +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/gauge.h b/carto_ws/src/cartographer-master/cartographer/metrics/gauge.h new file mode 100644 index 0000000..0b3c225 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/gauge.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_METRICS_GAUGE_H_ +#define CARTOGRAPHER_METRICS_GAUGE_H_ + +#include +#include + +namespace cartographer { +namespace metrics { + +class Gauge { + public: + // Gauge instance that does nothing. Safe for use in static initializers. + static Gauge* Null(); + + virtual ~Gauge() = default; + virtual void Increment() = 0; + virtual void Increment(double by_value) = 0; + virtual void Decrement() = 0; + virtual void Decrement(double by_value) = 0; + virtual void Set(double value) = 0; +}; + +} // namespace metrics +} // namespace cartographer + +#endif // CARTOGRAPHER_METRICS_GAUGE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/histogram.cc b/carto_ws/src/cartographer-master/cartographer/metrics/histogram.cc new file mode 100644 index 0000000..d7d0cf2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/histogram.cc @@ -0,0 +1,65 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/metrics/histogram.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace metrics { + +namespace { + +// Implementation of histogram that does nothing. +class NullHistogram : public Histogram { + public: + void Observe(double) override {} +}; + +} // namespace + +Histogram* Histogram::Null() { + static NullHistogram null_histogram; + return &null_histogram; +} + +Histogram::BucketBoundaries Histogram::FixedWidth(double width, + int num_finite_buckets) { + BucketBoundaries result; + double boundary = 0; + for (int i = 0; i < num_finite_buckets; ++i) { + boundary += width; + result.push_back(boundary); + } + return result; +} + +Histogram::BucketBoundaries Histogram::ScaledPowersOf(double base, + double scale_factor, + double max_value) { + CHECK_GT(base, 1); + CHECK_GT(scale_factor, 0); + BucketBoundaries result; + double boundary = scale_factor; + while (boundary < max_value) { + result.push_back(boundary); + boundary *= base; + } + return result; +} + +} // namespace metrics +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/histogram.h b/carto_ws/src/cartographer-master/cartographer/metrics/histogram.h new file mode 100644 index 0000000..51978f8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/histogram.h @@ -0,0 +1,44 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_METRICS_HISTOGRAM_H_ +#define CARTOGRAPHER_METRICS_HISTOGRAM_H_ + +#include +#include + +namespace cartographer { +namespace metrics { + +class Histogram { + public: + using BucketBoundaries = std::vector; + + // Histogram instance that does nothing. Safe for use in static initializers. + static Histogram* Null(); + + static BucketBoundaries FixedWidth(double width, int num_finite_buckets); + static BucketBoundaries ScaledPowersOf(double base, double scale_factor, + double max_value); + + virtual ~Histogram() = default; + virtual void Observe(double value) = 0; +}; + +} // namespace metrics +} // namespace cartographer + +#endif // CARTOGRAPHER_METRICS_HISTOGRAM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/register.cc b/carto_ws/src/cartographer-master/cartographer/metrics/register.cc new file mode 100644 index 0000000..d1c84e1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/register.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/metrics/register.h" + +#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" +#include "cartographer/mapping/internal/2d/pose_graph_2d.h" +#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" +#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" +#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" +#include "cartographer/mapping/internal/global_trajectory_builder.h" +#include "cartographer/sensor/internal/trajectory_collator.h" + +namespace cartographer { +namespace metrics { + +void RegisterAllMetrics(FamilyFactory* registry) { + mapping::constraints::ConstraintBuilder2D::RegisterMetrics(registry); + mapping::constraints::ConstraintBuilder3D::RegisterMetrics(registry); + mapping::GlobalTrajectoryBuilderRegisterMetrics(registry); + mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry); + mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry); + mapping::PoseGraph2D::RegisterMetrics(registry); + mapping::PoseGraph3D::RegisterMetrics(registry); + sensor::TrajectoryCollator::RegisterMetrics(registry); +} + +} // namespace metrics +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/metrics/register.h b/carto_ws/src/cartographer-master/cartographer/metrics/register.h new file mode 100644 index 0000000..565da23 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/metrics/register.h @@ -0,0 +1,30 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_METRICS_REGISTER_H_ +#define CARTOGRAPHER_METRICS_REGISTER_H_ + +#include "cartographer/metrics/family_factory.h" + +namespace cartographer { +namespace metrics { + +void RegisterAllMetrics(FamilyFactory *registry); + +} // namespace metrics +} // namespace cartographer + +#endif // CARTOGRAPHER_METRICS_REGISTER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/collator_interface.h b/carto_ws/src/cartographer-master/cartographer/sensor/collator_interface.h new file mode 100644 index 0000000..91d7c83 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/collator_interface.h @@ -0,0 +1,70 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_COLLATOR_INTERFACE_H_ +#define CARTOGRAPHER_SENSOR_COLLATOR_INTERFACE_H_ + +#include +#include +#include + +#include "absl/container/flat_hash_set.h" +#include "absl/types/optional.h" +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace sensor { + +class CollatorInterface { + public: + using Callback = + std::function)>; + + CollatorInterface() {} + virtual ~CollatorInterface() {} + CollatorInterface(const CollatorInterface&) = delete; + CollatorInterface& operator=(const CollatorInterface&) = delete; + + // Adds a trajectory to produce sorted sensor output for. Calls 'callback' + // for each collated sensor data. + virtual void AddTrajectory( + int trajectory_id, + const absl::flat_hash_set& expected_sensor_ids, + const Callback& callback) = 0; + + // Marks 'trajectory_id' as finished. + virtual void FinishTrajectory(int trajectory_id) = 0; + + // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid + // sensor data. Sensor packets with matching 'data.sensor_id_' must be added + // in time order. + virtual void AddSensorData(int trajectory_id, std::unique_ptr data) = 0; + + // Dispatches all queued sensor packets. May only be called once. + // AddSensorData may not be called after Flush. + virtual void Flush() = 0; + + // Must only be called if at least one unfinished trajectory exists. Returns + // the ID of the trajectory that needs more data before CollatorInterface is + // unblocked. Returns 'nullopt' for implementations that do not wait for a + // particular trajectory. + virtual absl::optional GetBlockingTrajectoryId() const = 0; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_COLLATOR_INTERFACE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud.cc b/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud.cc new file mode 100644 index 0000000..f5337e5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud.cc @@ -0,0 +1,195 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/compressed_point_cloud.h" + +#include + +#include "cartographer/common/math.h" +#include "cartographer/mapping/3d/hybrid_grid.h" + +namespace cartographer { +namespace sensor { + +namespace { + +// Points are encoded on a fixed grid with a grid spacing of 'kPrecision' with +// integers. Points are organized in blocks, where each point is encoded +// relative to the block's origin in an int32 with 'kBitsPerCoordinate' bits per +// coordinate. +constexpr float kPrecision = 0.001f; // in meters. +constexpr int kBitsPerCoordinate = 10; +constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1; +constexpr int kMaxBitsPerDirection = 23; + +} // namespace + +CompressedPointCloud::ConstIterator::ConstIterator( + const CompressedPointCloud* compressed_point_cloud) + : compressed_point_cloud_(compressed_point_cloud), + remaining_points_(compressed_point_cloud->num_points_), + remaining_points_in_current_block_(0), + input_(compressed_point_cloud->point_data_.begin()) { + if (remaining_points_ > 0) { + ReadNextPoint(); + } +} + +CompressedPointCloud::ConstIterator +CompressedPointCloud::ConstIterator::EndIterator( + const CompressedPointCloud* compressed_point_cloud) { + ConstIterator end_iterator(compressed_point_cloud); + end_iterator.remaining_points_ = 0; + return end_iterator; +} + +RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const { + CHECK_GT(remaining_points_, 0); + return {current_point_}; +} + +CompressedPointCloud::ConstIterator& +CompressedPointCloud::ConstIterator::operator++() { + --remaining_points_; + if (remaining_points_ > 0) { + ReadNextPoint(); + } + return *this; +} + +bool CompressedPointCloud::ConstIterator::operator!=( + const ConstIterator& it) const { + CHECK(compressed_point_cloud_ == it.compressed_point_cloud_); + return remaining_points_ != it.remaining_points_; +} + +void CompressedPointCloud::ConstIterator::ReadNextPoint() { + if (remaining_points_in_current_block_ == 0) { + remaining_points_in_current_block_ = *input_++; + for (int i = 0; i < 3; ++i) { + current_block_coordinates_[i] = *input_++ << kBitsPerCoordinate; + } + } + --remaining_points_in_current_block_; + const int point = *input_++; + constexpr int kMask = (1 << kBitsPerCoordinate) - 1; + current_point_[0] = + (current_block_coordinates_[0] + (point & kMask)) * kPrecision; + current_point_[1] = (current_block_coordinates_[1] + + ((point >> kBitsPerCoordinate) & kMask)) * + kPrecision; + current_point_[2] = + (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) * + kPrecision; +} + +CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) + : num_points_(point_cloud.size()) { + // Distribute points into blocks. + struct RasterPoint { + Eigen::Array3i point; + int index; + }; + using Blocks = mapping::HybridGridBase>; + Blocks blocks(kPrecision); + int num_blocks = 0; + CHECK_LE(point_cloud.size(), std::numeric_limits::max()); + for (int point_index = 0; point_index < static_cast(point_cloud.size()); + ++point_index) { + const RangefinderPoint& point = point_cloud[point_index]; + CHECK_LT(point.position.cwiseAbs().maxCoeff() / kPrecision, + 1 << kMaxBitsPerDirection) + << "Point out of bounds: " << point.position; + Eigen::Array3i raster_point; + Eigen::Array3i block_coordinate; + for (int i = 0; i < 3; ++i) { + raster_point[i] = common::RoundToInt(point.position[i] / kPrecision); + block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate; + raster_point[i] &= kCoordinateMask; + } + auto* const block = blocks.mutable_value(block_coordinate); + num_blocks += block->empty(); + block->push_back({raster_point, point_index}); + } + + // Encode blocks. + point_data_.reserve(4 * num_blocks + point_cloud.size()); + for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) { + const auto& raster_points = it.GetValue(); + CHECK_LE(raster_points.size(), std::numeric_limits::max()); + point_data_.push_back(raster_points.size()); + const Eigen::Array3i block_coordinate = it.GetCellIndex(); + point_data_.push_back(block_coordinate.x()); + point_data_.push_back(block_coordinate.y()); + point_data_.push_back(block_coordinate.z()); + for (const RasterPoint& raster_point : raster_points) { + point_data_.push_back((((raster_point.point.z() << kBitsPerCoordinate) + + raster_point.point.y()) + << kBitsPerCoordinate) + + raster_point.point.x()); + } + } + CHECK_EQ(num_blocks, 0); +} + +CompressedPointCloud::CompressedPointCloud( + const proto::CompressedPointCloud& proto) { + num_points_ = proto.num_points(); + const int data_size = proto.point_data_size(); + point_data_.reserve(data_size); + // TODO(wohe): Verify that 'point_data_' does not contain malformed data. + for (int i = 0; i != data_size; ++i) { + point_data_.emplace_back(proto.point_data(i)); + } +} + +bool CompressedPointCloud::empty() const { return num_points_ == 0; } + +size_t CompressedPointCloud::size() const { return num_points_; } + +CompressedPointCloud::ConstIterator CompressedPointCloud::begin() const { + return ConstIterator(this); +} + +CompressedPointCloud::ConstIterator CompressedPointCloud::end() const { + return ConstIterator::EndIterator(this); +} + +PointCloud CompressedPointCloud::Decompress() const { + PointCloud decompressed; + for (const RangefinderPoint& point : *this) { + decompressed.push_back(point); + } + return decompressed; +} + +bool CompressedPointCloud::operator==( + const CompressedPointCloud& right_hand_container) const { + return point_data_ == right_hand_container.point_data_ && + num_points_ == right_hand_container.num_points_; +} + +proto::CompressedPointCloud CompressedPointCloud::ToProto() const { + proto::CompressedPointCloud result; + result.set_num_points(num_points_); + for (const int32 data : point_data_) { + result.add_point_data(data); + } + return result; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud.h b/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud.h new file mode 100644 index 0000000..6cb01cc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud.h @@ -0,0 +1,96 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ +#define CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/proto/sensor.pb.h" + +namespace cartographer { +namespace sensor { + +// A compressed representation of a point cloud consisting of a collection of +// points (Vector3f) without time information. +// Internally, points are grouped by blocks. Each block encodes a bit of meta +// data (number of points in block, coordinates of the block) and encodes each +// point with a fixed bit rate in relation to the block. +class CompressedPointCloud { + public: + class ConstIterator; + + CompressedPointCloud() : num_points_(0) {} + explicit CompressedPointCloud(const PointCloud& point_cloud); + explicit CompressedPointCloud(const proto::CompressedPointCloud& proto); + + // Returns decompressed point cloud. + PointCloud Decompress() const; + + bool empty() const; + size_t size() const; + ConstIterator begin() const; + ConstIterator end() const; + + bool operator==(const CompressedPointCloud& right_hand_container) const; + proto::CompressedPointCloud ToProto() const; + + private: + std::vector point_data_; + size_t num_points_; +}; + +// Forward iterator for compressed point clouds. +class CompressedPointCloud::ConstIterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = RangefinderPoint; + using difference_type = int64; + using pointer = const RangefinderPoint*; + using reference = const RangefinderPoint&; + + // Creates begin iterator. + explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud); + + // Creates end iterator. + static ConstIterator EndIterator( + const CompressedPointCloud* compressed_point_cloud); + + RangefinderPoint operator*() const; + ConstIterator& operator++(); + bool operator!=(const ConstIterator& it) const; + + private: + // Reads next point from buffer. Also handles reading the meta data of the + // next block, if the current block is depleted. + void ReadNextPoint(); + + const CompressedPointCloud* compressed_point_cloud_; + size_t remaining_points_; + int32 remaining_points_in_current_block_; + Eigen::Vector3f current_point_; + Eigen::Vector3i current_block_coordinates_; + std::vector::const_iterator input_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud_test.cc new file mode 100644 index 0000000..7dcd6bc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/compressed_point_cloud_test.cc @@ -0,0 +1,122 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/compressed_point_cloud.h" + +#include "gmock/gmock.h" + +namespace Eigen { + +// Prints Vector3f in a readable format in matcher ApproximatelyEquals when +// failing a test. Without this function, the output is formated as hexadecimal +// 8 bit numbers. +void PrintTo(const Vector3f& x, std::ostream* os) { + *os << "(" << x[0] << ", " << x[1] << ", " << x[2] << ")"; +} + +} // namespace Eigen + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::Contains; +using ::testing::FloatNear; +using ::testing::PrintToString; + +constexpr float kPrecision = 0.001f; + +// Matcher for 3-d vectors w.r.t. to the target precision. +MATCHER_P(ApproximatelyEquals, expected, + std::string("is equal to ") + PrintToString(expected)) { + return (arg.position - expected).isZero(kPrecision); +} + +// Helper function to test the mapping of a single point. Includes test for +// recompressing the same point again. +void TestPoint(const Eigen::Vector3f& p) { + CompressedPointCloud compressed(PointCloud({{p}})); + EXPECT_EQ(1, compressed.size()); + EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p)); + CompressedPointCloud recompressed(PointCloud({*compressed.begin()})); + EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p)); +} + +TEST(CompressPointCloudTest, CompressesPointsCorrectly) { + TestPoint(Eigen::Vector3f(8000.f, 7500.f, 5000.f)); + TestPoint(Eigen::Vector3f(1000.f, 2000.f, 3000.f)); + TestPoint(Eigen::Vector3f(100.f, 200.f, 300.f)); + TestPoint(Eigen::Vector3f(10.f, 20.f, 30.f)); + TestPoint(Eigen::Vector3f(-0.00049f, -0.0005f, -0.0015f)); + TestPoint(Eigen::Vector3f(0.05119f, 0.0512f, 0.05121)); + TestPoint(Eigen::Vector3f(-0.05119f, -0.0512f, -0.05121)); + TestPoint(Eigen::Vector3f(0.8405f, 0.84f, 0.8396f)); + TestPoint(Eigen::Vector3f(0.8395f, 0.8394f, 0.8393f)); + TestPoint(Eigen::Vector3f(0.839f, 0.8391f, 0.8392f)); + TestPoint(Eigen::Vector3f(0.8389f, 0.8388f, 0.83985f)); +} + +TEST(CompressPointCloudTest, Compresses) { + const CompressedPointCloud compressed( + PointCloud({{Eigen::Vector3f(0.838f, 0, 0)}, + {Eigen::Vector3f(0.839f, 0, 0)}, + {Eigen::Vector3f(0.840f, 0, 0)}})); + EXPECT_FALSE(compressed.empty()); + EXPECT_EQ(3, compressed.size()); + const PointCloud decompressed = compressed.Decompress(); + EXPECT_EQ(3, decompressed.size()); + EXPECT_THAT(decompressed.points(), + Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0)))); + EXPECT_THAT(decompressed.points(), + Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0)))); + EXPECT_THAT(decompressed.points(), + Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0)))); +} + +TEST(CompressPointCloudTest, CompressesEmptyPointCloud) { + CompressedPointCloud compressed; + EXPECT_TRUE(compressed.empty()); + EXPECT_EQ(0, compressed.size()); +} + +// Test for gaps. +// Produces a series of points densly packed along the x axis, compresses these +// points (twice), and tests, whether there are gaps between two consecutive +// points. +TEST(CompressPointCloudTest, CompressesNoGaps) { + PointCloud point_cloud; + for (int i = 0; i < 3000; ++i) { + point_cloud.push_back({Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)}); + } + const CompressedPointCloud compressed(point_cloud); + const PointCloud decompressed = compressed.Decompress(); + const CompressedPointCloud recompressed(decompressed); + EXPECT_EQ(decompressed.size(), recompressed.size()); + + std::vector x_coord; + for (const RangefinderPoint& p : compressed) { + x_coord.push_back(p.position[0]); + } + std::sort(x_coord.begin(), x_coord.end()); + for (size_t i = 1; i < x_coord.size(); ++i) { + EXPECT_THAT(std::abs(x_coord[i] - x_coord[i - 1]), + FloatNear(kPrecision, 1e-7f)); + } +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/data.h b/carto_ws/src/cartographer-master/cartographer/sensor/data.h new file mode 100644 index 0000000..0bedbd2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/data.h @@ -0,0 +1,49 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_DATA_H_ +#define CARTOGRAPHER_MAPPING_DATA_H_ + +#include "absl/memory/memory.h" +#include "cartographer/common/time.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { + +namespace mapping { +class TrajectoryBuilderInterface; +} + +namespace sensor { + +class Data { + public: + explicit Data(const std::string &sensor_id) : sensor_id_(sensor_id) {} + virtual ~Data() {} + + virtual common::Time GetTime() const = 0; + const std::string &GetSensorId() const { return sensor_id_; } + virtual void AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *trajectory_builder) = 0; + + protected: + const std::string sensor_id_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/fixed_frame_pose_data.cc b/carto_ws/src/cartographer-master/cartographer/sensor/fixed_frame_pose_data.cc new file mode 100644 index 0000000..352f7e9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/fixed_frame_pose_data.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/fixed_frame_pose_data.h" + +#include "absl/types/optional.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data) { + proto::FixedFramePoseData proto; + proto.set_timestamp(common::ToUniversal(pose_data.time)); + if (pose_data.pose.has_value()) { + *proto.mutable_pose() = transform::ToProto(pose_data.pose.value()); + } + return proto; +} + +FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto) { + return FixedFramePoseData{common::FromUniversal(proto.timestamp()), + proto.has_pose() + ? absl::optional( + transform::ToRigid3(proto.pose())) + : absl::optional()}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/fixed_frame_pose_data.h b/carto_ws/src/cartographer-master/cartographer/sensor/fixed_frame_pose_data.h new file mode 100644 index 0000000..1eabb58 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/fixed_frame_pose_data.h @@ -0,0 +1,46 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_ +#define CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_ + +#include + +#include "absl/types/optional.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace sensor { + +// The fixed frame pose data (like GPS, pose, etc.) will be used in the +// optimization. +struct FixedFramePoseData { + common::Time time; + absl::optional pose; +}; + +// Converts 'pose_data' to a proto::FixedFramePoseData. +proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data); + +// Converts 'proto' to an FixedFramePoseData. +FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/imu_data.cc b/carto_ws/src/cartographer-master/cartographer/sensor/imu_data.cc new file mode 100644 index 0000000..896e2f8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/imu_data.cc @@ -0,0 +1,41 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/imu_data.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::ImuData ToProto(const ImuData& imu_data) { + proto::ImuData proto; + proto.set_timestamp(common::ToUniversal(imu_data.time)); + *proto.mutable_linear_acceleration() = + transform::ToProto(imu_data.linear_acceleration); + *proto.mutable_angular_velocity() = + transform::ToProto(imu_data.angular_velocity); + return proto; +} + +ImuData FromProto(const proto::ImuData& proto) { + return ImuData{common::FromUniversal(proto.timestamp()), + transform::ToEigen(proto.linear_acceleration()), + transform::ToEigen(proto.angular_velocity())}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/imu_data.h b/carto_ws/src/cartographer-master/cartographer/sensor/imu_data.h new file mode 100644 index 0000000..18f1b6e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/imu_data.h @@ -0,0 +1,42 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_IMU_DATA_H_ +#define CARTOGRAPHER_SENSOR_IMU_DATA_H_ + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" + +namespace cartographer { +namespace sensor { + +struct ImuData { + common::Time time; + Eigen::Vector3d linear_acceleration; + Eigen::Vector3d angular_velocity; +}; + +// Converts 'imu_data' to a proto::ImuData. +proto::ImuData ToProto(const ImuData& imu_data); + +// Converts 'proto' to an ImuData. +ImuData FromProto(const proto::ImuData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_IMU_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator.cc new file mode 100644 index 0000000..917d6be --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator.cc @@ -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. + */ + +#include "cartographer/sensor/internal/collator.h" + +namespace cartographer { +namespace sensor { + +void Collator::AddTrajectory( + const int trajectory_id, + const absl::flat_hash_set& expected_sensor_ids, + const Callback& callback) { + for (const auto& sensor_id : expected_sensor_ids) { + const auto queue_key = QueueKey{trajectory_id, sensor_id}; + queue_.AddQueue(queue_key, + [callback, sensor_id](std::unique_ptr data) { + callback(sensor_id, std::move(data)); + }); + queue_keys_[trajectory_id].push_back(queue_key); + } +} + +void Collator::FinishTrajectory(const int trajectory_id) { + for (const auto& queue_key : queue_keys_[trajectory_id]) { + queue_.MarkQueueAsFinished(queue_key); + } +} + +void Collator::AddSensorData(const int trajectory_id, + std::unique_ptr data) { + QueueKey queue_key{trajectory_id, data->GetSensorId()}; + queue_.Add(std::move(queue_key), std::move(data)); +} + +void Collator::Flush() { queue_.Flush(); } + +absl::optional Collator::GetBlockingTrajectoryId() const { + return absl::optional(queue_.GetBlocker().trajectory_id); +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator.h b/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator.h new file mode 100644 index 0000000..bea93af --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator.h @@ -0,0 +1,64 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_INTERNAL_COLLATOR_H_ +#define CARTOGRAPHER_SENSOR_INTERNAL_COLLATOR_H_ + +#include +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "absl/container/flat_hash_set.h" +#include "cartographer/sensor/collator_interface.h" +#include "cartographer/sensor/data.h" +#include "cartographer/sensor/internal/ordered_multi_queue.h" + +namespace cartographer { +namespace sensor { + +class Collator : public CollatorInterface { + public: + Collator() {} + + Collator(const Collator&) = delete; + Collator& operator=(const Collator&) = delete; + + void AddTrajectory( + int trajectory_id, + const absl::flat_hash_set& expected_sensor_ids, + const Callback& callback) override; + + void FinishTrajectory(int trajectory_id) override; + + void AddSensorData(int trajectory_id, std::unique_ptr data) override; + + void Flush() override; + + absl::optional GetBlockingTrajectoryId() const override; + + private: + // Queue keys are a pair of trajectory ID and sensor identifier. + OrderedMultiQueue queue_; + + // Map of trajectory ID to all associated QueueKeys. + absl::flat_hash_map> queue_keys_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_INTERNAL_COLLATOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator_test.cc new file mode 100644 index 0000000..6cf9099 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/collator_test.cc @@ -0,0 +1,183 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/internal/collator.h" + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/internal/test_helpers.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +using testing::CollatorInput; +using testing::CollatorOutput; + +TEST(Collator, Ordering) { + const int kTrajectoryId = 0; + const std::array kSensorId = { + {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}}; + + std::vector input_data; + // Send each sensor_id once to establish a common start time. + input_data.push_back( + CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[1], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 0)); + input_data.push_back( + CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 0)); + + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[0], 100)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[1], 200)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 300)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[0], 400)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[1], 500)); + input_data.push_back( + CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 600)); + + std::vector received; + Collator collator; + collator.AddTrajectory( + kTrajectoryId, + absl::flat_hash_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId, data->GetSensorId(), + data->GetTime())); + }); + + input_data[0].MoveToCollator(&collator); + input_data[1].MoveToCollator(&collator); + input_data[2].MoveToCollator(&collator); + input_data[3].MoveToCollator(&collator); + + input_data[4].MoveToCollator(&collator); + input_data[9].MoveToCollator(&collator); + input_data[7].MoveToCollator(&collator); + input_data[5].MoveToCollator(&collator); + input_data[8].MoveToCollator(&collator); + input_data[6].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId, collator.GetBlockingTrajectoryId().value()); + + ASSERT_EQ(7, received.size()); + EXPECT_EQ(input_data[4].expected_output, received[4]); + EXPECT_EQ(input_data[5].expected_output, received[5]); + EXPECT_EQ(input_data[6].expected_output, received[6]); + + collator.FinishTrajectory(kTrajectoryId); + collator.Flush(); + ASSERT_EQ(input_data.size(), received.size()); + for (size_t i = 4; i < input_data.size(); ++i) { + EXPECT_EQ(input_data[i].expected_output, received[i]); + } +} + +TEST(Collator, OrderingMultipleTrajectories) { + const int kTrajectoryId[] = {8, 5}; + const std::array kSensorId = {{"my_points", "some_imu"}}; + + std::vector input_data; + // Send each sensor_id once to establish a common start time. + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0)); + + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 100)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 200)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 300)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 400)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 400)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 500)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 600)); + + std::vector received; + Collator collator; + collator.AddTrajectory( + kTrajectoryId[0], + absl::flat_hash_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(), + data->GetTime())); + }); + collator.AddTrajectory( + kTrajectoryId[1], + absl::flat_hash_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(), + data->GetTime())); + }); + + input_data[0].MoveToCollator(&collator); + input_data[1].MoveToCollator(&collator); + input_data[2].MoveToCollator(&collator); + input_data[3].MoveToCollator(&collator); + + input_data[4].MoveToCollator(&collator); + input_data[6].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value()); + input_data[7].MoveToCollator(&collator); + input_data[8].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value()); + input_data[5].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value()); + input_data[10].MoveToCollator(&collator); + input_data[9].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value()); + + ASSERT_EQ(5, received.size()); + EXPECT_EQ(input_data[4].expected_output, received[4]); + + collator.FinishTrajectory(kTrajectoryId[0]); + collator.FinishTrajectory(kTrajectoryId[1]); + collator.Flush(); + ASSERT_EQ(input_data.size(), received.size()); + for (size_t i = 4; i < input_data.size(); ++i) { + EXPECT_EQ(input_data[i].expected_output, received[i]); + } +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/dispatchable.h b/carto_ws/src/cartographer-master/cartographer/sensor/internal/dispatchable.h new file mode 100644 index 0000000..5aae9de --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/dispatchable.h @@ -0,0 +1,52 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_INTERNAL_DISPATCHABLE_H_ +#define CARTOGRAPHER_SENSOR_INTERNAL_DISPATCHABLE_H_ + +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace sensor { + +template +class Dispatchable : public Data { + public: + Dispatchable(const std::string &sensor_id, const DataType &data) + : Data(sensor_id), data_(data) {} + + common::Time GetTime() const override { return data_.time; } + void AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *const trajectory_builder) override { + trajectory_builder->AddSensorData(sensor_id_, data_); + } + const DataType &data() const { return data_; } + + private: + const DataType data_; +}; + +template +std::unique_ptr> MakeDispatchable( + const std::string &sensor_id, const DataType &data) { + return absl::make_unique>(sensor_id, data); +} + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_INTERNAL_DISPATCHABLE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue.cc new file mode 100644 index 0000000..26c8ee1 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue.cc @@ -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. + */ + +#include "cartographer/sensor/internal/ordered_multi_queue.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +namespace { + +// Number of items that can be queued up before we log which queues are waiting +// for data. +const int kMaxQueueSize = 500; + +} // namespace + +inline std::ostream& operator<<(std::ostream& out, const QueueKey& key) { + return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')'; +} + +OrderedMultiQueue::OrderedMultiQueue() {} + +OrderedMultiQueue::~OrderedMultiQueue() { + for (auto& entry : queues_) { + CHECK(entry.second.finished); + } +} + +void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) { + CHECK_EQ(queues_.count(queue_key), 0); + queues_[queue_key].callback = std::move(callback); +} + +void OrderedMultiQueue::MarkQueueAsFinished(const QueueKey& queue_key) { + auto it = queues_.find(queue_key); + CHECK(it != queues_.end()) << "Did not find '" << queue_key << "'."; + auto& queue = it->second; + CHECK(!queue.finished); + queue.finished = true; + Dispatch(); +} + +void OrderedMultiQueue::Add(const QueueKey& queue_key, + std::unique_ptr data) { + auto it = queues_.find(queue_key); + if (it == queues_.end()) { + LOG_EVERY_N(WARNING, 1000) + << "Ignored data for queue: '" << queue_key << "'"; + return; + } + it->second.queue.Push(std::move(data)); + Dispatch(); +} + +void OrderedMultiQueue::Flush() { + std::vector unfinished_queues; + for (auto& entry : queues_) { + if (!entry.second.finished) { + unfinished_queues.push_back(entry.first); + } + } + for (auto& unfinished_queue : unfinished_queues) { + MarkQueueAsFinished(unfinished_queue); + } +} + +QueueKey OrderedMultiQueue::GetBlocker() const { + CHECK(!queues_.empty()); + return blocker_; +} + +void OrderedMultiQueue::Dispatch() { + while (true) { + const Data* next_data = nullptr; + Queue* next_queue = nullptr; + QueueKey next_queue_key; + for (auto it = queues_.begin(); it != queues_.end();) { + const auto* data = it->second.queue.Peek(); + if (data == nullptr) { + if (it->second.finished) { + queues_.erase(it++); + continue; + } + CannotMakeProgress(it->first); + return; + } + if (next_data == nullptr || data->GetTime() < next_data->GetTime()) { + next_data = data; + next_queue = &it->second; + next_queue_key = it->first; + } + CHECK_LE(last_dispatched_time_, next_data->GetTime()) + << "Non-sorted data added to queue: '" << it->first << "'"; + ++it; + } + if (next_data == nullptr) { + CHECK(queues_.empty()); + return; + } + + // If we haven't dispatched any data for this trajectory yet, fast forward + // all queues of this trajectory until a common start time has been reached. + const common::Time common_start_time = + GetCommonStartTime(next_queue_key.trajectory_id); + + if (next_data->GetTime() >= common_start_time) { + // Happy case, we are beyond the 'common_start_time' already. + last_dispatched_time_ = next_data->GetTime(); + next_queue->callback(next_queue->queue.Pop()); + } else if (next_queue->queue.Size() < 2) { + if (!next_queue->finished) { + // We cannot decide whether to drop or dispatch this yet. + CannotMakeProgress(next_queue_key); + return; + } + last_dispatched_time_ = next_data->GetTime(); + next_queue->callback(next_queue->queue.Pop()); + } else { + // We take a peek at the time after next data. If it also is not beyond + // 'common_start_time' we drop 'next_data', otherwise we just found the + // first packet to dispatch from this queue. + std::unique_ptr next_data_owner = next_queue->queue.Pop(); + if (next_queue->queue.Peek()->GetTime() > common_start_time) { + last_dispatched_time_ = next_data->GetTime(); + next_queue->callback(std::move(next_data_owner)); + } + } + } +} + +void OrderedMultiQueue::CannotMakeProgress(const QueueKey& queue_key) { + blocker_ = queue_key; + for (auto& entry : queues_) { + if (entry.second.queue.Size() > kMaxQueueSize) { + LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key; + return; + } + } +} + +common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) { + auto emplace_result = common_start_time_per_trajectory_.emplace( + trajectory_id, common::Time::min()); + common::Time& common_start_time = emplace_result.first->second; + if (emplace_result.second) { + for (auto& entry : queues_) { + if (entry.first.trajectory_id == trajectory_id) { + common_start_time = std::max( + common_start_time, entry.second.queue.Peek()->GetTime()); + } + } + LOG(INFO) << "All sensor data for trajectory " << trajectory_id + << " is available starting at '" << common_start_time << "'."; + } + return common_start_time; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue.h b/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue.h new file mode 100644 index 0000000..d484986 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue.h @@ -0,0 +1,101 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_INTERNAL_ORDERED_MULTI_QUEUE_H_ +#define CARTOGRAPHER_SENSOR_INTERNAL_ORDERED_MULTI_QUEUE_H_ + +#include +#include +#include +#include +#include + +#include "cartographer/common/internal/blocking_queue.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/internal/dispatchable.h" + +namespace cartographer { +namespace sensor { + +struct QueueKey { + int trajectory_id; + std::string sensor_id; + + bool operator<(const QueueKey& other) const { + return std::forward_as_tuple(trajectory_id, sensor_id) < + std::forward_as_tuple(other.trajectory_id, other.sensor_id); + } +}; + +// Maintains multiple queues of sorted sensor data and dispatches it in merge +// sorted order. It will wait to see at least one value for each unfinished +// queue before dispatching the next time ordered value across all queues. +// +// This class is thread-compatible. +class OrderedMultiQueue { + public: + using Callback = std::function)>; + + OrderedMultiQueue(); + OrderedMultiQueue(OrderedMultiQueue&& queue) = default; + + ~OrderedMultiQueue(); + + // Adds a new queue with key 'queue_key' which must not already exist. + // 'callback' will be called whenever data from this queue can be dispatched. + void AddQueue(const QueueKey& queue_key, Callback callback); + + // Marks a queue as finished, i.e. no further data can be added. The queue + // will be removed once the last piece of data from it has been dispatched. + void MarkQueueAsFinished(const QueueKey& queue_key); + + // Adds 'data' to a queue with the given 'queue_key'. Data must be added + // sorted per queue. + void Add(const QueueKey& queue_key, std::unique_ptr data); + + // Dispatches all remaining values in sorted order and removes the underlying + // queues. + void Flush(); + + // Must only be called if at least one unfinished queue exists. Returns the + // key of a queue that needs more data before the OrderedMultiQueue can + // dispatch data. + QueueKey GetBlocker() const; + + private: + struct Queue { + common::BlockingQueue> queue; + Callback callback; + bool finished = false; + }; + + void Dispatch(); + void CannotMakeProgress(const QueueKey& queue_key); + common::Time GetCommonStartTime(int trajectory_id); + + // Used to verify that values are dispatched in sorted order. + common::Time last_dispatched_time_ = common::Time::min(); + + std::map common_start_time_per_trajectory_; + std::map queues_; + QueueKey blocker_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_INTERNAL_ORDERED_MULTI_QUEUE_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue_test.cc new file mode 100644 index 0000000..1a98787 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/ordered_multi_queue_test.cc @@ -0,0 +1,116 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/internal/ordered_multi_queue.h" + +#include + +#include "absl/memory/memory.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +class OrderedMultiQueueTest : public ::testing::Test { + protected: + // These are keys are chosen so that they sort first, second, third. + const QueueKey kFirst{1, "a"}; + const QueueKey kSecond{1, "b"}; + const QueueKey kThird{2, "b"}; + + void SetUp() { + for (const auto& queue_key : {kFirst, kSecond, kThird}) { + queue_.AddQueue(queue_key, [this](std::unique_ptr data) { + if (!values_.empty()) { + EXPECT_GE(data->GetTime(), values_.back()->GetTime()); + } + values_.push_back(std::move(data)); + }); + } + } + + std::unique_ptr MakeImu(const int ordinal) { + return MakeDispatchable( + "imu", ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero()}); + } + + std::vector> values_; + OrderedMultiQueue queue_; +}; + +TEST_F(OrderedMultiQueueTest, Ordering) { + queue_.Add(kFirst, MakeImu(0)); + queue_.Add(kFirst, MakeImu(4)); + queue_.Add(kFirst, MakeImu(5)); + queue_.Add(kFirst, MakeImu(6)); + EXPECT_TRUE(values_.empty()); + queue_.Add(kSecond, MakeImu(0)); + queue_.Add(kSecond, MakeImu(1)); + EXPECT_TRUE(values_.empty()); + queue_.Add(kThird, MakeImu(0)); + queue_.Add(kThird, MakeImu(2)); + EXPECT_EQ(values_.size(), 4); + queue_.Add(kSecond, MakeImu(3)); + EXPECT_EQ(values_.size(), 5); + queue_.Add(kSecond, MakeImu(7)); + queue_.Add(kThird, MakeImu(8)); + queue_.Flush(); + + EXPECT_EQ(11, values_.size()); + for (size_t i = 0; i < values_.size() - 1; ++i) { + EXPECT_LE(values_[i]->GetTime(), values_[i + 1]->GetTime()); + } +} + +TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) { + queue_.Add(kFirst, MakeImu(1)); + queue_.Add(kFirst, MakeImu(2)); + queue_.Add(kFirst, MakeImu(3)); + EXPECT_TRUE(values_.empty()); + queue_.MarkQueueAsFinished(kFirst); + EXPECT_TRUE(values_.empty()); + queue_.MarkQueueAsFinished(kSecond); + EXPECT_TRUE(values_.empty()); + queue_.MarkQueueAsFinished(kThird); + + EXPECT_EQ(3, values_.size()); + for (size_t i = 0; i < values_.size(); ++i) { + EXPECT_EQ(i + 1, common::ToUniversal(values_[i]->GetTime())); + } +} + +TEST_F(OrderedMultiQueueTest, CommonStartTimePerTrajectory) { + queue_.Add(kFirst, MakeImu(0)); + queue_.Add(kFirst, MakeImu(1)); + queue_.Add(kFirst, MakeImu(2)); + queue_.Add(kFirst, MakeImu(3)); + queue_.Add(kSecond, MakeImu(2)); + EXPECT_TRUE(values_.empty()); + queue_.Add(kThird, MakeImu(4)); + EXPECT_EQ(values_.size(), 2); + queue_.MarkQueueAsFinished(kFirst); + EXPECT_EQ(values_.size(), 2); + queue_.MarkQueueAsFinished(kSecond); + EXPECT_EQ(values_.size(), 4); + queue_.MarkQueueAsFinished(kThird); + EXPECT_EQ(values_.size(), 4); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/test_helpers.h b/carto_ws/src/cartographer-master/cartographer/sensor/internal/test_helpers.h new file mode 100644 index 0000000..0b32d89 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/test_helpers.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. + */ + +#ifndef CARTOGRAPHER_SENSOR_INTERNAL_TEST_HELPERS_H_ +#define CARTOGRAPHER_SENSOR_INTERNAL_TEST_HELPERS_H_ + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/collator_interface.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/internal/dispatchable.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace sensor { + +MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") { + return arg.isApprox(point, 0.001f); +} + +namespace testing { + +typedef std::tuple + CollatorOutput; + +struct CollatorInput { + static CollatorInput CreateImuData(int trajectory_id, + const std::string& sensor_id, int time) { + return CollatorInput{ + trajectory_id, + MakeDispatchable(sensor_id, ImuData{common::FromUniversal(time)}), + CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}}; + } + static CollatorInput CreateTimedPointCloudData(int trajectory_id, + const std::string& sensor_id, + int time) { + return CollatorInput{ + trajectory_id, + MakeDispatchable( + sensor_id, + TimedPointCloudData{ + common::FromUniversal(time), Eigen::Vector3f::Zero(), {}}), + CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}}; + } + static CollatorInput CreateOdometryData(int trajectory_id, + const std::string& sensor_id, + int time) { + return CollatorInput{ + trajectory_id, + MakeDispatchable(sensor_id, + OdometryData{common::FromUniversal(time), + transform::Rigid3d::Identity()}), + CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}}; + } + void MoveToCollator(CollatorInterface* collator) { + collator->AddSensorData(trajectory_id, std::move(data)); + } + + const int trajectory_id; + std::unique_ptr data; + const CollatorOutput expected_output; +}; + +} // namespace testing +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_INTERNAL_TEST_HELPERS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator.cc new file mode 100644 index 0000000..02d2b51 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator.cc @@ -0,0 +1,93 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/internal/trajectory_collator.h" + +#include "absl/strings/str_cat.h" + +namespace cartographer { +namespace sensor { + +metrics::Family* + TrajectoryCollator::collator_metrics_family_ = + metrics::Family::Null(); + +void TrajectoryCollator::AddTrajectory( + const int trajectory_id, + const absl::flat_hash_set& expected_sensor_ids, + const Callback& callback) { + CHECK_EQ(trajectory_to_queue_.count(trajectory_id), 0); + for (const auto& sensor_id : expected_sensor_ids) { + const auto queue_key = QueueKey{trajectory_id, sensor_id}; + trajectory_to_queue_[trajectory_id].AddQueue( + queue_key, [callback, sensor_id](std::unique_ptr data) { + callback(sensor_id, std::move(data)); + }); + trajectory_to_queue_keys_[trajectory_id].push_back(queue_key); + } +} + +void TrajectoryCollator::FinishTrajectory(const int trajectory_id) { + for (const auto& queue_key : trajectory_to_queue_keys_[trajectory_id]) { + trajectory_to_queue_.at(trajectory_id).MarkQueueAsFinished(queue_key); + } +} + +void TrajectoryCollator::AddSensorData(const int trajectory_id, + std::unique_ptr data) { + QueueKey queue_key{trajectory_id, data->GetSensorId()}; + auto* metric = GetOrCreateSensorMetric(data->GetSensorId(), trajectory_id); + metric->Increment(); + trajectory_to_queue_.at(trajectory_id) + .Add(std::move(queue_key), std::move(data)); +} + +void TrajectoryCollator::Flush() { + for (auto& it : trajectory_to_queue_) { + it.second.Flush(); + } +} + +absl::optional TrajectoryCollator::GetBlockingTrajectoryId() const { + return absl::optional(); +} + +void TrajectoryCollator::RegisterMetrics( + metrics::FamilyFactory* family_factory) { + collator_metrics_family_ = family_factory->NewCounterFamily( + "collator_input_total", "Sensor data received"); +} + +metrics::Counter* TrajectoryCollator::GetOrCreateSensorMetric( + const std::string& sensor_id, int trajectory_id) { + const std::string trajectory_id_str = absl::StrCat(trajectory_id); + const std::string map_key = absl::StrCat(sensor_id, "/", trajectory_id_str); + + auto metrics_map_itr = metrics_map_.find(map_key); + if (metrics_map_itr != metrics_map_.end()) { + return metrics_map_itr->second; + } + + LOG(INFO) << "Create metrics handler for key: " << map_key; + auto new_counter = collator_metrics_family_->Add( + {{"sensor_id", sensor_id}, {"trajectory_id", trajectory_id_str}}); + + metrics_map_[map_key] = new_counter; + return new_counter; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator.h b/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator.h new file mode 100644 index 0000000..4b7c7e4 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator.h @@ -0,0 +1,79 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_INTERNAL_TRAJECTORY_COLLATOR_H_ +#define CARTOGRAPHER_SENSOR_INTERNAL_TRAJECTORY_COLLATOR_H_ + +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "cartographer/metrics/counter.h" +#include "cartographer/metrics/family_factory.h" +#include "cartographer/sensor/collator_interface.h" +#include "cartographer/sensor/internal/ordered_multi_queue.h" + +namespace cartographer { +namespace sensor { + +// Waits to see at least one data item for all sensor ids and dispatches data +// in merge-sorted order. Contrary to 'Collator', it does not wait for other +// trajectories. +// Also contrary to 'Collator', whose output is deterministic, the sequence in +// which data is dispatched is not sorted, so non-deterministic input sequences +// will result in non-deterministic output. +class TrajectoryCollator : public CollatorInterface { + public: + TrajectoryCollator() {} + + TrajectoryCollator(const TrajectoryCollator&) = delete; + TrajectoryCollator& operator=(const TrajectoryCollator&) = delete; + + void AddTrajectory( + int trajectory_id, + const absl::flat_hash_set& expected_sensor_ids, + const Callback& callback) override; + + void FinishTrajectory(int trajectory_id) override; + + void AddSensorData(int trajectory_id, std::unique_ptr data) override; + + void Flush() override; + + absl::optional GetBlockingTrajectoryId() const override; + + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + + private: + metrics::Counter* GetOrCreateSensorMetric(const std::string& sensor_id, + int trajectory_id); + + static cartographer::metrics::Family* + collator_metrics_family_; + + // Holds individual counters for each trajectory/sensor pair. + absl::flat_hash_map metrics_map_; + + absl::flat_hash_map trajectory_to_queue_; + + // Map of trajectory ID to all associated QueueKeys. + absl::flat_hash_map> trajectory_to_queue_keys_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_INTERNAL_TRAJECTORY_COLLATOR_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator_test.cc new file mode 100644 index 0000000..cb8f5f5 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/trajectory_collator_test.cc @@ -0,0 +1,121 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/internal/trajectory_collator.h" + +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/internal/test_helpers.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +using testing::CollatorInput; +using testing::CollatorOutput; + +TEST(TrajectoryCollator, OrderingMultipleTrajectories) { + const int kTrajectoryId[] = {4, 7}; + const std::array kSensorId = {{"my_points", "some_imu"}}; + + std::vector input_data; + + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 0)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 100)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 50)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 60)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 150)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 120)); + + std::vector received; + TrajectoryCollator collator; + collator.AddTrajectory( + kTrajectoryId[0], + absl::flat_hash_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(), + data->GetTime())); + }); + collator.AddTrajectory( + kTrajectoryId[1], + absl::flat_hash_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(), + data->GetTime())); + }); + + // Send each sensor_id once to establish a common start time. + input_data[0].MoveToCollator(&collator); + input_data[1].MoveToCollator(&collator); + EXPECT_EQ(0, received.size()); + input_data[2].MoveToCollator(&collator); + input_data[3].MoveToCollator(&collator); + EXPECT_EQ(2, received.size()); + EXPECT_EQ(input_data[1].expected_output, received[0]); + EXPECT_EQ(input_data[0].expected_output, received[1]); + + // Does not wait for other trajectory. + input_data[4].MoveToCollator(&collator); + EXPECT_EQ(3, received.size()); + EXPECT_EQ(input_data[2].expected_output, received[2]); + + input_data[5].MoveToCollator(&collator); + EXPECT_EQ(4, received.size()); + EXPECT_EQ(input_data[3].expected_output, received[3]); + input_data[6].MoveToCollator(&collator); + EXPECT_EQ(5, received.size()); + EXPECT_EQ(input_data[5].expected_output, received[4]); + + // Sorts different sensors. + input_data[7].MoveToCollator(&collator); + EXPECT_EQ(5, received.size()); + input_data[8].MoveToCollator(&collator); + EXPECT_EQ(7, received.size()); + EXPECT_EQ(input_data[4].expected_output, received[5]); + EXPECT_EQ(input_data[8].expected_output, received[6]); + + EXPECT_FALSE(collator.GetBlockingTrajectoryId().has_value()); + + collator.FinishTrajectory(kTrajectoryId[0]); + collator.FinishTrajectory(kTrajectoryId[1]); + collator.Flush(); + ASSERT_EQ(input_data.size(), received.size()); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter.cc new file mode 100644 index 0000000..a6e5c0b --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter.cc @@ -0,0 +1,200 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/internal/voxel_filter.h" + +#include +#include +#include + +#include "absl/container/flat_hash_map.h" +#include "cartographer/common/math.h" + +namespace cartographer { +namespace sensor { + +namespace { + +PointCloud FilterByMaxRange(const PointCloud& point_cloud, + const float max_range) { + return point_cloud.copy_if([max_range](const RangefinderPoint& point) { + return point.position.norm() <= max_range; + }); +} + +PointCloud AdaptivelyVoxelFiltered( + const proto::AdaptiveVoxelFilterOptions& options, + const PointCloud& point_cloud) { + if (point_cloud.size() <= options.min_num_points()) { + // 'point_cloud' is already sparse enough. + return point_cloud; + } + PointCloud result = VoxelFilter(point_cloud, options.max_length()); + if (result.size() >= options.min_num_points()) { + // Filtering with 'max_length' resulted in a sufficiently dense point cloud. + return result; + } + // Search for a 'low_length' that is known to result in a sufficiently + // dense point cloud. We give up and use the full 'point_cloud' if reducing + // the edge length by a factor of 1e-2 is not enough. + for (float high_length = options.max_length(); + high_length > 1e-2f * options.max_length(); high_length /= 2.f) { + float low_length = high_length / 2.f; + result = VoxelFilter(point_cloud, low_length); + if (result.size() >= options.min_num_points()) { + // Binary search to find the right amount of filtering. 'low_length' gave + // a sufficiently dense 'result', 'high_length' did not. We stop when the + // edge length is at most 10% off. + while ((high_length - low_length) / low_length > 1e-1f) { + const float mid_length = (low_length + high_length) / 2.f; + const PointCloud candidate = VoxelFilter(point_cloud, mid_length); + if (candidate.size() >= options.min_num_points()) { + low_length = mid_length; + result = candidate; + } else { + high_length = mid_length; + } + } + return result; + } + } + return result; +} + +using VoxelKeyType = uint64_t; + +VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point, + const float resolution) { + const Eigen::Array3f index = point.array() / resolution; + const uint64_t x = common::RoundToInt(index.x()); + const uint64_t y = common::RoundToInt(index.y()); + const uint64_t z = common::RoundToInt(index.z()); + return (x << 42) + (y << 21) + z; +} + +template +std::vector RandomizedVoxelFilterIndices( + const std::vector& point_cloud, const float resolution, + PointFunction&& point_function) { + // According to https://en.wikipedia.org/wiki/Reservoir_sampling + std::minstd_rand0 generator; + absl::flat_hash_map> + voxel_count_and_point_index; + for (size_t i = 0; i < point_cloud.size(); i++) { + auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex( + point_function(point_cloud[i]), resolution)]; + voxel.first++; + if (voxel.first == 1) { + voxel.second = i; + } else { + std::uniform_int_distribution<> distribution(1, voxel.first); + if (distribution(generator) == voxel.first) { + voxel.second = i; + } + } + } + std::vector points_used(point_cloud.size(), false); + for (const auto& voxel_and_index : voxel_count_and_point_index) { + points_used[voxel_and_index.second.second] = true; + } + return points_used; +} + +template +std::vector RandomizedVoxelFilter(const std::vector& point_cloud, + const float resolution, + PointFunction&& point_function) { + const std::vector points_used = + RandomizedVoxelFilterIndices(point_cloud, resolution, point_function); + + std::vector results; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + results.push_back(point_cloud[i]); + } + } + return results; +} + +} // namespace + +std::vector VoxelFilter( + const std::vector& points, const float resolution) { + return RandomizedVoxelFilter( + points, resolution, + [](const RangefinderPoint& point) { return point.position; }); +} + +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { + const std::vector points_used = RandomizedVoxelFilterIndices( + point_cloud.points(), resolution, + [](const RangefinderPoint& point) { return point.position; }); + + std::vector filtered_points; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + filtered_points.push_back(point_cloud[i]); + } + } + std::vector filtered_intensities; + CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size()); + for (size_t i = 0; i < point_cloud.intensities().size(); i++) { + if (points_used[i]) { + filtered_intensities.push_back(point_cloud.intensities()[i]); + } + } + return PointCloud(std::move(filtered_points), + std::move(filtered_intensities)); +} + +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution) { + return RandomizedVoxelFilter( + timed_point_cloud, resolution, + [](const TimedRangefinderPoint& point) { return point.position; }); +} + +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution) { + return RandomizedVoxelFilter( + range_measurements, resolution, + [](const sensor::TimedPointCloudOriginData::RangeMeasurement& + range_measurement) { + return range_measurement.point_time.position; + }); +} + +proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::AdaptiveVoxelFilterOptions options; + options.set_max_length(parameter_dictionary->GetDouble("max_length")); + options.set_min_num_points( + parameter_dictionary->GetNonNegativeInt("min_num_points")); + options.set_max_range(parameter_dictionary->GetDouble("max_range")); + return options; +} + +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options) { + return AdaptivelyVoxelFiltered( + options, FilterByMaxRange(point_cloud, options.max_range())); +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter.h b/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter.h new file mode 100644 index 0000000..a99f861 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter.h @@ -0,0 +1,50 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_INTERNAL_VOXEL_FILTER_H_ +#define CARTOGRAPHER_SENSOR_INTERNAL_VOXEL_FILTER_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" +#include "cartographer/sensor/timed_point_cloud_data.h" + +namespace cartographer { +namespace sensor { + +std::vector VoxelFilter( + const std::vector& points, const float resolution); +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution); +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution); +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution); + +proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_INTERNAL_VOXEL_FILTER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter_test.cc new file mode 100644 index 0000000..b84152f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/internal/voxel_filter_test.cc @@ -0,0 +1,86 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/internal/voxel_filter.h" + +#include + +#include "gmock/gmock.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::Contains; +using ::testing::IsEmpty; + +TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { + const PointCloud point_cloud({{{0.f, 0.f, 0.f}}, + {{0.1f, -0.1f, 0.1f}}, + {{0.3f, -0.1f, 0.f}}, + {{0.f, 0.f, 0.1f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + ASSERT_EQ(result.size(), 2); + EXPECT_THAT(result.intensities(), IsEmpty()); + EXPECT_THAT(point_cloud.points(), Contains(result[0])); + EXPECT_THAT(point_cloud.points(), Contains(result[1])); + EXPECT_THAT(result.points(), Contains(point_cloud[2])); +} + +TEST(VoxelFilterTest, CorrectIntensities) { + std::vector points; + std::vector intensities; + for (int i = 0; i < 100; ++i) { + const float value = 0.1f * i; + // We add points with intensity equal to the z coordinate, so we can later + // verify that the resulting intensities are corresponding to the filtered + // points. + points.push_back({{-100.f, 0.3f, value}}); + intensities.push_back(value); + } + const PointCloud point_cloud(points, intensities); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + + ASSERT_EQ(result.intensities().size(), result.points().size()); + for (size_t i = 0; i < result.size(); ++i) { + ASSERT_NEAR(result[i].position.z(), result.intensities()[i], 1e-6); + } +} + +TEST(VoxelFilterTest, HandlesLargeCoordinates) { + const PointCloud point_cloud({{{100000.f, 0.f, 0.f}}, + {{100000.001f, -0.0001f, 0.0001f}}, + {{100000.003f, -0.0001f, 0.f}}, + {{-200000.f, 0.f, 0.f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.01f); + EXPECT_EQ(result.size(), 2); + EXPECT_THAT(result.intensities(), IsEmpty()); + EXPECT_THAT(result.points(), Contains(point_cloud[3])); +} + +TEST(VoxelFilterTest, IgnoresTime) { + TimedPointCloud timed_point_cloud; + for (int i = 0; i < 100; ++i) { + timed_point_cloud.push_back({{-100.f, 0.3f, 0.4f}, 1.f * i}); + } + const TimedPointCloud result = VoxelFilter(timed_point_cloud, 0.3f); + ASSERT_EQ(result.size(), 1); + EXPECT_THAT(timed_point_cloud, Contains(result[0])); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data.cc b/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data.cc new file mode 100644 index 0000000..4f9fbc9 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data.cc @@ -0,0 +1,53 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/landmark_data.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::LandmarkData ToProto(const LandmarkData& landmark_data) { + proto::LandmarkData proto; + proto.set_timestamp(common::ToUniversal(landmark_data.time)); + for (const auto& observation : landmark_data.landmark_observations) { + auto* item = proto.add_landmark_observations(); + item->set_id(observation.id); + *item->mutable_landmark_to_tracking_transform() = + transform::ToProto(observation.landmark_to_tracking_transform); + item->set_translation_weight(observation.translation_weight); + item->set_rotation_weight(observation.rotation_weight); + } + return proto; +} + +LandmarkData FromProto(const proto::LandmarkData& proto) { + LandmarkData landmark_data; + landmark_data.time = common::FromUniversal(proto.timestamp()); + for (const auto& item : proto.landmark_observations()) { + landmark_data.landmark_observations.push_back({ + item.id(), + transform::ToRigid3(item.landmark_to_tracking_transform()), + item.translation_weight(), + item.rotation_weight(), + }); + } + return landmark_data; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data.h b/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data.h new file mode 100644 index 0000000..ddab8ba --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data.h @@ -0,0 +1,53 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_ +#define CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace sensor { + +struct LandmarkObservation { + std::string id; + transform::Rigid3d landmark_to_tracking_transform; + double translation_weight; + double rotation_weight; +}; + +struct LandmarkData { + common::Time time; + std::vector landmark_observations; +}; + +// Converts 'landmark_data' to a proto::LandmarkData. +proto::LandmarkData ToProto(const LandmarkData& landmark_data); + +// Converts 'proto' to an LandmarkData. +LandmarkData FromProto(const proto::LandmarkData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data_test.cc new file mode 100644 index 0000000..dfd9c16 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/landmark_data_test.cc @@ -0,0 +1,77 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/landmark_data.h" + +#include + +#include "cartographer/sensor/internal/test_helpers.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::DoubleNear; +using ::testing::Field; + +::testing::Matcher EqualsLandmark( + const LandmarkObservation& expected) { + return ::testing::AllOf( + Field(&LandmarkObservation::id, expected.id), + Field(&LandmarkObservation::landmark_to_tracking_transform, + transform::IsNearly(expected.landmark_to_tracking_transform, 1e-2)), + Field(&LandmarkObservation::translation_weight, + DoubleNear(expected.translation_weight, 0.01)), + Field(&LandmarkObservation::rotation_weight, + DoubleNear(expected.rotation_weight, 0.01))); +} + +class LandmarkDataTest : public ::testing::Test { + protected: + LandmarkDataTest() + : observations_( + {{ + "ID1", + transform::Rigid3d(Eigen::Vector3d(1., 1., 1.), + Eigen::Quaterniond(1., 1., -1., -1.)), + 1.f, + 3.f, + }, + { + "ID2", + transform::Rigid3d(Eigen::Vector3d(2., 2., 2.), + Eigen::Quaterniond(2., 2., -2., -2.)), + 2.f, + 4.f, + }}) {} + std::vector observations_; +}; + +TEST_F(LandmarkDataTest, LandmarkDataToAndFromProto) { + const auto expected = LandmarkData{common::FromUniversal(50), observations_}; + const auto actual = FromProto(ToProto(expected)); + EXPECT_EQ(expected.time, actual.time); + EXPECT_THAT(actual.landmark_observations, + ElementsAre(EqualsLandmark(expected.landmark_observations[0]), + EqualsLandmark(expected.landmark_observations[1]))); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/map_by_time.h b/carto_ws/src/cartographer-master/cartographer/sensor/map_by_time.h new file mode 100644 index 0000000..ab7e9a2 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/map_by_time.h @@ -0,0 +1,213 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_ +#define CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_ + +#include +#include +#include +#include +#include + +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/id.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +// 'DataType' must contain a 'time' member of type common::Time. +template +class MapByTime { + public: + // Appends data to a 'trajectory_id', creating trajectories as needed. + void Append(const int trajectory_id, const DataType& data) { + CHECK_GE(trajectory_id, 0); + auto& trajectory = data_[trajectory_id]; + if (!trajectory.empty()) { + CHECK_GT(data.time, std::prev(trajectory.end())->first); + } + trajectory.emplace(data.time, data); + } + + // Removes data no longer needed once 'node_id' gets removed from 'nodes'. + // 'NodeType' must contain a 'time' member of type common::Time. + template + void Trim(const mapping::MapById& nodes, + const mapping::NodeId& node_id) { + const int trajectory_id = node_id.trajectory_id; + CHECK_GE(trajectory_id, 0); + if (data_.count(trajectory_id) == 0) { + return; + } + + // Data only important between 'gap_start' and 'gap_end' is no longer + // needed. We retain the first and last data of the gap so that + // interpolation with the adjacent data outside the gap is still possible. + const auto node_it = nodes.find(node_id); + CHECK(node_it != nodes.end()); + const common::Time gap_start = + node_it != nodes.BeginOfTrajectory(trajectory_id) + ? std::prev(node_it)->data.time + : common::Time::min(); + const auto next_it = std::next(node_it); + const common::Time gap_end = next_it != nodes.EndOfTrajectory(trajectory_id) + ? next_it->data.time + : common::Time::max(); + CHECK_LT(gap_start, gap_end); + + auto& trajectory = data_.at(trajectory_id); + auto data_it = trajectory.lower_bound(gap_start); + auto data_end = trajectory.upper_bound(gap_end); + if (data_it == data_end) { + return; + } + if (gap_end != common::Time::max()) { + // Retain the last data inside the gap. + data_end = std::prev(data_end); + if (data_it == data_end) { + return; + } + } + if (gap_start != common::Time::min()) { + // Retain the first data inside the gap. + data_it = std::next(data_it); + } + while (data_it != data_end) { + data_it = trajectory.erase(data_it); + } + if (trajectory.empty()) { + data_.erase(trajectory_id); + } + } + + bool HasTrajectory(const int trajectory_id) const { + return data_.count(trajectory_id) != 0; + } + + class ConstIterator { + public: + using iterator_category = std::bidirectional_iterator_tag; + using value_type = DataType; + using difference_type = int64; + using pointer = const DataType*; + using reference = const DataType&; + + explicit ConstIterator( + typename std::map::const_iterator iterator) + : iterator_(iterator) {} + + const DataType& operator*() const { return iterator_->second; } + + const DataType* operator->() const { return &iterator_->second; } + + ConstIterator& operator++() { + ++iterator_; + return *this; + } + + ConstIterator& operator--() { + --iterator_; + return *this; + } + + bool operator==(const ConstIterator& it) const { + return iterator_ == it.iterator_; + } + + bool operator!=(const ConstIterator& it) const { return !operator==(it); } + + private: + typename std::map::const_iterator iterator_; + }; + + class ConstTrajectoryIterator { + public: + using iterator_category = std::bidirectional_iterator_tag; + using value_type = int; + using difference_type = int64; + using pointer = const int*; + using reference = const int&; + + explicit ConstTrajectoryIterator( + typename std::map>::const_iterator + current_trajectory) + : current_trajectory_(current_trajectory) {} + + int operator*() const { return current_trajectory_->first; } + + ConstTrajectoryIterator& operator++() { + ++current_trajectory_; + return *this; + } + + ConstTrajectoryIterator& operator--() { + --current_trajectory_; + return *this; + } + + bool operator==(const ConstTrajectoryIterator& it) const { + return current_trajectory_ == it.current_trajectory_; + } + + bool operator!=(const ConstTrajectoryIterator& it) const { + return !operator==(it); + } + + private: + typename std::map>::const_iterator + current_trajectory_; + }; + + ConstIterator BeginOfTrajectory(const int trajectory_id) const { + return ConstIterator(data_.at(trajectory_id).begin()); + } + + ConstIterator EndOfTrajectory(const int trajectory_id) const { + return ConstIterator(data_.at(trajectory_id).end()); + } + + // Returns Range object for range-based loops over the trajectory IDs. + mapping::Range trajectory_ids() const { + return mapping::Range( + ConstTrajectoryIterator(data_.begin()), + ConstTrajectoryIterator(data_.end())); + } + + mapping::Range trajectory(const int trajectory_id) const { + return mapping::Range(BeginOfTrajectory(trajectory_id), + EndOfTrajectory(trajectory_id)); + } + + // Returns an iterator to the first element in the container belonging to + // trajectory 'trajectory_id' whose time is not considered to go before + // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go + // before 'time'. 'trajectory_id' must refer to an existing trajectory. + ConstIterator lower_bound(const int trajectory_id, + const common::Time time) const { + return ConstIterator(data_.at(trajectory_id).lower_bound(time)); + } + + private: + std::map> data_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/map_by_time_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/map_by_time_test.cc new file mode 100644 index 0000000..4be3587 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/map_by_time_test.cc @@ -0,0 +1,102 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/map_by_time.h" + +#include + +#include "cartographer/common/time.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +common::Time CreateTime(const int milliseconds) { + return common::Time(common::FromMilliseconds(milliseconds)); +} + +struct Data { + common::Time time; +}; + +struct NodeData { + common::Time time; +}; + +TEST(MapByTimeTest, AppendAndViewTrajectory) { + MapByTime map_by_time; + map_by_time.Append(0, Data{CreateTime(10)}); + map_by_time.Append(42, Data{CreateTime(42)}); + map_by_time.Append(42, Data{CreateTime(43)}); + std::deque expected_data = {Data{CreateTime(42)}, Data{CreateTime(43)}}; + for (const Data& data : map_by_time.trajectory(42)) { + ASSERT_FALSE(expected_data.empty()); + EXPECT_EQ(expected_data.front().time, data.time); + expected_data.pop_front(); + } + EXPECT_TRUE(expected_data.empty()); +} + +TEST(MapByTimeTest, Trimming) { + MapByTime map_by_time; + EXPECT_FALSE(map_by_time.HasTrajectory(42)); + map_by_time.Append(42, Data{CreateTime(1)}); + map_by_time.Append(42, Data{CreateTime(41)}); + map_by_time.Append(42, Data{CreateTime(42)}); + map_by_time.Append(42, Data{CreateTime(43)}); + map_by_time.Append(42, Data{CreateTime(47)}); + map_by_time.Append(42, Data{CreateTime(48)}); + map_by_time.Append(42, Data{CreateTime(49)}); + map_by_time.Append(42, Data{CreateTime(5000)}); + EXPECT_TRUE(map_by_time.HasTrajectory(42)); + // Trim one node. + mapping::MapById map_by_id; + map_by_id.Append(42, NodeData{CreateTime(42)}); + map_by_id.Append(42, NodeData{CreateTime(46)}); + map_by_id.Append(42, NodeData{CreateTime(48)}); + map_by_time.Trim(map_by_id, mapping::NodeId{42, 1}); + map_by_id.Trim(mapping::NodeId{42, 1}); + ASSERT_TRUE(map_by_time.HasTrajectory(42)); + std::deque expected_data = { + Data{CreateTime(1)}, Data{CreateTime(41)}, Data{CreateTime(42)}, + Data{CreateTime(48)}, Data{CreateTime(49)}, Data{CreateTime(5000)}}; + for (const Data& data : map_by_time.trajectory(42)) { + ASSERT_FALSE(expected_data.empty()); + EXPECT_EQ(expected_data.front().time, data.time); + expected_data.pop_front(); + } + EXPECT_TRUE(expected_data.empty()); + // Trim everything. + map_by_time.Trim(map_by_id, mapping::NodeId{42, 2}); + map_by_id.Trim(mapping::NodeId{42, 2}); + map_by_time.Trim(map_by_id, mapping::NodeId{42, 0}); + map_by_id.Trim(mapping::NodeId{42, 0}); + EXPECT_FALSE(map_by_time.HasTrajectory(42)); +} + +TEST(MapByTimeTest, TrimmingDoesNotCreateTrajectory) { + MapByTime map_by_time; + EXPECT_FALSE(map_by_time.HasTrajectory(42)); + mapping::MapById map_by_id; + map_by_id.Append(42, NodeData{CreateTime(42)}); + map_by_time.Trim(map_by_id, mapping::NodeId{42, 0}); + EXPECT_FALSE(map_by_time.HasTrajectory(42)); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/odometry_data.cc b/carto_ws/src/cartographer-master/cartographer/sensor/odometry_data.cc new file mode 100644 index 0000000..50b5027 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/odometry_data.cc @@ -0,0 +1,37 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/odometry_data.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::OdometryData ToProto(const OdometryData& odometry_data) { + proto::OdometryData proto; + proto.set_timestamp(common::ToUniversal(odometry_data.time)); + *proto.mutable_pose() = transform::ToProto(odometry_data.pose); + return proto; +} + +OdometryData FromProto(const proto::OdometryData& proto) { + return OdometryData{common::FromUniversal(proto.timestamp()), + transform::ToRigid3(proto.pose())}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/odometry_data.h b/carto_ws/src/cartographer-master/cartographer/sensor/odometry_data.h new file mode 100644 index 0000000..c4f0778 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/odometry_data.h @@ -0,0 +1,41 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_ODOMETRY_DATA_H_ +#define CARTOGRAPHER_SENSOR_ODOMETRY_DATA_H_ + +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace sensor { + +struct OdometryData { + common::Time time; + transform::Rigid3d pose; +}; + +// Converts 'odometry_data' to a proto::OdometryData. +proto::OdometryData ToProto(const OdometryData& odometry_data); + +// Converts 'proto' to an OdometryData. +OdometryData FromProto(const proto::OdometryData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_ODOMETRY_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud.cc b/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud.cc new file mode 100644 index 0000000..095d163 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud.cc @@ -0,0 +1,154 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/point_cloud.h" + +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +PointCloud::PointCloud() {} +PointCloud::PointCloud(std::vector points) + : points_(std::move(points)) {} + +// 构造时先拷贝, 再进行移动 +PointCloud::PointCloud(std::vector points, + std::vector intensities) + : points_(std::move(points)), intensities_(std::move(intensities)) { + if (!intensities_.empty()) { + CHECK_EQ(points_.size(), intensities_.size()); + } +} + +size_t PointCloud::size() const { return points_.size(); } +bool PointCloud::empty() const { return points_.empty(); } + +//....返回vector的引用 后面这个const代表什么 代表这个函数不能修改任何数据成员 +const std::vector& PointCloud::points() const { + return points_; +} + + std::vector& PointCloud::pointreturn() { + return points_; +} + + +// 返回vector的引用 +const std::vector& PointCloud::intensities() const { + return intensities_; +} +const PointCloud::PointType& PointCloud::operator[](const size_t index) const { + return points_[index]; +} + +PointCloud::ConstIterator PointCloud::begin() const { return points_.begin(); } +PointCloud::ConstIterator PointCloud::end() const { return points_.end(); } + +//。。。 void PointCloud::push_back(PointCloud::PointType value) { +// points_.push_back(std::move(value)); +// } +void PointCloud::push_back(PointCloud::PointType value,float insitity) { + points_.push_back(std::move(value)); + intensities_.push_back(std::move(insitity)); +} +void PointCloud::push_back(PointCloud::PointType value) { + points_.push_back(std::move(value)); + +} + +/** + * @brief 对输入的点云做坐标变换 + * + * @param[in] point_cloud 输入的点云 + * @param[in] transform 坐标变换 + * @return PointCloud 变换之后的点云 + */ +PointCloud TransformPointCloud(const PointCloud& point_cloud, + const transform::Rigid3f& transform) { + std::vector points; + points.reserve(point_cloud.size()); + for (const RangefinderPoint& point : point_cloud.points()) { + points.emplace_back(transform * point); + } + return PointCloud(points, point_cloud.intensities()); +} + +PointCloud TransformPointCloudNew( PointCloud& point_cloud, + const transform::Rigid3f& transform) { + std::vector points; + points.reserve(point_cloud.size()); + for (const RangefinderPoint& point : point_cloud.points()) { + points.emplace_back(transform * point); + } + return PointCloud(points, point_cloud.intensities()); +} + + +/** + * @brief 返回坐标变换后的点云 + * + * @param[in] point_cloud 点云数据 + * @param[in] transform 旋转变换矩阵 + * @return TimedPointCloud 返回坐标变换后的点云 + */ +//... +PointCloudWithIntensities TransformTimedPointCloud(//。。。 const TimedPointCloud& point_cloud, + const PointCloudWithIntensities& point_cloud, + const transform::Rigid3f& transform) { + //。。。 TimedPointCloud result; + PointCloudWithIntensities result; + //。。。 result.reserve(point_cloud.size()); + result.points.reserve(point_cloud.points.size()); + result.intensities.reserve(point_cloud.intensities.size()); + result.intensities=point_cloud.intensities; + //。。。for (const TimedRangefinderPoint& point : point_cloud) { + //。。。 result.push_back(transform * point); + for (const TimedRangefinderPoint& point : point_cloud.points) { + result.points.push_back(transform * point); + } + return result; +} +TimedPointCloud TransformTimedPointCloud( + const TimedPointCloud& point_cloud, + const transform::Rigid3f& transform) { + TimedPointCloud result; + result.reserve(point_cloud.size()); + for (const TimedRangefinderPoint& point : point_cloud) { + result.push_back(transform * point); + } + return result; +} +/** + * @brief 对输入的点云进行滤波, 保留数据点的z坐标处于min_z与max_z之间的点 + * + * @param[in] point_cloud 输入的点云 + * @param[in] min_z 最小的z + * @param[in] max_z 最大的z + * @return PointCloud 裁剪之后的点云 拷贝 + */ +PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, + const float max_z) { + // 将lamda表达式传入copy_if, 当lamda表达式返回true时才进行复制, + return point_cloud.copy_if([min_z, max_z](const RangefinderPoint& point) { + return min_z <= point.position.z() && point.position.z() <= max_z; + }); +} + +} // namespace sensor +} // namespace cartographer + diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud.h b/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud.h new file mode 100644 index 0000000..db96989 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud.h @@ -0,0 +1,144 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ +#define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/sensor/rangefinder_point.h" +#include "cartographer/transform/rigid_transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +// Stores 3D positions of points together with some additional data, e.g. +// intensities. +/** + * @brief 点云结构, 包含雷达一帧数据的所有数据点 与 数据点对应的强度值 + * + */ +class PointCloud { + public: + using PointType = RangefinderPoint; + + PointCloud(); + explicit PointCloud(std::vector points); + PointCloud(std::vector points, std::vector intensities); + + // Returns the number of points in the point cloud. + size_t size() const; + // Checks whether there are any points in the point cloud. + bool empty() const; + + const std::vector& points() const; + //....重载 + std::vector& pointreturn(); + + const std::vector& intensities() const; + const PointType& operator[](const size_t index) const; + + // Iterator over the points in the point cloud. + using ConstIterator = std::vector::const_iterator; + ConstIterator begin() const; + ConstIterator end() const; + + //。。。void push_back(PointType value); + void push_back(PointType value,float intensity); + void push_back(PointType value); + // Creates a PointCloud consisting of all the points for which `predicate` + // returns true, together with the corresponding intensities. + // 根据条件进行赋值 + template + PointCloud copy_if(UnaryPredicate predicate) const { + std::vector points; + std::vector intensities; + + // Note: benchmarks show that it is better to have this conditional outside + // the loop. + if (intensities_.empty()) { + for (size_t index = 0; index < size(); ++index) { + const PointType& point = points_[index]; + // 表达式为true时才使用这个点 + if (predicate(point)) { + points.push_back(point); + } + } + } else { + for (size_t index = 0; index < size(); ++index) { + const PointType& point = points_[index]; + if (predicate(point)) { + points.push_back(point); + intensities.push_back(intensities_[index]); + } + } + } + + return PointCloud(points, intensities); + } + + private: + // For 2D points, the third entry is 0.f. + std::vector points_; + // Intensities are optional. If non-empty, they must have the same size as + // points. + std::vector intensities_; +}; + +// Stores 3D positions of points with their relative measurement time in the +// fourth entry. Time is in seconds, increasing and relative to the moment when +// the last point was acquired. So, the fourth entry for the last point is 0.f. +// If timing is not available, all fourth entries are 0.f. For 2D points, the +// third entry is 0.f (and the fourth entry is time). +// 将点的3D位置及其相对测量时间存储在第四项中. +// 时间以秒为单位, 相对于获取最后一点的时间增加. 因此, 最后一点的第四个条目是0.f. +// 如果计时不可用, 则所有第四项均为0.f. 对于2D点, z坐标是0.f（第四项是时间）. + +using TimedPointCloud = std::vector; +// TODO(wohe): Retained for cartographer_ros. To be removed once it is no +// longer used there. +struct PointCloudWithIntensities { + TimedPointCloud points; + std::vector intensities; + +}; + +// Transforms 'point_cloud' according to 'transform'. +PointCloud TransformPointCloud(const PointCloud& point_cloud, + const transform::Rigid3f& transform); +//... +PointCloud TransformPointCloudNew(PointCloud& point_cloud, + const transform::Rigid3f& transform); + + +// ...Transforms 'point_cloud' according to 'transform'. +PointCloudWithIntensities TransformTimedPointCloud(const PointCloudWithIntensities& point_cloud, + const transform::Rigid3f& transform); +TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, + const transform::Rigid3f& transform); +// Returns a new point cloud without points that fall outside the region defined +// by 'min_z' and 'max_z'. +PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z, + float max_z); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ + diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud_test.cc new file mode 100644 index 0000000..47c6feb --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/point_cloud_test.cc @@ -0,0 +1,91 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/point_cloud.h" + +#include + +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::ElementsAre; +using ::testing::FloatNear; +using ::testing::IsEmpty; + +TEST(PointCloudTest, TransformPointCloud) { + PointCloud point_cloud; + point_cloud.push_back({{Eigen::Vector3f{0.5f, 0.5f, 1.f}}}); + point_cloud.push_back({{Eigen::Vector3f{3.5f, 0.5f, 42.f}}}); + const PointCloud transformed_point_cloud = TransformPointCloud( + point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); + EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); + EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6); + EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6); +} + +TEST(PointCloudTest, TransformTimedPointCloud) { + TimedPointCloud point_cloud; + point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}, 0.f}); + point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}, 0.f}); + const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud( + point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); + EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); + EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6); + EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6); +} + +TEST(PointCloudTest, CopyIf) { + std::vector points = { + {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}; + + const PointCloud point_cloud(points); + const PointCloud copied_point_cloud = point_cloud.copy_if( + [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; }); + + EXPECT_EQ(copied_point_cloud.size(), 2); + EXPECT_THAT(copied_point_cloud.intensities(), IsEmpty()); + EXPECT_THAT(copied_point_cloud.points(), + ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}}, + RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}})); +} + +TEST(PointCloudTest, CopyIfWithIntensities) { + std::vector points = { + {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}; + std::vector intensities = {0.f, 1.f, 2.f}; + + const PointCloud point_cloud(points, intensities); + const PointCloud copied_point_cloud = point_cloud.copy_if( + [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; }); + EXPECT_EQ(copied_point_cloud.size(), 2); + EXPECT_EQ(copied_point_cloud.intensities().size(), 2); + EXPECT_THAT(copied_point_cloud.points(), + ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}}, + RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}})); + EXPECT_THAT(copied_point_cloud.intensities(), + ElementsAre(FloatNear(1.f, 1e-6), FloatNear(2.f, 1e-6))); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/proto/adaptive_voxel_filter_options.proto b/carto_ws/src/cartographer-master/cartographer/sensor/proto/adaptive_voxel_filter_options.proto new file mode 100644 index 0000000..57c4770 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/proto/adaptive_voxel_filter_options.proto @@ -0,0 +1,29 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.sensor.proto; + +message AdaptiveVoxelFilterOptions { + // 'max_length' of a voxel edge. + float max_length = 1; + + // If there are more points and not at least 'min_num_points' remain, the + // voxel length is reduced trying to get this minimum number of points. + float min_num_points = 2; + + // Points further away from the origin are removed. + float max_range = 3; +} diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/proto/sensor.proto b/carto_ws/src/cartographer-master/cartographer/sensor/proto/sensor.proto new file mode 100644 index 0000000..62cc640 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/proto/sensor.proto @@ -0,0 +1,85 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 = "proto3"; + +package cartographer.sensor.proto; + +option java_outer_classname = "Sensor"; + +import "cartographer/transform/proto/transform.proto"; + +message RangefinderPoint { + transform.proto.Vector3f position = 1; +} + +message TimedRangefinderPoint { + transform.proto.Vector3f position = 1; + float time = 2; +} + +// Compressed collection of a 3D point cloud. +message CompressedPointCloud { + int32 num_points = 1; + repeated int32 point_data = 3; +} + +// Proto representation of ::cartographer::sensor::TimedPointCloudData. +message TimedPointCloudData { + int64 timestamp = 1; + transform.proto.Vector3f origin = 2; + repeated transform.proto.Vector4f point_data_legacy = 3; + repeated TimedRangefinderPoint point_data = 4; + repeated float intensities = 5; +} + +// Proto representation of ::cartographer::sensor::RangeData. +message RangeData { + transform.proto.Vector3f origin = 1; + repeated transform.proto.Vector3f returns_legacy = 2; + repeated transform.proto.Vector3f misses_legacy = 3; + repeated RangefinderPoint returns = 4; + repeated RangefinderPoint misses = 5; +} + +// Proto representation of ::cartographer::sensor::ImuData. +message ImuData { + int64 timestamp = 1; + transform.proto.Vector3d linear_acceleration = 2; + transform.proto.Vector3d angular_velocity = 3; +} + +// Proto representation of ::cartographer::sensor::OdometryData. +message OdometryData { + int64 timestamp = 1; + transform.proto.Rigid3d pose = 2; +} + +// Proto representation of ::cartographer::sensor::FixedFramePoseData. +message FixedFramePoseData { + int64 timestamp = 1; + transform.proto.Rigid3d pose = 2; +} + +// Proto representation of ::cartographer::sensor::LandmarkData. +message LandmarkData { + message LandmarkObservation { + bytes id = 1; + transform.proto.Rigid3d landmark_to_tracking_transform = 2; + double translation_weight = 3; + double rotation_weight = 4; + } + int64 timestamp = 1; + repeated LandmarkObservation landmark_observations = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/range_data.cc b/carto_ws/src/cartographer-master/cartographer/sensor/range_data.cc new file mode 100644 index 0000000..c636a7e --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/range_data.cc @@ -0,0 +1,106 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/range_data.h" + +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +RangeData TransformRangeData(const RangeData& range_data, + const transform::Rigid3f& transform) { + return RangeData{ + transform * range_data.origin, + TransformPointCloud(range_data.returns, transform), + TransformPointCloud(range_data.misses, transform), + }; +} + + + +RangeData TransformRangeDataNew( RangeData& range_data, + const transform::Rigid3f& transform) { + return RangeData{ + transform * range_data.origin, + TransformPointCloudNew(range_data.returns, transform), + TransformPointCloudNew(range_data.misses, transform), + }; +} +RangeData CropRangeData(const RangeData& range_data, const float min_z, + const float max_z) { + return RangeData{range_data.origin, + CropPointCloud(range_data.returns, min_z, max_z), + CropPointCloud(range_data.misses, min_z, max_z)}; +} + +RangeData CropRangeDataNew(RangeData& range_data, float min_z, + float max_z) { + return RangeData{range_data.origin, + CropPointCloud(range_data.returns, min_z, max_z), + CropPointCloud(range_data.misses, min_z, max_z)}; +} + + + + + +proto::RangeData ToProto(const RangeData& range_data) { + proto::RangeData proto; + *proto.mutable_origin() = transform::ToProto(range_data.origin); + proto.mutable_returns()->Reserve(range_data.returns.size()); + for (const RangefinderPoint& point : range_data.returns) { + *proto.add_returns() = ToProto(point); + } + proto.mutable_misses()->Reserve(range_data.misses.size()); + for (const RangefinderPoint& point : range_data.misses) { + *proto.add_misses() = ToProto(point); + } + return proto; +} + +RangeData FromProto(const proto::RangeData& proto) { + std::vector returns; + if (proto.returns_size() > 0) { + returns.reserve(proto.returns().size()); + for (const auto& point_proto : proto.returns()) { + returns.push_back(FromProto(point_proto)); + } + } else { + returns.reserve(proto.returns_legacy().size()); + for (const auto& point_proto : proto.returns_legacy()) { + returns.push_back({transform::ToEigen(point_proto)}); + } + } + std::vector misses; + if (proto.misses_size() > 0) { + misses.reserve(proto.misses().size()); + for (const auto& point_proto : proto.misses()) { + misses.push_back(FromProto(point_proto)); + } + } else { + misses.reserve(proto.misses_legacy().size()); + for (const auto& point_proto : proto.misses_legacy()) { + misses.push_back({transform::ToEigen(point_proto)}); + } + } + return RangeData{transform::ToEigen(proto.origin()), PointCloud(returns), + PointCloud(misses)}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/range_data.h b/carto_ws/src/cartographer-master/cartographer/sensor/range_data.h new file mode 100644 index 0000000..276fc84 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/range_data.h @@ -0,0 +1,62 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_RANGE_DATA_H_ +#define CARTOGRAPHER_SENSOR_RANGE_DATA_H_ + +#include "cartographer/common/port.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/proto/sensor.pb.h" + +namespace cartographer { +namespace sensor { + +// Rays begin at 'origin'. 'returns' are the points where obstructions were +// detected. 'misses' are points in the direction of rays for which no return +// was detected, and were inserted at a configured distance. It is assumed that +// between the 'origin' and 'misses' is free space. +struct RangeData { + Eigen::Vector3f origin; + PointCloud returns; + PointCloud misses; +}; + +RangeData TransformRangeData(const RangeData& range_data, + const transform::Rigid3f& transform); +//.... + +RangeData TransformRangeDataNew( RangeData& range_data, + const transform::Rigid3f& transform); + + + +// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. +RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z); + +RangeData CropRangeDataNew(RangeData& range_data, float min_z, float max_z); + + +// Converts 'range_data' to a proto::RangeData. +proto::RangeData ToProto(const RangeData& range_data); + +// Converts 'proto' to RangeData. +RangeData FromProto(const proto::RangeData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_RANGE_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/range_data_test.cc b/carto_ws/src/cartographer-master/cartographer/sensor/range_data_test.cc new file mode 100644 index 0000000..c91d168 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/range_data_test.cc @@ -0,0 +1,46 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/range_data.h" + +#include +#include + +#include "cartographer/sensor/internal/test_helpers.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::Contains; + +class RangeDataTest : public ::testing::Test { + protected: + RangeDataTest() : origin_(Eigen::Vector3f(1, 1, 1)) { + returns_.emplace_back(0, 1, 2); + returns_.emplace_back(4, 5, 6); + returns_.emplace_back(0, 1, 2); + misses_.emplace_back(7, 8, 9); + } + Eigen::Vector3f origin_; + std::vector returns_; + std::vector misses_; +}; + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/rangefinder_point.h b/carto_ws/src/cartographer-master/cartographer/sensor/rangefinder_point.h new file mode 100644 index 0000000..fda0f7f --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/rangefinder_point.h @@ -0,0 +1,108 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_ +#define CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +// Stores 3D position of a point observed by a rangefinder sensor. +struct RangefinderPoint { + Eigen::Vector3f position; +}; + +// Stores 3D position of a point with its relative measurement time. +// See point_cloud.h for more details. +struct TimedRangefinderPoint { + Eigen::Vector3f position; + float time; +}; + +template +inline RangefinderPoint operator*(const transform::Rigid3& lhs, + const RangefinderPoint& rhs) { + RangefinderPoint result = rhs; + result.position = lhs * rhs.position; + return result; +} + +template +inline TimedRangefinderPoint operator*(const transform::Rigid3& lhs, + const TimedRangefinderPoint& rhs) { + TimedRangefinderPoint result = rhs; + result.position = lhs * rhs.position; + return result; +} + +inline bool operator==(const RangefinderPoint& lhs, + const RangefinderPoint& rhs) { + return lhs.position == rhs.position; +} + +inline bool operator==(const TimedRangefinderPoint& lhs, + const TimedRangefinderPoint& rhs) { + return lhs.position == rhs.position && lhs.time == rhs.time; +} + +inline RangefinderPoint FromProto( + const proto::RangefinderPoint& rangefinder_point_proto) { + return {transform::ToEigen(rangefinder_point_proto.position())}; +} + +inline proto::RangefinderPoint ToProto( + const RangefinderPoint& rangefinder_point) { + proto::RangefinderPoint proto; + *proto.mutable_position() = transform::ToProto(rangefinder_point.position); + return proto; +} + +inline TimedRangefinderPoint FromProto( + const proto::TimedRangefinderPoint& timed_rangefinder_point_proto) { + return {transform::ToEigen(timed_rangefinder_point_proto.position()), + timed_rangefinder_point_proto.time()}; +} + +inline proto::TimedRangefinderPoint ToProto( + const TimedRangefinderPoint& timed_rangefinder_point) { + proto::TimedRangefinderPoint proto; + *proto.mutable_position() = + transform::ToProto(timed_rangefinder_point.position); + proto.set_time(timed_rangefinder_point.time); + return proto; +} + +inline RangefinderPoint ToRangefinderPoint( + const TimedRangefinderPoint& timed_rangefinder_point) { + return {timed_rangefinder_point.position}; +} + +inline TimedRangefinderPoint ToTimedRangefinderPoint( + const RangefinderPoint& rangefinder_point, const float time) { + return {rangefinder_point.position, time}; +} + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/timed_point_cloud_data.cc b/carto_ws/src/cartographer-master/cartographer/sensor/timed_point_cloud_data.cc new file mode 100644 index 0000000..2f01f08 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/timed_point_cloud_data.cc @@ -0,0 +1,64 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/timed_point_cloud_data.h" + +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::TimedPointCloudData ToProto( + const TimedPointCloudData& timed_point_cloud_data) { + proto::TimedPointCloudData proto; + proto.set_timestamp(common::ToUniversal(timed_point_cloud_data.time)); + *proto.mutable_origin() = transform::ToProto(timed_point_cloud_data.origin); + proto.mutable_point_data()->Reserve(timed_point_cloud_data.ranges.size()); + for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) { + *proto.add_point_data() = ToProto(range); + } + for (const float intensity : timed_point_cloud_data.intensities) { + proto.add_intensities(intensity); + } + return proto; +} + +TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { + CHECK(proto.intensities().size() == 0 || + proto.intensities().size() == proto.point_data().size()); + TimedPointCloud timed_point_cloud; + if (proto.point_data().size() > 0) { + timed_point_cloud.reserve(proto.point_data().size()); + for (const auto& timed_point_proto : proto.point_data()) { + timed_point_cloud.push_back(FromProto(timed_point_proto)); + } + } else { + timed_point_cloud.reserve(proto.point_data_legacy().size()); + for (const auto& timed_point_proto : proto.point_data_legacy()) { + const Eigen::Vector4f timed_point = transform::ToEigen(timed_point_proto); + timed_point_cloud.push_back({timed_point.head<3>(), timed_point[3]}); + } + } + return TimedPointCloudData{common::FromUniversal(proto.timestamp()), + transform::ToEigen(proto.origin()), + timed_point_cloud, + std::vector(proto.intensities().begin(), + proto.intensities().end())}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/sensor/timed_point_cloud_data.h b/carto_ws/src/cartographer-master/cartographer/sensor/timed_point_cloud_data.h new file mode 100644 index 0000000..8bdd928 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/sensor/timed_point_cloud_data.h @@ -0,0 +1,56 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_ +#define CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_ + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace sensor { + +struct TimedPointCloudData { + common::Time time; + Eigen::Vector3f origin; + TimedPointCloud ranges; + // 'intensities' has to be same size as 'ranges', or empty. + std::vector intensities; +}; + +struct TimedPointCloudOriginData { + struct RangeMeasurement { + TimedRangefinderPoint point_time; + float intensity; + size_t origin_index; + }; + common::Time time; + std::vector origins; + std::vector ranges; +}; + +// Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData. +proto::TimedPointCloudData ToProto( + const TimedPointCloudData& timed_point_cloud_data); + +// Converts 'proto' to TimedPointCloudData. +TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/transform/proto/timestamped_transform.proto b/carto_ws/src/cartographer-master/cartographer/transform/proto/timestamped_transform.proto new file mode 100644 index 0000000..0db6703 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/proto/timestamped_transform.proto @@ -0,0 +1,25 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.transform.proto; + +import "cartographer/transform/proto/transform.proto"; + +// NEXT ID: 3 +message TimestampedTransform { + int64 time = 1; + transform.proto.Rigid3d transform = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/transform/proto/transform.proto b/carto_ws/src/cartographer-master/cartographer/transform/proto/transform.proto new file mode 100644 index 0000000..82ed9a6 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/proto/transform.proto @@ -0,0 +1,84 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.transform.proto; + +// All coordinates are expressed in the right-handed Cartesian coordinate system +// used by Cartographer (x forward, y left, z up). Message names are chosen to +// mirror those used in the Eigen library. + +message Vector2d { + double x = 1; + double y = 2; +} + +message Vector2f { + float x = 1; + float y = 2; +} + +message Vector3d { + double x = 1; + double y = 2; + double z = 3; +} + +message Vector3f { + float x = 1; + float y = 2; + float z = 3; +} + +message Vector4f { + float x = 1; + float y = 2; + float z = 3; + float t = 4; +} + +message Quaterniond { + double x = 1; + double y = 2; + double z = 3; + double w = 4; +} + +message Quaternionf { + float x = 1; + float y = 2; + float z = 3; + float w = 4; +} + +message Rigid2d { + Vector2d translation = 1; + double rotation = 2; +} + +message Rigid2f { + Vector2f translation = 1; + float rotation = 2; +} + +message Rigid3d { + Vector3d translation = 1; + Quaterniond rotation = 2; +} + +message Rigid3f { + Vector3f translation = 1; + Quaternionf rotation = 2; +} diff --git a/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform.cc b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform.cc new file mode 100644 index 0000000..64a37c8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform.cc @@ -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. + */ + +#include "cartographer/transform/rigid_transform.h" + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "glog/logging.h" + +namespace cartographer { +namespace transform { + +namespace { + +Eigen::Vector3d TranslationFromDictionary( + common::LuaParameterDictionary* dictionary) { + const std::vector translation = dictionary->GetArrayValuesAsDoubles(); + CHECK_EQ(3, translation.size()) << "Need (x, y, z) for translation."; + return Eigen::Vector3d(translation[0], translation[1], translation[2]); +} + +} // namespace + +Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch, + const double yaw) { + const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX()); + const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY()); + const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ()); + return yaw_angle * pitch_angle * roll_angle; +} + +transform::Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary) { + const Eigen::Vector3d translation = + TranslationFromDictionary(dictionary->GetDictionary("translation").get()); + + auto rotation_dictionary = dictionary->GetDictionary("rotation"); + if (rotation_dictionary->HasKey("w")) { + const Eigen::Quaterniond rotation(rotation_dictionary->GetDouble("w"), + rotation_dictionary->GetDouble("x"), + rotation_dictionary->GetDouble("y"), + rotation_dictionary->GetDouble("z")); + CHECK_NEAR(rotation.norm(), 1., 1e-9); + return transform::Rigid3d(translation, rotation); + } else { + const std::vector rotation = + rotation_dictionary->GetArrayValuesAsDoubles(); + CHECK_EQ(3, rotation.size()) << "Need (roll, pitch, yaw) for rotation."; + return transform::Rigid3d( + translation, RollPitchYaw(rotation[0], rotation[1], rotation[2])); + } +} + +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform.h b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform.h new file mode 100644 index 0000000..0823037 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform.h @@ -0,0 +1,221 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ +#define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/strings/substitute.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" + +namespace cartographer { +namespace transform { + +template +class Rigid2 { + public: + using Vector = Eigen::Matrix; + using Rotation2D = Eigen::Rotation2D; + + Rigid2() : translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {} + Rigid2(const Vector& translation, const Rotation2D& rotation) + : translation_(translation), rotation_(rotation) {} + Rigid2(const Vector& translation, const double rotation) + : translation_(translation), rotation_(rotation) {} + + static Rigid2 Rotation(const double rotation) { + return Rigid2(Vector::Zero(), rotation); + } + + static Rigid2 Rotation(const Rotation2D& rotation) { + return Rigid2(Vector::Zero(), rotation); + } + + static Rigid2 Translation(const Vector& vector) { + return Rigid2(vector, Rotation2D::Identity()); + } + + static Rigid2 Identity() { return Rigid2(); } + + template + Rigid2 cast() const { + return Rigid2(translation_.template cast(), + rotation_.template cast()); + } + + const Vector& translation() const { return translation_; } + + Rotation2D rotation() const { return rotation_; } + + double normalized_angle() const { + return common::NormalizeAngleDifference(rotation().angle()); + } + + Rigid2 inverse() const { + const Rotation2D rotation = rotation_.inverse(); + const Vector translation = -(rotation * translation_); + return Rigid2(translation, rotation); + } + + std::string DebugString() const { + return absl::Substitute("{ t: [$0, $1], r: [$2] }", translation().x(), + translation().y(), rotation().angle()); + } + + private: + Vector translation_; + Rotation2D rotation_; +}; + +template +Rigid2 operator*(const Rigid2& lhs, + const Rigid2& rhs) { + return Rigid2( + lhs.rotation() * rhs.translation() + lhs.translation(), + lhs.rotation() * rhs.rotation()); +} + +template +typename Rigid2::Vector operator*( + const Rigid2& rigid, + const typename Rigid2::Vector& point) { + return rigid.rotation() * point + rigid.translation(); +} + +// This is needed for gmock. +template +std::ostream& operator<<(std::ostream& os, + const cartographer::transform::Rigid2& rigid) { + os << rigid.DebugString(); + return os; +} + +using Rigid2d = Rigid2; +using Rigid2f = Rigid2; + +template +class Rigid3 { + public: + using Vector = Eigen::Matrix; + using Quaternion = Eigen::Quaternion; + using AngleAxis = Eigen::AngleAxis; + + Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {} + Rigid3(const Vector& translation, const Quaternion& rotation) + : translation_(translation), rotation_(rotation) {} + Rigid3(const Vector& translation, const AngleAxis& rotation) + : translation_(translation), rotation_(rotation) {} + + static Rigid3 Rotation(const AngleAxis& angle_axis) { + return Rigid3(Vector::Zero(), Quaternion(angle_axis)); + } + + static Rigid3 Rotation(const Quaternion& rotation) { + return Rigid3(Vector::Zero(), rotation); + } + + static Rigid3 Translation(const Vector& vector) { + return Rigid3(vector, Quaternion::Identity()); + } + + static Rigid3 FromArrays(const std::array& rotation, + const std::array& translation) { + return Rigid3(Eigen::Map(translation.data()), + Eigen::Quaternion(rotation[0], rotation[1], + rotation[2], rotation[3])); + } + + static Rigid3 Identity() { return Rigid3(); } + + template + Rigid3 cast() const { + return Rigid3(translation_.template cast(), + rotation_.template cast()); + } + + const Vector& translation() const { return translation_; } + const Quaternion& rotation() const { return rotation_; } + + Rigid3 inverse() const { + const Quaternion rotation = rotation_.conjugate(); + const Vector translation = -(rotation * translation_); + return Rigid3(translation, rotation); + } + + std::string DebugString() const { + return absl::Substitute("{ t: [$0, $1, $2], q: [$3, $4, $5, $6] }", + translation().x(), translation().y(), + translation().z(), rotation().w(), rotation().x(), + rotation().y(), rotation().z()); + } + + bool IsValid() const { + return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) && + !std::isnan(translation_.z()) && + std::abs(FloatType(1) - rotation_.norm()) < FloatType(1e-3); + } + + private: + Vector translation_; + Quaternion rotation_; +}; + +template +Rigid3 operator*(const Rigid3& lhs, + const Rigid3& rhs) { + return Rigid3( + lhs.rotation() * rhs.translation() + lhs.translation(), + (lhs.rotation() * rhs.rotation()).normalized()); +} + +template +typename Rigid3::Vector operator*( + const Rigid3& rigid, + const typename Rigid3::Vector& point) { + return rigid.rotation() * point + rigid.translation(); +} + +// This is needed for gmock. +template +std::ostream& operator<<(std::ostream& os, + const cartographer::transform::Rigid3& rigid) { + os << rigid.DebugString(); + return os; +} + +using Rigid3d = Rigid3; +using Rigid3f = Rigid3; + +// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF +// specification http://wiki.ros.org/urdf/XML/joint. +Eigen::Quaterniond RollPitchYaw(double roll, double pitch, double yaw); + +// Returns an transform::Rigid3d given a 'dictionary' containing 'translation' +// (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw) +// or a dictionary with (w, x, y, z) values as a quaternion. +Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary); + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform_test.cc b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform_test.cc new file mode 100644 index 0000000..c1115e8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform_test.cc @@ -0,0 +1,98 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "cartographer/transform/rigid_transform.h" + +#include + +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +template +class RigidTransformTest : public ::testing::Test { + protected: + T eps() { return std::numeric_limits::epsilon(); } + + Rigid2 GetRandomRigid2() { + const T x = T(0.7) * distribution_(prng_); + const T y = T(0.7) * distribution_(prng_); + const T theta = T(0.2) * distribution_(prng_); + return transform::Rigid2(typename Rigid2::Vector(x, y), theta); + } + + Rigid3 GetRandomRigid3() { + const T x = T(0.7) * distribution_(prng_); + const T y = T(0.7) * distribution_(prng_); + const T z = T(0.7) * distribution_(prng_); + const T ax = T(0.7) * distribution_(prng_); + const T ay = T(0.7) * distribution_(prng_); + const T az = T(0.7) * distribution_(prng_); + return transform::Rigid3(typename Rigid3::Vector(x, y, z), + AngleAxisVectorToRotationQuaternion( + typename Rigid3::Vector(ax, ay, az))); + } + + std::mt19937 prng_ = std::mt19937(42); + std::uniform_real_distribution distribution_ = + std::uniform_real_distribution(-1., 1.); +}; + +using ScalarTypes = ::testing::Types; +TYPED_TEST_CASE(RigidTransformTest, ScalarTypes); + +TYPED_TEST(RigidTransformTest, Identity2DTest) { + const auto pose = this->GetRandomRigid2(); + EXPECT_THAT(pose * Rigid2(), IsNearly(pose, this->eps())); + EXPECT_THAT(Rigid2() * pose, IsNearly(pose, this->eps())); + EXPECT_THAT(pose * Rigid2::Identity(), + IsNearly(pose, this->eps())); + EXPECT_THAT(Rigid2::Identity() * pose, + IsNearly(pose, this->eps())); +} + +TYPED_TEST(RigidTransformTest, Inverse2DTest) { + const auto pose = this->GetRandomRigid2(); + EXPECT_THAT(pose.inverse() * pose, + IsNearly(Rigid2::Identity(), this->eps())); + EXPECT_THAT(pose * pose.inverse(), + IsNearly(Rigid2::Identity(), this->eps())); +} + +TYPED_TEST(RigidTransformTest, Identity3DTest) { + const auto pose = this->GetRandomRigid3(); + EXPECT_THAT(pose * Rigid3(), IsNearly(pose, this->eps())); + EXPECT_THAT(Rigid3() * pose, IsNearly(pose, this->eps())); + EXPECT_THAT(pose * Rigid3::Identity(), + IsNearly(pose, this->eps())); + EXPECT_THAT(Rigid3::Identity() * pose, + IsNearly(pose, this->eps())); +} + +TYPED_TEST(RigidTransformTest, Inverse3DTest) { + const auto pose = this->GetRandomRigid3(); + EXPECT_THAT(pose.inverse() * pose, + IsNearly(Rigid3::Identity(), this->eps())); + EXPECT_THAT(pose * pose.inverse(), + IsNearly(Rigid3::Identity(), this->eps())); +} + +} // namespace +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform_test_helpers.h b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform_test_helpers.h new file mode 100644 index 0000000..682c765 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/rigid_transform_test_helpers.h @@ -0,0 +1,51 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ +#define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/port.h" +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { + +template +Eigen::Transform ToEigen(const Rigid2& rigid2) { + return Eigen::Translation(rigid2.translation()) * rigid2.rotation(); +} + +template +Eigen::Transform ToEigen(const Rigid3& rigid3) { + return Eigen::Translation(rigid3.translation()) * rigid3.rotation(); +} + +MATCHER_P2(IsNearly, rigid, epsilon, + std::string(std::string(negation ? "isn't nearly " : "is nearly ") + + rigid.DebugString())) { + return ToEigen(arg).isApprox(ToEigen(rigid), epsilon); +} + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform.cc b/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform.cc new file mode 100644 index 0000000..fd68db0 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform.cc @@ -0,0 +1,54 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/transform/timestamped_transform.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace transform { + +TimestampedTransform Interpolate(const TimestampedTransform& start, + const TimestampedTransform& end, + const common::Time time) { + CHECK_LE(start.time, time); + CHECK_GE(end.time, time); + + const double duration = common::ToSeconds(end.time - start.time); + const double factor = common::ToSeconds(time - start.time) / duration; + const Eigen::Vector3d origin = + start.transform.translation() + + (end.transform.translation() - start.transform.translation()) * factor; + const Eigen::Quaterniond rotation = + Eigen::Quaterniond(start.transform.rotation()) + .slerp(factor, Eigen::Quaterniond(end.transform.rotation())); + return TimestampedTransform{time, transform::Rigid3d(origin, rotation)}; +} + +TimestampedTransform FromProto(const proto::TimestampedTransform& proto) { + return TimestampedTransform{common::FromUniversal(proto.time()), + ToRigid3(proto.transform())}; +} + +proto::TimestampedTransform ToProto(const TimestampedTransform& transform) { + proto::TimestampedTransform proto; + proto.set_time(common::ToUniversal(transform.time)); + *proto.mutable_transform() = ToProto(transform.transform); + return proto; +} + +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform.h b/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform.h new file mode 100644 index 0000000..dc87082 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform.h @@ -0,0 +1,42 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ +#define CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ + +#include "cartographer/common/time.h" +#include "cartographer/transform/proto/timestamped_transform.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace transform { + +struct TimestampedTransform { + common::Time time; + transform::Rigid3d transform; +}; + +TimestampedTransform FromProto(const proto::TimestampedTransform& proto); +proto::TimestampedTransform ToProto(const TimestampedTransform& transform); + +TimestampedTransform Interpolate(const TimestampedTransform& start, + const TimestampedTransform& end, + const common::Time time); + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform_test.cc b/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform_test.cc new file mode 100644 index 0000000..acc5381 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/timestamped_transform_test.cc @@ -0,0 +1,39 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/transform/timestamped_transform.h" + +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +TEST(TimestampedTransformTest, ToProtoAndBack) { + const TimestampedTransform expected{ + common::FromUniversal(12345678), + Rigid3d(Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(1., 2., 3., 4.).normalized())}; + const TimestampedTransform actual = FromProto(ToProto(expected)); + EXPECT_EQ(expected.time, actual.time); + EXPECT_THAT(actual.transform, IsNearly(expected.transform, 1e-6)); +} + +} // namespace +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/transform.cc b/carto_ws/src/cartographer-master/cartographer/transform/transform.cc new file mode 100644 index 0000000..851e646 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/transform.cc @@ -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. + */ + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace transform { + +Rigid2d ToRigid2(const proto::Rigid2d& transform) { + return Rigid2d({transform.translation().x(), transform.translation().y()}, + transform.rotation()); +} + +Eigen::Vector2d ToEigen(const proto::Vector2d& vector) { + return Eigen::Vector2d(vector.x(), vector.y()); +} + +Eigen::Vector3f ToEigen(const proto::Vector3f& vector) { + return Eigen::Vector3f(vector.x(), vector.y(), vector.z()); +} + +Eigen::Vector4f ToEigen(const proto::Vector4f& vector) { + return Eigen::Vector4f(vector.x(), vector.y(), vector.z(), vector.t()); +} + +Eigen::Vector3d ToEigen(const proto::Vector3d& vector) { + return Eigen::Vector3d(vector.x(), vector.y(), vector.z()); +} + +Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion) { + return Eigen::Quaterniond(quaternion.w(), quaternion.x(), quaternion.y(), + quaternion.z()); +} + +proto::Rigid2d ToProto(const transform::Rigid2d& transform) { + proto::Rigid2d proto; + proto.mutable_translation()->set_x(transform.translation().x()); + proto.mutable_translation()->set_y(transform.translation().y()); + proto.set_rotation(transform.rotation().angle()); + return proto; +} + +proto::Rigid2f ToProto(const transform::Rigid2f& transform) { + proto::Rigid2f proto; + proto.mutable_translation()->set_x(transform.translation().x()); + proto.mutable_translation()->set_y(transform.translation().y()); + proto.set_rotation(transform.rotation().angle()); + return proto; +} + +proto::Rigid3d ToProto(const transform::Rigid3d& rigid) { + proto::Rigid3d proto; + *proto.mutable_translation() = ToProto(rigid.translation()); + *proto.mutable_rotation() = ToProto(rigid.rotation()); + return proto; +} + +transform::Rigid3d ToRigid3(const proto::Rigid3d& rigid) { + return transform::Rigid3d(ToEigen(rigid.translation()), + ToEigen(rigid.rotation())); +} + +proto::Rigid3f ToProto(const transform::Rigid3f& rigid) { + proto::Rigid3f proto; + *proto.mutable_translation() = ToProto(rigid.translation()); + *proto.mutable_rotation() = ToProto(rigid.rotation()); + return proto; +} + +proto::Vector2d ToProto(const Eigen::Vector2d& vector) { + proto::Vector2d proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + return proto; +} + +proto::Vector3f ToProto(const Eigen::Vector3f& vector) { + proto::Vector3f proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + proto.set_z(vector.z()); + return proto; +} + +proto::Vector4f ToProto(const Eigen::Vector4f& vector) { + proto::Vector4f proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + proto.set_z(vector.z()); + proto.set_t(vector.w()); + return proto; +} + +proto::Vector3d ToProto(const Eigen::Vector3d& vector) { + proto::Vector3d proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + proto.set_z(vector.z()); + return proto; +} + +proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion) { + proto::Quaternionf proto; + proto.set_w(quaternion.w()); + proto.set_x(quaternion.x()); + proto.set_y(quaternion.y()); + proto.set_z(quaternion.z()); + return proto; +} + +proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion) { + proto::Quaterniond proto; + proto.set_w(quaternion.w()); + proto.set_x(quaternion.x()); + proto.set_y(quaternion.y()); + proto.set_z(quaternion.z()); + return proto; +} + +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/transform.h b/carto_ws/src/cartographer-master/cartographer/transform/transform.h new file mode 100644 index 0000000..b84f8bc --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/transform.h @@ -0,0 +1,139 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ +#define CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace transform { + +// Returns the non-negative rotation angle in radians of the 3D transformation +// 'transform'. +template +FloatType GetAngle(const Rigid3& transform) { + return FloatType(2) * std::atan2(transform.rotation().vec().norm(), + std::abs(transform.rotation().w())); +} + +// Returns the yaw component in radians of the given 3D 'rotation'. Assuming +// 'rotation' is composed of three rotations around X, then Y, then Z, returns +// the angle of the Z rotation. +template +T GetYaw(const Eigen::Quaternion& rotation) { + const Eigen::Matrix direction = + rotation * Eigen::Matrix::UnitX(); + return atan2(direction.y(), direction.x()); +} + +// Returns the yaw component in radians of the given 3D transformation +// 'transform'. +template +T GetYaw(const Rigid3& transform) { + return GetYaw(transform.rotation()); +} + +// Returns an angle-axis vector (a vector with the length of the rotation angle +// pointing to the direction of the rotation axis) representing the same +// rotation as the given 'quaternion'. +template +Eigen::Matrix RotationQuaternionToAngleAxisVector( + const Eigen::Quaternion& quaternion) { + Eigen::Quaternion normalized_quaternion = quaternion.normalized(); + // We choose the quaternion with positive 'w', i.e., the one with a smaller + // angle that represents this orientation. + if (normalized_quaternion.w() < 0.) { + // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 + normalized_quaternion.w() = -1. * normalized_quaternion.w(); + normalized_quaternion.x() = -1. * normalized_quaternion.x(); + normalized_quaternion.y() = -1. * normalized_quaternion.y(); + normalized_quaternion.z() = -1. * normalized_quaternion.z(); + } + // We convert the normalized_quaternion into a vector along the rotation axis + // with length of the rotation angle. + const T angle = + 2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w()); + constexpr double kCutoffAngle = 1e-7; // We linearize below this angle. + const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.); + return Eigen::Matrix(scale * normalized_quaternion.x(), + scale * normalized_quaternion.y(), + scale * normalized_quaternion.z()); +} + +// Returns a quaternion representing the same rotation as the given 'angle_axis' +// vector. +template +Eigen::Quaternion AngleAxisVectorToRotationQuaternion( + const Eigen::Matrix& angle_axis) { + T scale = T(0.5); + T w = T(1.); + constexpr double kCutoffAngle = 1e-8; // We linearize below this angle. + if (angle_axis.squaredNorm() > kCutoffAngle) { + const T norm = angle_axis.norm(); + scale = sin(norm / 2.) / norm; + w = cos(norm / 2.); + } + const Eigen::Matrix quaternion_xyz = scale * angle_axis; + return Eigen::Quaternion(w, quaternion_xyz.x(), quaternion_xyz.y(), + quaternion_xyz.z()); +} + +// Projects 'transform' onto the XY plane. +template +Rigid2 Project2D(const Rigid3& transform) { + return Rigid2(transform.translation().template head<2>(), + GetYaw(transform)); +} + +// Embeds 'transform' into 3D space in the XY plane. +template +Rigid3 Embed3D(const Rigid2& transform) { + return Rigid3( + {transform.translation().x(), transform.translation().y(), T(0)}, + Eigen::AngleAxis(transform.rotation().angle(), + Eigen::Matrix::UnitZ())); +} + +// Conversions between Eigen and proto. +Rigid2d ToRigid2(const proto::Rigid2d& transform); +Eigen::Vector2d ToEigen(const proto::Vector2d& vector); +Eigen::Vector3f ToEigen(const proto::Vector3f& vector); +Eigen::Vector4f ToEigen(const proto::Vector4f& vector); +Eigen::Vector3d ToEigen(const proto::Vector3d& vector); +Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion); +proto::Rigid2d ToProto(const Rigid2d& transform); +proto::Rigid2f ToProto(const Rigid2f& transform); +proto::Rigid3d ToProto(const Rigid3d& rigid); +Rigid3d ToRigid3(const proto::Rigid3d& rigid); +proto::Rigid3f ToProto(const Rigid3f& rigid); +proto::Vector2d ToProto(const Eigen::Vector2d& vector); +proto::Vector3f ToProto(const Eigen::Vector3f& vector); +proto::Vector4f ToProto(const Eigen::Vector4f& vector); +proto::Vector3d ToProto(const Eigen::Vector3d& vector); +proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion); +proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion); + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer.cc b/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer.cc new file mode 100644 index 0000000..ae65721 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer.cc @@ -0,0 +1,106 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/transform/transform_interpolation_buffer.h" + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace transform { + +TransformInterpolationBuffer::TransformInterpolationBuffer( + const mapping::proto::Trajectory& trajectory) { + for (const mapping::proto::Trajectory::Node& node : trajectory.node()) { + Push(common::FromUniversal(node.timestamp()), + transform::ToRigid3(node.pose())); + } +} + +void TransformInterpolationBuffer::Push(const common::Time time, + const transform::Rigid3d& transform) { + if (!timestamped_transforms_.empty()) { + CHECK_GE(time, latest_time()) << "New transform is older than latest."; + } + timestamped_transforms_.push_back(TimestampedTransform{time, transform}); + RemoveOldTransformsIfNeeded(); +} + +void TransformInterpolationBuffer::SetSizeLimit( + const size_t buffer_size_limit) { + buffer_size_limit_ = buffer_size_limit; + RemoveOldTransformsIfNeeded(); +} + +void TransformInterpolationBuffer::Clear() { timestamped_transforms_.clear(); } + +bool TransformInterpolationBuffer::Has(const common::Time time) const { + if (timestamped_transforms_.empty()) { + return false; + } + return earliest_time() <= time && time <= latest_time(); +} + +transform::Rigid3d TransformInterpolationBuffer::Lookup( + const common::Time time) const { + CHECK(Has(time)) << "Missing transform for: " << time; + const auto end = std::lower_bound( + timestamped_transforms_.begin(), timestamped_transforms_.end(), time, + [](const TimestampedTransform& timestamped_transform, + const common::Time time) { + return timestamped_transform.time < time; + }); + if (end->time == time) { + return end->transform; + } + const auto start = std::prev(end); + return Interpolate(*start, *end, time).transform; +} + +void TransformInterpolationBuffer::RemoveOldTransformsIfNeeded() { + while (timestamped_transforms_.size() > buffer_size_limit_) { + timestamped_transforms_.pop_front(); + } +} + +common::Time TransformInterpolationBuffer::earliest_time() const { + CHECK(!empty()) << "Empty buffer."; + return timestamped_transforms_.front().time; +} + +common::Time TransformInterpolationBuffer::latest_time() const { + CHECK(!empty()) << "Empty buffer."; + return timestamped_transforms_.back().time; +} + +bool TransformInterpolationBuffer::empty() const { + return timestamped_transforms_.empty(); +} + +size_t TransformInterpolationBuffer::size_limit() const { + return buffer_size_limit_; +} + +size_t TransformInterpolationBuffer::size() const { + return timestamped_transforms_.size(); +} + +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer.h b/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer.h new file mode 100644 index 0000000..84a7dcd --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer.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. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ +#define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace transform { + +constexpr size_t kUnlimitedBufferSize = std::numeric_limits::max(); + +// A time-ordered buffer of transforms that supports interpolated lookups. +// Unless explicitly set, the buffer size is unlimited. +class TransformInterpolationBuffer { + public: + TransformInterpolationBuffer() = default; + explicit TransformInterpolationBuffer( + const mapping::proto::Trajectory& trajectory); + + // Sets the transform buffer size limit and removes old transforms + // if it is exceeded. + void SetSizeLimit(size_t buffer_size_limit); + + // Adds a new transform to the buffer and removes the oldest transform if the + // buffer size limit is exceeded. + void Push(common::Time time, const transform::Rigid3d& transform); + + // Clears the transform buffer. + void Clear(); + + // Returns true if an interpolated transform can be computed at 'time'. + bool Has(common::Time time) const; + + // Returns an interpolated transform at 'time'. CHECK()s that a transform at + // 'time' is available. + transform::Rigid3d Lookup(common::Time time) const; + + // Returns the timestamp of the earliest transform in the buffer or 0 if the + // buffer is empty. + common::Time earliest_time() const; + + // Returns the timestamp of the earliest transform in the buffer or 0 if the + // buffer is empty. + common::Time latest_time() const; + + // Returns true if the buffer is empty. + bool empty() const; + + // Returns the maximum allowed size of the transform buffer. + size_t size_limit() const; + + // Returns the current size of the transform buffer. + size_t size() const; + + private: + void RemoveOldTransformsIfNeeded(); + + std::deque timestamped_transforms_; + size_t buffer_size_limit_ = kUnlimitedBufferSize; +}; + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ diff --git a/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer_test.cc b/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer_test.cc new file mode 100644 index 0000000..b78d569 --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/transform_interpolation_buffer_test.cc @@ -0,0 +1,91 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/transform/transform_interpolation_buffer.h" + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +TEST(TransformInterpolationBufferTest, testHas) { + TransformInterpolationBuffer buffer; + EXPECT_FALSE(buffer.Has(common::FromUniversal(50))); + buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity()); + EXPECT_FALSE(buffer.Has(common::FromUniversal(25))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(50))); + EXPECT_FALSE(buffer.Has(common::FromUniversal(75))); + buffer.Push(common::FromUniversal(100), transform::Rigid3d::Identity()); + EXPECT_FALSE(buffer.Has(common::FromUniversal(25))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(50))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(75))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(100))); + EXPECT_FALSE(buffer.Has(common::FromUniversal(125))); + EXPECT_EQ(common::FromUniversal(50), buffer.earliest_time()); + EXPECT_EQ(common::FromUniversal(100), buffer.latest_time()); +} + +TEST(TransformInterpolationBufferTest, testLookup) { + TransformInterpolationBuffer buffer; + buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity()); + // The rotation needs to be relatively small in order for the interpolation to + // remain a z-axis rotation. + buffer.Push(common::FromUniversal(100), + transform::Rigid3d::Translation(Eigen::Vector3d(10., 10., 10.)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(2., Eigen::Vector3d::UnitZ()))); + const common::Time time = common::FromUniversal(75); + const transform::Rigid3d interpolated = buffer.Lookup(time); + EXPECT_THAT( + interpolated, + IsNearly(transform::Rigid3d::Translation(Eigen::Vector3d(5., 5., 5.)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(1., Eigen::Vector3d::UnitZ())), + 1e-6)); +} + +TEST(TransformInterpolationBufferTest, testLookupSingleTransform) { + TransformInterpolationBuffer buffer; + const common::Time time = common::FromUniversal(75); + buffer.Push(time, transform::Rigid3d::Identity()); + const transform::Rigid3d interpolated = buffer.Lookup(time); + EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6)); +} + +TEST(TransformInterpolationBufferTest, testSetSizeLimit) { + TransformInterpolationBuffer buffer; + EXPECT_EQ(buffer.size_limit(), kUnlimitedBufferSize); + buffer.Push(common::FromUniversal(0), transform::Rigid3d::Identity()); + buffer.Push(common::FromUniversal(1), transform::Rigid3d::Identity()); + buffer.Push(common::FromUniversal(2), transform::Rigid3d::Identity()); + EXPECT_EQ(buffer.size(), 3); + buffer.SetSizeLimit(2); + EXPECT_EQ(buffer.size_limit(), 2); + EXPECT_EQ(buffer.size(), 2); + EXPECT_FALSE(buffer.Has(common::FromUniversal(0))); + buffer.Push(common::FromUniversal(3), transform::Rigid3d::Identity()); + EXPECT_EQ(buffer.size(), 2); + EXPECT_FALSE(buffer.Has(common::FromUniversal(1))); +} + +} // namespace +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cartographer/transform/transform_test.cc b/carto_ws/src/cartographer-master/cartographer/transform/transform_test.cc new file mode 100644 index 0000000..277d53c --- /dev/null +++ b/carto_ws/src/cartographer-master/cartographer/transform/transform_test.cc @@ -0,0 +1,74 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/transform/transform.h" + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +TEST(TransformTest, GetAngle) { + std::mt19937 rng(42); + std::uniform_real_distribution angle_distribution(0.f, M_PI); + std::uniform_real_distribution position_distribution(-1.f, 1.f); + + for (int i = 0; i != 100; ++i) { + const float angle = angle_distribution(rng); + const float x = position_distribution(rng); + const float y = position_distribution(rng); + const float z = position_distribution(rng); + const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized(); + EXPECT_NEAR(angle, + GetAngle(Rigid3f::Rotation(AngleAxisVectorToRotationQuaternion( + Eigen::Vector3f(angle * axis)))), + 1e-6f); + } +} + +TEST(TransformTest, GetYaw) { + const auto rotation = + Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ())); + EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9); + EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9); +} + +TEST(TransformTest, GetYawAxisOrdering) { + const auto rotation = + Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX())); + EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9); +} + +TEST(TransformTest, Embed3D) { + const Rigid2d rigid2d({1., 2.}, 0.); + const Rigid3d rigid3d( + Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) * + Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized())); + const Rigid3d expected = + rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.)); + EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9)); +} + +} // namespace +} // namespace transform +} // namespace cartographer diff --git a/carto_ws/src/cartographer-master/cmake/functions.cmake b/carto_ws/src/cartographer-master/cmake/functions.cmake new file mode 100644 index 0000000..b3f57e8 --- /dev/null +++ b/carto_ws/src/cartographer-master/cmake/functions.cmake @@ -0,0 +1,143 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +include(CMakeParseArguments) + +macro(_parse_arguments ARGS) + set(OPTIONS) + set(ONE_VALUE_ARG) + set(MULTI_VALUE_ARGS SRCS) + cmake_parse_arguments(ARG + "${OPTIONS}" "${ONE_VALUE_ARG}" "${MULTI_VALUE_ARGS}" ${ARGS}) +endmacro(_parse_arguments) + +macro(_common_compile_stuff VISIBILITY) + set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") + + set_target_properties(${NAME} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + + target_include_directories(${NAME} PUBLIC ${PROJECT_NAME}) + target_link_libraries(${NAME} PUBLIC ${PROJECT_NAME}) +endmacro(_common_compile_stuff) + +function(google_test NAME ARG_SRC) + add_executable(${NAME} ${ARG_SRC}) + _common_compile_stuff("PRIVATE") + + # Make sure that gmock always includes the correct gtest/gtest.h. + target_include_directories("${NAME}" SYSTEM PRIVATE + "${GMOCK_INCLUDE_DIRS}") + target_link_libraries("${NAME}" PUBLIC ${GMOCK_LIBRARIES}) + + add_test(${NAME} ${NAME}) +endfunction() + +function(google_binary NAME) + _parse_arguments("${ARGN}") + + add_executable(${NAME} ${ARG_SRCS}) + + _common_compile_stuff("PRIVATE") + + install(TARGETS "${NAME}" RUNTIME DESTINATION bin) +endfunction() + +# Create a variable 'VAR_NAME'='FLAG'. If VAR_NAME is already set, FLAG is +# appended. +function(google_add_flag VAR_NAME FLAG) + if (${VAR_NAME}) + set(${VAR_NAME} "${${VAR_NAME}} ${FLAG}" PARENT_SCOPE) + else() + set(${VAR_NAME} "${FLAG}" PARENT_SCOPE) + endif() +endfunction() + +macro(google_initialize_cartographer_project) + if(CARTOGRAPHER_CMAKE_DIR) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} + ${CARTOGRAPHER_CMAKE_DIR}/modules) + else() + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules) + endif() + + if(WIN32) + # TODO turn on equivalent warnings on Windows + else() + set(GOOG_CXX_FLAGS "-pthread -fPIC ${GOOG_CXX_FLAGS}") + + google_add_flag(GOOG_CXX_FLAGS "-Wall") + google_add_flag(GOOG_CXX_FLAGS "-Wpedantic") + + # Turn some warnings into errors. + google_add_flag(GOOG_CXX_FLAGS "-Werror=format-security") + google_add_flag(GOOG_CXX_FLAGS "-Werror=missing-braces") + google_add_flag(GOOG_CXX_FLAGS "-Werror=reorder") + google_add_flag(GOOG_CXX_FLAGS "-Werror=return-type") + google_add_flag(GOOG_CXX_FLAGS "-Werror=switch") + google_add_flag(GOOG_CXX_FLAGS "-Werror=uninitialized") + + if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" OR CMAKE_CXX_COMPILER_ID MATCHES "AppleClang") + google_add_flag(GOOG_CXX_FLAGS "-Wthread-safety") + endif() + + 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") + if(FORCE_DEBUG_BUILD) + message(WARNING "Building in Debug mode, expect very slow performance.") + google_add_flag(GOOG_CXX_FLAGS "-g") + else() + message(FATAL_ERROR + "Compiling in Debug mode is not supported and can cause severely degraded performance. " + "You should change the build type to Release. If you want to build in Debug mode anyway, " + "call CMake with -DFORCE_DEBUG_BUILD=True" + ) + endif() + # Support for Debian packaging CMAKE_BUILD_TYPE + elseif(CMAKE_BUILD_TYPE STREQUAL "None") + message(WARNING "Building with CMAKE_BUILD_TYPE None, " + "please make sure you have set CFLAGS and CXXFLAGS according to your needs.") + else() + message(FATAL_ERROR "Unknown CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}") + endif() + + message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") + + # Add a hook that reruns CMake when source files are added or removed. + set(LIST_FILES_CMD "find ${PROJECT_SOURCE_DIR}/ -not -iwholename '*.git*' | sort | sed 's/^/#/'") + set(FILES_LIST_PATH "${PROJECT_BINARY_DIR}/AllFiles.cmake") + set(DETECT_CHANGES_CMD "bash" "-c" "${LIST_FILES_CMD} | diff -N -q ${FILES_LIST_PATH} - || ${LIST_FILES_CMD} > ${FILES_LIST_PATH}") + add_custom_target(${PROJECT_NAME}_detect_changes ALL + COMMAND ${DETECT_CHANGES_CMD} + VERBATIM + ) + if(NOT EXISTS ${FILES_LIST_PATH}) + execute_process(COMMAND ${DETECT_CHANGES_CMD}) + endif() + include(${FILES_LIST_PATH}) + endif() +endmacro() + +macro(google_enable_testing) + enable_testing() + find_package(GMock REQUIRED) +endmacro() diff --git a/carto_ws/src/cartographer-master/cmake/modules/FindEigen3.cmake b/carto_ws/src/cartographer-master/cmake/modules/FindEigen3.cmake new file mode 100644 index 0000000..9838e92 --- /dev/null +++ b/carto_ws/src/cartographer-master/cmake/modules/FindEigen3.cmake @@ -0,0 +1,30 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +find_package(Eigen3 QUIET NO_MODULE) +if (NOT EIGEN3_FOUND) + list(APPEND EIGEN3_POSSIBLE_DIRS + /usr/local/include/eigen3 + /usr/include/eigen3 + ) + find_path(EIGEN3_INCLUDE_DIR + NAMES Eigen/Core + PATHS ${EIGEN3_POSSIBLE_DIRS} + ) + if (EIGEN3_INCLUDE_DIR AND EXISTS ${EIGEN3_INCLUDE_DIR}) + set(EIGEN3_FOUND TRUE) + else() + message(WARNING "Failed to find Eigen3. Please, define the path manually.") + endif() +endif() diff --git a/carto_ws/src/cartographer-master/cmake/modules/FindGMock.cmake b/carto_ws/src/cartographer-master/cmake/modules/FindGMock.cmake new file mode 100644 index 0000000..b29c0fe --- /dev/null +++ b/carto_ws/src/cartographer-master/cmake/modules/FindGMock.cmake @@ -0,0 +1,94 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +if(NOT GMock_FOUND) + find_path(GMOCK_INCLUDE_DIRS gmock/gmock.h + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES include + PATHS + /usr + ) + + # Find system-wide installed gmock. + find_library(GMOCK_LIBRARIES + NAMES gmock_main + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + if(GMOCK_LIBRARIES) + find_library(GMOCK_LIBRARY + NAMES gmock + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + find_library(GTEST_LIBRARY + NAMES gtest + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + list(APPEND GMOCK_LIBRARIES ${GMOCK_LIBRARY} ${GTEST_LIBRARY}) + endif() + + # Find system-wide gtest header. + find_path(GTEST_INCLUDE_DIRS gtest/gtest.h + HINTS + ENV GTEST_DIR + PATH_SUFFIXES include + PATHS + /usr + ) + list(APPEND GMOCK_INCLUDE_DIRS ${GTEST_INCLUDE_DIRS}) + + if(NOT GMOCK_LIBRARIES) + # If no system-wide gmock found, then find src version. + # Ubuntu might have this. + find_path(GMOCK_SRC_DIR src/gmock.cc + HINTS + ENV GMOCK_DIR + PATHS + /usr/src/googletest/googlemock + /usr/src/gmock + ) + if(GMOCK_SRC_DIR) + # If src version found, build it. + if(NOT TARGET gmock) + add_subdirectory(${GMOCK_SRC_DIR} "${CMAKE_CURRENT_BINARY_DIR}/gmock" + EXCLUDE_FROM_ALL) + endif() + set(GMOCK_LIBRARIES gmock_main) + set(GMOCK_INCLUDE_DIRS ${GMOCK_SRC_DIR}/include) + endif() + endif() + + # System-wide installed gmock library might require pthreads. + find_package(Threads REQUIRED) + list(APPEND GMOCK_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GMock DEFAULT_MSG GMOCK_LIBRARIES + GMOCK_INCLUDE_DIRS) + +# Mitigate build issue with Catkin +set(GMOCK_FOUND FALSE) diff --git a/carto_ws/src/cartographer-master/cmake/modules/FindLuaGoogle.cmake b/carto_ws/src/cartographer-master/cmake/modules/FindLuaGoogle.cmake new file mode 100644 index 0000000..c76b8cc --- /dev/null +++ b/carto_ws/src/cartographer-master/cmake/modules/FindLuaGoogle.cmake @@ -0,0 +1,220 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# TODO(hrapp): Take from the cmake master branch and simplified. As soon as we +# require a CMakeVersion that ships with this, remove again. + +#.rst: +# FindLua +# ------- +# +# +# +# Locate Lua library This module defines +# +# :: +# +# LUA_FOUND - if false, do not try to link to Lua +# LUA_LIBRARIES - both lua and lualib +# LUA_INCLUDE_DIR - where to find lua.h +# LUA_VERSION_STRING - the version of Lua found +# LUA_VERSION_MAJOR - the major version of Lua +# LUA_VERSION_MINOR - the minor version of Lua +# LUA_VERSION_PATCH - the patch version of Lua +# +# +# +# Note that the expected include convention is +# +# :: +# +# #include "lua.h" +# +# and not +# +# :: +# +# #include +# +# This is because, the lua location is not standardized and may exist in +# locations other than lua/ + +#============================================================================= +# Copyright 2007-2009 Kitware, Inc. +# Copyright 2013 Rolf Eike Beer +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + +unset(_lua_include_subdirs) +unset(_lua_library_names) +unset(_lua_append_versions) + +# this is a function only to have all the variables inside go away automatically +function(_lua_set_version_vars) + set(LUA_VERSIONS5 5.3 5.2) + + if (Lua_FIND_VERSION_EXACT) + if (Lua_FIND_VERSION_COUNT GREATER 1) + set(_lua_append_versions ${Lua_FIND_VERSION_MAJOR}.${Lua_FIND_VERSION_MINOR}) + endif () + elseif (Lua_FIND_VERSION) + # once there is a different major version supported this should become a loop + if (NOT Lua_FIND_VERSION_MAJOR GREATER 5) + if (Lua_FIND_VERSION_COUNT EQUAL 1) + set(_lua_append_versions ${LUA_VERSIONS5}) + else () + foreach (subver IN LISTS LUA_VERSIONS5) + if (NOT subver VERSION_LESS ${Lua_FIND_VERSION}) + list(APPEND _lua_append_versions ${subver}) + endif () + endforeach () + endif () + endif () + else () + # once there is a different major version supported this should become a loop + set(_lua_append_versions ${LUA_VERSIONS5}) + endif () + + list(APPEND _lua_include_subdirs "include/lua" "include") + + foreach (ver IN LISTS _lua_append_versions) + string(REGEX MATCH "^([0-9]+)\\.([0-9]+)$" _ver "${ver}") + list(APPEND _lua_include_subdirs + include/lua${CMAKE_MATCH_1}${CMAKE_MATCH_2} + include/lua${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + include/lua-${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + ) + list(APPEND _lua_library_names + lua${CMAKE_MATCH_1}${CMAKE_MATCH_2} + lua${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + lua-${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + lua.${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + ) + endforeach () + + set(_lua_include_subdirs "${_lua_include_subdirs}" PARENT_SCOPE) + set(_lua_library_names "${_lua_library_names}" PARENT_SCOPE) + set(_lua_append_versions "${_lua_append_versions}" PARENT_SCOPE) +endfunction(_lua_set_version_vars) + +function(_lua_check_header_version _hdr_file) + # At least 5.[012] have different ways to express the version + # so all of them need to be tested. Lua 5.2 defines LUA_VERSION + # and LUA_RELEASE as joined by the C preprocessor, so avoid those. + file(STRINGS "${_hdr_file}" lua_version_strings + REGEX "^#define[ \t]+LUA_(RELEASE[ \t]+\"Lua [0-9]|VERSION([ \t]+\"Lua [0-9]|_[MR])).*") + + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION_MAJOR[ \t]+\"([0-9])\"[ \t]*;.*" "\\1" LUA_VERSION_MAJOR ";${lua_version_strings};") + if (LUA_VERSION_MAJOR MATCHES "^[0-9]+$") + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION_MINOR[ \t]+\"([0-9])\"[ \t]*;.*" "\\1" LUA_VERSION_MINOR ";${lua_version_strings};") + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION_RELEASE[ \t]+\"([0-9])\"[ \t]*;.*" "\\1" LUA_VERSION_PATCH ";${lua_version_strings};") + set(LUA_VERSION_STRING "${LUA_VERSION_MAJOR}.${LUA_VERSION_MINOR}.${LUA_VERSION_PATCH}") + else () + string(REGEX REPLACE ".*;#define[ \t]+LUA_RELEASE[ \t]+\"Lua ([0-9.]+)\"[ \t]*;.*" "\\1" LUA_VERSION_STRING ";${lua_version_strings};") + if (NOT LUA_VERSION_STRING MATCHES "^[0-9.]+$") + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION[ \t]+\"Lua ([0-9.]+)\"[ \t]*;.*" "\\1" LUA_VERSION_STRING ";${lua_version_strings};") + endif () + string(REGEX REPLACE "^([0-9]+)\\.[0-9.]*$" "\\1" LUA_VERSION_MAJOR "${LUA_VERSION_STRING}") + string(REGEX REPLACE "^[0-9]+\\.([0-9]+)[0-9.]*$" "\\1" LUA_VERSION_MINOR "${LUA_VERSION_STRING}") + string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]).*" "\\1" LUA_VERSION_PATCH "${LUA_VERSION_STRING}") + endif () + foreach (ver IN LISTS _lua_append_versions) + if (ver STREQUAL "${LUA_VERSION_MAJOR}.${LUA_VERSION_MINOR}") + set(LUA_VERSION_MAJOR ${LUA_VERSION_MAJOR} PARENT_SCOPE) + set(LUA_VERSION_MINOR ${LUA_VERSION_MINOR} PARENT_SCOPE) + set(LUA_VERSION_PATCH ${LUA_VERSION_PATCH} PARENT_SCOPE) + set(LUA_VERSION_STRING ${LUA_VERSION_STRING} PARENT_SCOPE) + return() + endif () + endforeach () +endfunction(_lua_check_header_version) + +_lua_set_version_vars() + +if (LUA_INCLUDE_DIR AND EXISTS "${LUA_INCLUDE_DIR}/lua.h") + _lua_check_header_version("${LUA_INCLUDE_DIR}/lua.h") +endif () + +if (NOT LUA_VERSION_STRING) + foreach (subdir IN LISTS _lua_include_subdirs) + unset(LUA_INCLUDE_PREFIX CACHE) + find_path(LUA_INCLUDE_PREFIX ${subdir}/lua.h + HINTS + ENV LUA_DIR + PATHS + ~/Library/Frameworks + /Library/Frameworks + /sw # Fink + /opt/local # DarwinPorts + /opt/csw # Blastwave + /opt + ) + if (LUA_INCLUDE_PREFIX) + _lua_check_header_version("${LUA_INCLUDE_PREFIX}/${subdir}/lua.h") + if (LUA_VERSION_STRING) + set(LUA_INCLUDE_DIR "${LUA_INCLUDE_PREFIX}/${subdir}") + break() + endif () + endif () + endforeach () +endif () +unset(_lua_include_subdirs) +unset(_lua_append_versions) + +find_library(LUA_LIBRARY + NAMES ${_lua_library_names} lua + HINTS + ENV LUA_DIR + PATH_SUFFIXES lib + PATHS + ~/Library/Frameworks + /Library/Frameworks + /sw + /opt/local + /opt/csw + /opt +) +unset(_lua_library_names) + +if (LUA_LIBRARY) + # include the math library for Unix + if (UNIX AND NOT APPLE AND NOT BEOS) + find_library(LUA_MATH_LIBRARY m) + set(LUA_LIBRARIES "${LUA_LIBRARY};${LUA_MATH_LIBRARY}") + # For Windows and Mac, don't need to explicitly include the math library + else () + set(LUA_LIBRARIES "${LUA_LIBRARY}") + endif () +endif () + +# handle the QUIETLY and REQUIRED arguments and set LUA_FOUND to TRUE if +# all listed variables are TRUE +FIND_PACKAGE_HANDLE_STANDARD_ARGS(Lua + REQUIRED_VARS LUA_LIBRARIES LUA_INCLUDE_DIR + VERSION_VAR LUA_VERSION_STRING) +mark_as_advanced(LUA_INCLUDE_DIR LUA_LIBRARY LUA_MATH_LIBRARY) + +if (NOT LUA_FOUND) + MESSAGE(FATAL_ERROR "Did not find Lua >= 5.2.") +endif () + + diff --git a/carto_ws/src/cartographer-master/cmake/modules/FindSphinx.cmake b/carto_ws/src/cartographer-master/cmake/modules/FindSphinx.cmake new file mode 100644 index 0000000..f956f3d --- /dev/null +++ b/carto_ws/src/cartographer-master/cmake/modules/FindSphinx.cmake @@ -0,0 +1,24 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# TODO(hrapp): Replace through the one of ceres + +find_program(SPHINX_EXECUTABLE + NAMES sphinx-build + PATHS + /usr/bin + DOC "Sphinx") + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Sphinx DEFAULT_MSG SPHINX_EXECUTABLE) diff --git a/carto_ws/src/cartographer-master/configuration_files/map_builder.lua b/carto_ws/src/cartographer-master/configuration_files/map_builder.lua new file mode 100644 index 0000000..3f5479c --- /dev/null +++ b/carto_ws/src/cartographer-master/configuration_files/map_builder.lua @@ -0,0 +1,23 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT 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 "pose_graph.lua" + +MAP_BUILDER = { + use_trajectory_builder_2d = false, + use_trajectory_builder_3d = false, + num_background_threads = 4, + pose_graph = POSE_GRAPH, + collate_by_trajectory = false, +} diff --git a/carto_ws/src/cartographer-master/configuration_files/map_builder_server.lua b/carto_ws/src/cartographer-master/configuration_files/map_builder_server.lua new file mode 100644 index 0000000..0f8fe3a --- /dev/null +++ b/carto_ws/src/cartographer-master/configuration_files/map_builder_server.lua @@ -0,0 +1,28 @@ +-- Copyright 2017 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT 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 "map_builder.lua" + +MAP_BUILDER_SERVER = { + map_builder = MAP_BUILDER, + num_event_threads = 4, + num_grpc_threads = 4, + server_address = "0.0.0.0:50051", + uplink_server_address = "", + upload_batch_size = 100, + enable_ssl_encryption = false, + enable_google_auth = false, +} + +MAP_BUILDER.collate_by_trajectory = true diff --git a/carto_ws/src/cartographer-master/configuration_files/pose_graph.lua b/carto_ws/src/cartographer-master/configuration_files/pose_graph.lua new file mode 100644 index 0000000..a962641 --- /dev/null +++ b/carto_ws/src/cartographer-master/configuration_files/pose_graph.lua @@ -0,0 +1,95 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +POSE_GRAPH = { + optimize_every_n_nodes = 90, + constraint_builder = { + sampling_ratio = 0.3, + max_constraint_distance = 15., + min_score = 0.55, + global_localization_min_score = 0.6, + loop_closure_translation_weight = 1.1e4, + loop_closure_rotation_weight = 1e5, + log_matches = true, + fast_correlative_scan_matcher = { + linear_search_window = 7., + angular_search_window = math.rad(30.), + branch_and_bound_depth = 7, + }, + ceres_scan_matcher = { + occupied_space_weight = 20., + translation_weight = 10., + rotation_weight = 1., + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 10, + num_threads = 1, + }, + }, + fast_correlative_scan_matcher_3d = { + branch_and_bound_depth = 8, + full_resolution_depth = 3, + min_rotational_score = 0.77, + min_low_resolution_score = 0.55, + linear_xy_search_window = 5., + linear_z_search_window = 1., + angular_search_window = math.rad(15.), + }, + ceres_scan_matcher_3d = { + occupied_space_weight_0 = 5., + occupied_space_weight_1 = 30., + translation_weight = 10., + rotation_weight = 1., + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 10, + num_threads = 1, + }, + }, + }, + matcher_translation_weight = 5e2, + matcher_rotation_weight = 1.6e3, + optimization_problem = { + huber_scale = 1e1, + acceleration_weight = 1.1e2, + rotation_weight = 1.6e4, + local_slam_pose_translation_weight = 1e5, + local_slam_pose_rotation_weight = 1e5, + odometry_translation_weight = 1e5, + odometry_rotation_weight = 1e5, + fixed_frame_pose_translation_weight = 1e1, + fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, + log_solver_summary = false, + use_online_imu_extrinsics_in_3d = true, + fix_z_in_3d = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 50, + num_threads = 7, + }, + }, + max_num_final_iterations = 200, + global_sampling_ratio = 0.003, + log_residual_histograms = true, + global_constraint_search_after_n_seconds = 10., + -- overlapping_submaps_trimmer_2d = { + -- fresh_submaps_count = 1, + -- min_covered_area = 2, + -- min_added_submaps_count = 5, + -- }, +} diff --git a/carto_ws/src/cartographer-master/configuration_files/trajectory_builder.lua b/carto_ws/src/cartographer-master/configuration_files/trajectory_builder.lua new file mode 100644 index 0000000..11c2e3e --- /dev/null +++ b/carto_ws/src/cartographer-master/configuration_files/trajectory_builder.lua @@ -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. + +include "trajectory_builder_2d.lua" +include "trajectory_builder_3d.lua" + +TRAJECTORY_BUILDER = { + trajectory_builder_2d = TRAJECTORY_BUILDER_2D, + trajectory_builder_3d = TRAJECTORY_BUILDER_3D, +-- pure_localization_trimmer = { +-- max_submaps_to_keep = 3, +-- }, + collate_fixed_frame = true, + collate_landmarks = false, +} diff --git a/carto_ws/src/cartographer-master/configuration_files/trajectory_builder_2d.lua b/carto_ws/src/cartographer-master/configuration_files/trajectory_builder_2d.lua new file mode 100644 index 0000000..8ff8bce --- /dev/null +++ b/carto_ws/src/cartographer-master/configuration_files/trajectory_builder_2d.lua @@ -0,0 +1,115 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +TRAJECTORY_BUILDER_2D = { + use_imu_data = true, + min_range = 0., + max_range = 30., + min_z =-0.5, ---0.8, + max_z = 0.5, --2., + missing_data_ray_length = 5., + num_accumulated_range_data = 1, + voxel_filter_size = 0.025, + + adaptive_voxel_filter = { + max_length = 0.5, + min_num_points = 100, --200, + max_range = 6, --50., + }, + + loop_closure_adaptive_voxel_filter = { + max_length = 0.9, + min_num_points = 100, + max_range = 50., + }, + + use_online_correlative_scan_matching = true, --false, + real_time_correlative_scan_matcher = { + linear_search_window = 0.2, --0.1, + angular_search_window = math.rad(20.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1e-1, + }, + + ceres_scan_matcher = { + occupied_space_weight = 1., + translation_weight = 10., + rotation_weight = 40., + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 20, + num_threads = 1, + }, + }, + + motion_filter = { + max_time_seconds = 5., + max_distance_meters = 0.2, + max_angle_radians = math.rad(1.), + }, + + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. + imu_gravity_time_constant = 10., + pose_extrapolator = { + use_imu_based = false, + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, + }, + + submaps = { + num_range_data = 90, + grid_options_2d = { + grid_type = "PROBABILITY_GRID", + resolution = 0.05, + }, + range_data_inserter = { + range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", + probability_grid_range_data_inserter = { + insert_free_space = true, + hit_probability = 0.55, + miss_probability = 0.49, + }, + tsdf_range_data_inserter = { + truncation_distance = 0.3, + maximum_weight = 10., + update_free_space = false, + normal_estimation_options = { + num_normal_samples = 4, + sample_radius = 0.5, + }, + project_sdf_distance_to_scan_normal = true, + update_weight_range_exponent = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5, + update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5, + }, + }, + }, +} diff --git a/carto_ws/src/cartographer-master/configuration_files/trajectory_builder_3d.lua b/carto_ws/src/cartographer-master/configuration_files/trajectory_builder_3d.lua new file mode 100644 index 0000000..489889d --- /dev/null +++ b/carto_ws/src/cartographer-master/configuration_files/trajectory_builder_3d.lua @@ -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. + +MAX_3D_RANGE = 60. +INTENSITY_THRESHOLD = 40 + +TRAJECTORY_BUILDER_3D = { + min_range = 1., + max_range = MAX_3D_RANGE, + num_accumulated_range_data = 1, + voxel_filter_size = 0.15, + + high_resolution_adaptive_voxel_filter = { + max_length = 2., + min_num_points = 150, + max_range = 15., + }, + + low_resolution_adaptive_voxel_filter = { + max_length = 4., + min_num_points = 200, + max_range = MAX_3D_RANGE, + }, + + use_online_correlative_scan_matching = false, + real_time_correlative_scan_matcher = { + linear_search_window = 0.15, + angular_search_window = math.rad(1.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1e-1, + }, + + ceres_scan_matcher = { + occupied_space_weight_0 = 1., + occupied_space_weight_1 = 6., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 0.3, + intensity_threshold = INTENSITY_THRESHOLD, + }, + translation_weight = 5., + rotation_weight = 4e2, + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 12, + num_threads = 1, + }, + }, + + motion_filter = { + max_time_seconds = 0.5, + max_distance_meters = 0.1, + max_angle_radians = 0.004, + }, + + rotational_histogram_size = 120, + + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. + imu_gravity_time_constant = 10., + pose_extrapolator = { + use_imu_based = false, + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + -- TODO(wohe): Tune these parameters on the example datasets. + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, + }, + + submaps = { + high_resolution = 0.10, + high_resolution_max_range = 20., + low_resolution = 0.45, + num_range_data = 160, + range_data_inserter = { + hit_probability = 0.55, + miss_probability = 0.49, + num_free_space_voxels = 2, + intensity_threshold = INTENSITY_THRESHOLD, + }, + }, + + -- When setting use_intensites to true, the intensity_cost_function_options_0 + -- parameter in ceres_scan_matcher has to be set up as well or otherwise + -- CeresScanMatcher will CHECK-fail. + use_intensities = false, +} diff --git a/carto_ws/src/cartographer-master/docs/CMakeLists.txt b/carto_ws/src/cartographer-master/docs/CMakeLists.txt new file mode 100644 index 0000000..a5d6a09 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/CMakeLists.txt @@ -0,0 +1,24 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(OUTPUT_DIR "${PROJECT_BINARY_DIR}/docs/html") + +add_custom_target(build_doc ALL + ${SPHINX_EXECUTABLE} -b html + ${CMAKE_CURRENT_SOURCE_DIR}/source + ${CMAKE_CURRENT_BINARY_DIR}/html + COMMENT "Building documentation." +) + +# TODO(hrapp): Install documentation? diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_1024dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_1024dp.png new file mode 100644 index 0000000..fe00a0d Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_1024dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_128dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_128dp.png new file mode 100644 index 0000000..18d464a Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_128dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_16dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_16dp.png new file mode 100644 index 0000000..44a3f6d Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_16dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_16dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_16dp.svg new file mode 100644 index 0000000..f764085 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_16dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_192dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_192dp.png new file mode 100644 index 0000000..940bd61 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_192dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_24dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_24dp.png new file mode 100644 index 0000000..ea179e0 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_24dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_24dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_24dp.svg new file mode 100644 index 0000000..44dab2b --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_24dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_32dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_32dp.png new file mode 100644 index 0000000..3e1b9a6 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_32dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_32dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_32dp.svg new file mode 100644 index 0000000..94d08ce --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_32dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_36dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_36dp.png new file mode 100644 index 0000000..7b6ffef Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_36dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_36dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_36dp.svg new file mode 100644 index 0000000..4ebefca --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_36dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_48dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_48dp.png new file mode 100644 index 0000000..7b97b2b Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_48dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_48dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_48dp.svg new file mode 100644 index 0000000..b1411ba --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_48dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_512dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_512dp.png new file mode 100644 index 0000000..009ca9f Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_512dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_512dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_512dp.svg new file mode 100644 index 0000000..3c52e38 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_512dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_64dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_64dp.png new file mode 100644 index 0000000..bf5245e Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_64dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_64dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_64dp.svg new file mode 100644 index 0000000..6d3619e --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_64dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_72dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_72dp.png new file mode 100644 index 0000000..85935f1 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_72dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_96dp.png b/carto_ws/src/cartographer-master/docs/assets/logo_96dp.png new file mode 100644 index 0000000..f7a6892 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/logo_96dp.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/logo_96dp.svg b/carto_ws/src/cartographer-master/docs/assets/logo_96dp.svg new file mode 100644 index 0000000..21a9b9c --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/logo_96dp.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer.ai b/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer.ai new file mode 100644 index 0000000..a631ca2 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer.ai @@ -0,0 +1,225 @@ +%PDF-1.5 % +1 0 obj <>/OCGs[5 0 R]>>/Pages 3 0 R/Type/Catalog>> endobj 2 0 obj <>stream + + + + + Adobe Illustrator CC 2015 (Macintosh) + 2015-11-17T16:08:11+01:00 + 2015-11-17T16:08:11+01:00 + 2015-11-17T16:08:11+01:00 + + + + 256 + 256 + JPEG + /9j/4AAQSkZJRgABAgEASABIAAD/7QAsUGhvdG9zaG9wIDMuMAA4QklNA+0AAAAAABAASAAAAAEA AQBIAAAAAQAB/+4ADkFkb2JlAGTAAAAAAf/bAIQABgQEBAUEBgUFBgkGBQYJCwgGBggLDAoKCwoK DBAMDAwMDAwQDA4PEA8ODBMTFBQTExwbGxscHx8fHx8fHx8fHwEHBwcNDA0YEBAYGhURFRofHx8f Hx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8fHx8f/8AAEQgBAAEAAwER AAIRAQMRAf/EAaIAAAAHAQEBAQEAAAAAAAAAAAQFAwIGAQAHCAkKCwEAAgIDAQEBAQEAAAAAAAAA AQACAwQFBgcICQoLEAACAQMDAgQCBgcDBAIGAnMBAgMRBAAFIRIxQVEGE2EicYEUMpGhBxWxQiPB UtHhMxZi8CRygvElQzRTkqKyY3PCNUQnk6OzNhdUZHTD0uIIJoMJChgZhJRFRqS0VtNVKBry4/PE 1OT0ZXWFlaW1xdXl9WZ2hpamtsbW5vY3R1dnd4eXp7fH1+f3OEhYaHiImKi4yNjo+Ck5SVlpeYmZ qbnJ2en5KjpKWmp6ipqqusra6voRAAICAQIDBQUEBQYECAMDbQEAAhEDBCESMUEFURNhIgZxgZEy obHwFMHR4SNCFVJicvEzJDRDghaSUyWiY7LCB3PSNeJEgxdUkwgJChgZJjZFGidkdFU38qOzwygp 0+PzhJSktMTU5PRldYWVpbXF1eX1RlZmdoaWprbG1ub2R1dnd4eXp7fH1+f3OEhYaHiImKi4yNjo +DlJWWl5iZmpucnZ6fkqOkpaanqKmqq6ytrq+v/aAAwDAQACEQMRAD8A9U4q7FXYq7FXYq7FXYq7 FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7FXYq7F XYq7FXYq7FXYq7FXYq7FXYq7FXzin/OQvnpSawWD/wCtDJ/CUZj+KXnB2tl7o/j4oyz/AOcjvMyN /pml2U69hD6sJ+9ml/Vj4xZx7Xn1AT/Tv+ckdJckajotxbr2NvKk5P0OIKffkhmciHbEf4on7/1M u0f85Py91Mog1MWczivp3iNDx9jIR6X/AA+TGQFy8faGGXWvezG2ura6gS4tZkngkFY5YmDowPcM tQcm5kZAiwqYpdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdiqVan5s 8r6W/p6lq9nZydRHNPGjmngpblgMgGueaEeZAY1c/nf+WMDsh1kSMvURwXDg/JhHxP35HxIuOdfh HX70BJ/zkH+XKOVWa6kA6OsDUP8AwRU/hg8UMD2li83zXmM8w7FXYq7FUdpWt6xpE/r6Xez2Uppy aCRk5cTUBgpow9jhBIZwySgbiaemeWf+chPMFmUh162TU4KgNcR0hnAruSFHptQdBxX55YMp6uzw 9rTG0xxfe9g8qfmD5V80Rj9F3g+tU5PYzUjuF2qfgJ+ICu7ISPfLozBdvg1ePL9J37urI8k5LsVd irsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirG/Nf5i+UPKykatfot1Sq2UX724O1R+7X7I NNi9B75GUwHHzamGP6ju8h8yf85K6pMzxeXdNjtItwt1eH1ZSCNmEaFUQg+JcZUcvc6zL2oT9Ar3 vNdc8/8AnTXea6prFzPFIOLwK/pQke8UXCP/AIXKzIlwMmpyT5ksfyLS7FXYqj8Wl2KuxV2KuxV2 Kro5JIpFliYpIhDI6khlYGoII6EYqDT1byP+fOsaaY7PzGralYj4RdrT61GKbVrQSj50b3PTLY5S ObtdN2pKO09x9v7Xu+i67pGt2CX+lXSXdpJ0kQ9DQHiymjKwrurCuXg273HljMXE2EdhbHYq7FXY q7FXYq7FXYq7FXYq7FXYq7FXYq7FUp8y+a9A8taeb7WbtbaHcRod5JGH7MaD4mPy6d8BkA1Zc0cY uRfP3nn8/fMWsmSz0ENo+nGo9VT/AKXIvvIP7v5Jv/lHKJZSeTpdR2jKe0fSPteWO7yOzuxd3JZm Y1JJ3JJOVOuW4q7FVaG0up/7mF5PdVJH4YoJARkfl7V339DiPFmUfhWuGmPiBWHlbVCP91j2LH+m NI8QIo+X78D9g+3L+zGmviCk+i6kn+6uQ/ySD/HGk8QQ0ttcRf3sbJ7sCMCbUsVdirsVdirsVTny t5u13yxqIvtIuDE5oJoW+KKVQfsyJtUb/MdiMMZEN2DPPEbiX0t5A/MnRfOFmfR/0bVIVBurBzVg OnOM7c0r37d+1cmEwXpdLrI5httLuZdk3LdirsVdirsVdirsVdirsVdirsVSPVfO3lnStbs9Fvr1 IdQvf7qM9Fr9n1G6JzOy165EyANNE9TCMhEncp5km955+Zv5waV5QR7CzC32vslUtqn04eQ+FpyK fMIDU+wIOVznTg6rWxxbDeT5o1/zFrPmDUX1HV7p7u6fbk52Va1Coo+FVFegzHJt0GTLKZuRspbg YL4oZZpBHEhdz0VRU4oJTuy8q3D0a7cRL/ItGb7+g/HDTWcvcndromm24HCEOw/bf4j+O33YaazM lGgACg6YWLeKuxV2KuxV3XFUJcaVYTg8ogrfzJ8J/DBSQUquvL0yVa3f1B/I2zff0OCmQklUkckb lJFKOOqkUOBksxV2KuxVE6dqN9pt9Df2E7215btzhmjNGU9PuI2I7jEGmUJmJsbF9Mflh+Z9l5ts ha3RW3123Ws8A2WVR/u2L2/mXt8syYTt6XRa0ZRR+pnmWOe7FXYq7FXYq7FXYq7FXYqxH8yfP9p5 P0QzALLqtyCmn2zdCw6yPTfgld/Hp3qITnQcTWaoYY3/ABHk+WdR1G91K+nv76Zp7u5cyTTN1Zj8 th7AbDMUm3lpzMjZ5lnlh+efmzT/ACk+joFm1BAI7TVZDykjipuChBDuv7LH6a5YMhqnYY+0pxhw 9e95fPPNPNJPPI0s0rF5ZXJZmZjVmZjuST1OVuITe5U8UJtpXl+4vKSy1htz+1+03+qP44QGuU6Z TZ2NraR8IIwo7t1Y/M5JpJJRGKHYq7FXYq7FXYq7FXYq7FXYqo3Npb3KcJkDeB7j5HFQWPaho01t WSOskPj3HzGRIZiSXYGTsVdiqJ07Ub7Tb6C/sJmt7y2cSQzJ1Vh89iPEHY4g0yhMxNjmH1N+W3n+ z84aL61Fh1S2ol/ag7Bj0kSu/B6beHT3OVCdh6nR6oZo3/EObLsm5bsVdirsVdirsVdiqB13WrDR NIutVv39O0tELyHap7Kq1IqzMQqjxwE015cghEyPIPknzd5p1HzPrtxq98aPKeMMIJKxRL9iNemw r9Jqe+YkpWXk8+c5ZmRSbA0uxVQmg/aUfMYs4yT3RPLwAW5vVqTvHAe3u39MIDCeToGRZJqdirsV dirsVdiqumn38g5JbSuviqMR+AxpkMcj0Ln0+/jHJ7aVF8WRgPxGNKcch0KhixdirsVdirsVdiqS arooIae1Wh6vEP1r/TIkMhJIsDN2KuxVOfKPmnUfLGu2+r2Jq8R4zQkkLLE324267Gn0Gh7YYyot 2DOcUxIPrbQtasNb0i11Wwf1LS7QPGdqjsytQmjKwKsPHMsG3rMWQTiJDkUdhbHYq7FXYq7FXYq+ evz588HUtYXy5ZyVsdNbldlaUkuqUp8ogeP+sT4DMfLKzTz3amp4pcA5R+/9jyfKnVOxV2Kp7oul ABbqddzvEh/4kf4YQGEineSYuxV2KuxVlHl/8uvMOrhZmjFlaNuJpwQSP8lPtH8B75MQJc/T9m5c m/0x82faV+VfluzCtdiS/lHUyEolfZEp+JOWDGHcYeycUfq9RZPZ6RpVkALOzht6dDHGqn7wMkA7 CGGEfpACLwtjsVQl5pOl3oIu7OG4r/v2NXP3kYCGueGEvqALGdV/KzyzeBmtVewmPQxMWSvuj1/A jInGHAy9k4pcvSWBa/8Alx5h0kNLGgvrRd/VgBLAf5Uf2h9FR75WYEOn1HZuXHuPUPJiuQde7FXY qgLy86xRH2Zh+oZm6fT36pNOTJ0CSXlr1lQf6w/jjqtP/EGWHL0KBzXuS7FXYq9Y/IbzwdN1dvLl 7JSx1JuVoWpRLqlKV8JQKf6wHictxSrZ2vZep4ZcB5S+/wDa+hcyHoXYq7FXYq7FWOfmD5rj8r+V bzVKj61T0bFGp8VxICE2NKhd3YeAORnKg42rz+FjMuvT3vkeSSSWRpZWLyOSzuxJZmJqSSepOYjy RNrcVdiqY6Np/wBZn9SQfuY+vuew/rhAYyLJ8kwdirsVRWm6ZfaneJZ2MLTXEn2UXsO5J6ADxOIF s8WKWSXDEWXr3lP8udM0dUub0Leal15kVjjP+Qp6n/KP4ZfGFPS6Ps2GLeXqkzDJuzdirsVdirsV dirsVdirEfNn5d6XrKvc2oWz1I1PqKKRyH/ixR/xIb/PIShbrdX2bDLuPTL8c3kGqaVf6XeyWd9E YZ4+qnoQejKe4PjlJFPNZcUscuGQopLeXnWKI+zMP1DMzT6e/VJxcmToEDmc0OxVLby39J+Sj4G6 ex8M1Wpw8Bsci5mKdjzQ2Yzc7FV0ckkUiyxMUkQhkdSQysDUEEdCMVBp9cfl95rj80eVbPVKj61T 0b5Fp8NxGAH2FaBtnUeBGZcJWHrdJn8XGJdevvZHknJdirsVdir58/5yE8zG88wW2gwufQ0xPUuF BNDPMAwBHQ8Y+ND/AJRzHynenn+1s1zEB/D97yXKnUuxVfHG8kixoKu5AUe5xVl9pbJbW6Qp+yNz 4nucm1Eq2KuxVE6bp15qV9FZWcZkuJjxRe3uSewA64gWzxYpTkIx5l7n5U8qWPl6x9KKkl3IAbm5 I3c+A8FHYZkRjT1uk0kcMaHPqU8yTluxV2KuxV2KuxV2KuxV2KuxV59+Yb6frcH6PiA9WEkrejqr d1U91P7WWwxjmXR9pzjlHCOnV4jeWdxZ3L21wvGWM0I7HwI9jmYC8rOJiaKjixdiqyWNZIyh79D7 5DJASjRZRlRtKGUqxU7EbHNLIEGi54NtYEuxV61/zj35mNn5gudBmc+hqaepbqSaCeEFiAOg5R8q n/JGW4jvTtuyc1TMD/F976DzIegdirsVUrq5gtbWa6ncRwQI0srnYKiAsxPyAxRKQAsvjXW9Vn1f WL3VJ9pb2eSdlqW482LBQT2UbDMMmy8bkmZyMj1QOBg7FU48vWvOZ7hhtH8Kf6x6/cMIYyLIMkwd irsVezflv5TGk6aNQuU/3IXqhqEbxxHdU+Z6t93bL4Rp6fszSeHDiP1S+5mWTdo7FXYqsmnhhjMk zrHGvVmNB+OKJSAFlIL3zpYxMVto2uCP2z8C/jU/hlgxlwMnaER9ItLJfOupt/dxRIPkxP6/4ZLw w4x7Rn0AdF511Nf7yKJx8mB/X/DHwwo7Rn1ATOy86WMrBbmNrcn9sfGv4UP4ZE4y5OPtCJ+oUn8M 8M0YkhdZI26MpqPwytz4yBFhfiliXmXzLy52Vk/w/ZmmXv4qp/WcthDqXVazWfwx+JYtlrrEh82a IL+zNxCtbu3BK06snUr/ABGSiacTVYeIWOYefZa6p2KuxVAahFRxIOjbH5jNdrMdHi73KwS2pB5h OQ7FUdomqz6RrFlqkG8tlPHOq1K8uDBipI7MNjhBos8czCQkOj7KtbmC6tYbqBxJBOiyxONwyOAy kfMHMx7KMgRYVcUuxVhX5yawdM/L3UyjhJrwJZx1/a9ZgJAP+eXPIZDQcLtDJw4T57PlfMV5Z2Ku xVlulW4gsIlp8TDm3zbfJBrJReFDsVZJ5A0Aax5hhSVOVpa/v7gHoQp+FT/rNT6K5KAsud2fp/Fy i+Q3L3TMh612KuxVA6tq1vptv6svxO20cY6sf6YYxtpz5xjFlgepareahN6lw9QPsRjZV+Qy8RAd FmzSyGyg8k1OxV2KuxVGabqt5p83qW70B+3Gd1b5jImILbhzSxmwm+sebHurVYLRWh9Rf37Hr/qq R298jGFOXqNdxRqO3exzLHXuxV2KvN/NGmCw1VwgpBP+9ip0FTuv0HLomw6bU4+GXklGFodiqldR 84GHcCo+jKdRDigWzFKpJTmnc52KuxV9Ufk3rB1P8vdMLuHmsw9nJT9n0WIjB/55cMysZsPU9n5O LCPLZmuTc12KvHP+ckdRZNK0XTQPhuJ5bhj4GBAg/wCT5ynMXT9sT9MY+f4+94LlDoXYqq20Xq3E cX87BT9JxUsz6ZNqdirsVewflNpQtvL8l8w/eX0pIP8AxXFVFH/BcsuxjZ6XsjFw4+L+cfuZxljt nYqsnmjhheaQ8Y41LMfYb4olIAWXnGq6lNqF49xJsDtGn8qjoMyIig87mzHJKyg8k1OxV2KuxV2K uxV2KuxV2KuxVjvniyE2lrcgfHbOCT/kP8J/GmTgd3D1kLjfcwLLHVuxV2KpNIvCRl8CRmjnGpEO xibFrcil2Kvev+cbtRZ9K1rTSPht54rhT4mdCh/5MDL8Jd92PP0yj5/j7nseXO4dir59/wCcjrxn 8z6ZZU+CCy9YGveaV1O1P+KhmPm5vP8Aa8vWB5PJMqdS7FUdoqc9Si8Fq33A4QiXJlWSa3Yq7FX0 N5bsxZ+X9OtgKGO3jDf6xUFj/wAETmTEbPaaaHDjiPIJlhb3Yqx3zpetFYx2ymhuGq/+qm/6yMsx jd1/aGSoiPewvLnTuxV2KuxV2KuxV2KuxV2KuxV2KoTV4BcaXdQ0qXifj8wKj8cI5teWNwI8nleX OjdirsVSu8FLl/eh/DNRqRUy5uE+lQyhtdir1v8A5xxvGTzPqdlT4J7L1ia94ZUUbU/4tOW4ebtu yJesjyfQWZD0DsVfOP8AzkK5bz1AKfYsIV/5KSn+OY+Xm852sf3o/q/reYZU6x2Kpn5fAN/8kan4 YQxlyZLkmDsVcASaDrir6XVVVQqiiqKAewzKe7AbxV2KsK86y8tTij7JEPvLH+zLsfJ03aJ9YHkx 7LHAdirsVdirsVdirsVdirsVdirsVcQCCDuDscUPIMvdA7FXYqlt/wD3/wDsRmr1n1/BzMH0obMV udir0/8A5x6cr56nFPt2Ey/8lIj/AAy3Fzdn2Sf3p/q/qfR2ZD0bsVfOP/OQqFfPUBr9uwhb/kpK P4Zj5ebznaw/ej+r+t5hlTrHYqmfl8gX/wA0an4YQxlyZLkmDsVdir6WR1dFdfssAR8jvmU90Da7 FLsVYV51i46nFJ2eIfeGP9mXY+Tpu0R6wfJj2WOA7FXYq7FXYq7FXYq7FXYq7FXYq0zBVLHooqfo xQS8hy90DsVdiqR6wf8ATPko/jmr1n1/B2Gm+lA5iuQ+o/8AnHyN0/LmFmFBJdTsh8RyC/rU5k4u T0PZo/dfF6TljnuxV2Kvn3/nI6zZPM+mXtfgnsvRAp3hldjvX/i0Zj5ubz/a8fWD5PJMqdS7FUdo r8NSi8Gqv3g4QiXJlWSa3Yq7FX0H5UvRe+W9NuAalrdFc/5aDg3/AAynMmJ2ez0k+LFE+Sa4XIdi rHfOlk0tjHcqKm3aj/6r7frAyzGd3X9oY7iJdzC8udO7FXYq7FXYq7FXYq7FXYq7FXYqgtbuBb6R dy1oREwX/WYcR+Jwjm1ZpVAl5blzpHYq7FUg1RuV7J7UH3AZqdSbmXZYB6AhMx259ZfkhbPB+WOj B14tIJ5CPEPcSFT9K0zKx/S9JoBWEfjqzrJuY7FXYq8c/wCckdOZ9K0XUgfht55bdh4mdA4/5MHK cwdP2xD0xl5/j7nguUOhdiqrbS+lcRy/yMGP0HFSzPrk2p2KuxV63+UWrCfR7jTXP7yzk5xj/iuX f8HB+/LsZ2ej7HzXAw/m/pZ7ljuHYqsnhjmheGQco5FKsPY7YolEEUXnGq6bNp949vJuBvG/8yno cyImw87mwnHKig8k1OxV2KuxV2KuxV2KuxV2KuxVjPnq+EenxWin47h+TD/ITf8A4lTJwDha2dRA 72DZY6x2KuxVjM8nqTO/8zE/ec0c5XIl20BQAU8iyfa/lPTH0vyvpGmyf3lnZwQyEbVdI1DH/gsz Iig9ZhhwwA7gmuFsdirsVYV+cmjnU/y91MIgeazCXkdf2fRYGQj/AJ5c8hkFhwu0MfFhPlu+V8xX lnYq7FWW6VcCewiavxKODfNdskGshF4UOxVPPJevfoTX7e6c0tpP3N1/xjcip/2Jo30ZKJouXotR 4WQHpyL3tWDAMpqp3BHQjMh7BvFXYqgdW0m31K39KX4XXeOQdVP9MMZU058AyCiwPUtKvNPm9O4S gP2JBurfI5eJAuizYZYzRQeSanYq7FXYqjNN0q81Cb07dKgfbkOyr8zkTIBtw4ZZDQT3VPJ/pWay WTNJNGP3qn9v3UePtkI5N93OzaCo3HchixFNj1y11jsVcSAKnYDqcUPM/MWp/pHVJZlNYU/dw/6q 9/pO+XRFB02fJxytLMLS7FUPqE3pWkjdyOK/M7ZTqJ8MC24Y3IMdzTuzZB5A0P8ATvnTR9LZBJFP co06N0MMX72Uf8i0bJRFlu02PjyAeb7MzLeqdirsVdiqldW0F1azWs6CSCdGilQ7hkcFWB+YOKJR BFF8a63pU+kaxe6XPvLZTyQM1CvLgxUMAezDcZhkUXjckDCRieiBwMHYqnHl664TPbsdpPiT/WHX 7xhDGQZBkmDsVdir1r8r/Nq3lmNFu3/0u2X/AEVj/uyIfs/6yfq+Ry7HLo9H2VrOKPhy+ocvd+xn 2WO4dirsVWTQQzRmOZFkjbqrCo/HFEogiikF75LsZWLW0jW5P7B+NfxofxywZC4GTs+J+k0lkvkr U1/u5YnHzYH9X8cl4gcY9nT6EOi8lam395LEg+bE/q/jj4gUdnT6kJnZeS7GJg1zI1wR+wPgX8Kn 8cichcnH2fEfUbT+GCGGMRwosca9FUUH4ZW58YgCgvxSxrzJ5b9fle2S/vussQ/b9x/lfryyE+hd dq9JxeqPNhxFNj1y51DGvOOuC2tzYQN+/mH70j9mM9vm36snAOFq81DhHNguWOsdirsVSfWbjlKs I6Ju3zP9ma7WZLPD3OdpYULS3MJyntX/ADjV5babVNS8xSp+6tIxZ2rEAgyy0eQqeoKIoHyfLsQ6 u17LxWTPu2fQWXu7dirsVdirsVfPn/OQnlk2fmC216FD6Gpp6dwwBoJ4QFBJ6DlHxoP8k5j5Rvbz /a2GpiY/i+95LlTqXYqvjkeORZENHQgqfcYqy+0uUubdJk/aG48D3GTaiFbFXYqq2tzcWtxHc27m KeFg8ci9Qw3BxTGRibHMPbPJfnW01+1WKUiLVIl/fwdOdOsiex8O2Xxlb1eh10cwo/WyfJue7FXY q7FXYq7FXYq7FXYq7FXnP5n3um6QgntiG1ScV+qjpT/frU6fx+/LIZADRdD2sYY94/Wen6Xic88s 8zzTMXlkPJ2PUk5mvLEkmysxQ7FVK5nWCFpG7dB4nsMhkmIxss4Q4jTHHdndnY1ZjUn55pZEk2Xa AUKciPI6oil3chVVRUknYAAYEvsb8uvKi+VvKFhpJA+tKnq3rCm9xL8Um4pUL9gHwAzLhGg9RpsP hwEerJMk5DsVdirsVdirHPzB8qR+aPKt5pdB9ap61i7U+G4jBKbmtA26MfAnIzjYcbV4PFxmPXp7 3yPJHJFI0UqlJEJV0YEMrA0IIPQjMR5IiluKuxVMdG1D6tN6ch/cyHf2PY4QWMgyfJMHYq7FVS2u bi1njuLeRop4iGjkQ0YEdwcUxkYmxsXqflP80rW5VLTXCLe52C3nSJ/9f+Q+/T5ZdHJ3vQ6TtUS9 OTY9/R6Ajo6h0YMjCqsDUEHuDljuAbbxS7FXYq7FXYq7FWmZVUsxCqoqzHYADucVJYF5s/NCzs1e 00Urc3e6tddYU91/nP4fPK5ZO50+r7VjH049z39P2vKrq6ubu4kubmRpZ5Tykkc1JJyl56czI2dy ld5Z9ZYh7so/WMzdPqK9MnHyY+oQOZzQ7FUi1O89eXgh/dJ09z45qtTm4zQ5B2ODFwizzQWYze9V /ILyMdZ8xHXryOunaOwaKo+GS76xj/nn9s+/Hxy3FGzbseztPxy4jyj976XzId+7FXYq7FXYq7FX Yq+evz58jnTdYXzHZx0sdSbjdhaUjuqVr8pQOX+sD4jMfLGjbz3amm4ZcY5S+/8Aa8nyp1TsVdiq e6LqoIW1nbcbROf+In+GEFhIJ3kmLsVdirsVTjQvN2vaIwFlcn0K1NtJ8cR/2J6f7GmESIcnBrMm L6Tt3dGe6V+cGnyAJqlm8D95YCJE+ZU8WH45aMjuMPbMT9Yr3Mns/O/lO7AMWpwqT2lPon/kpxyQ kHPhrsMuUh933pnFqWnS/wB1dQybV+GRW2+g4bcgZYnkQ6XUtOi/vbqGPavxSKu30nG1OWI5kJbe edvKloD6upwMR2ib1j90fLAZBx567DHnIff9zGNV/ODTogU0y0e4foJZiI0+YUcmP4ZE5HAy9sxH 0C/ewPXfOGv62St5cEW9ai2i+CIf7Efa/wBkTlZkS6jUa3Jl+o7d3RJci4rsVdiqAvLPrLEPdlH6 xmbp9RXpk05MfUMd1TUBQwRHfpIw/UMdTqP4Q24MP8RSnNe5iZeXdA1HzBrNrpGnJzurtwi1rxUd WdiK0VV3OEC2eLGZyERzL7E8qeWtP8taBaaNYj9zbLR5CKNJId3kb3Zt/bpmXEUHqMOIY4iITbC2 uxV2KuxV2KuxV2KoHXdFsNb0i60q/T1LS7QpINqjurLUGjKwDKfHARbXlxicTE8i+SfN3lbUfLGu 3GkXwq8R5QzAELLE32JF67Gn0Go7ZiSjReTz4DimYlJsDS7FVCaf9lT8zizjFPdE8wghba9ahG0c 57+zf1wgsJ4+oZFkmp2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KpJqutABoLVqno8o/Uv9ciSyEWN zw9XX6RgboyU4IJp5o4II2lmlYJFEgLMzMaKqqNySegxbAL2D6l/J/8ALJPKGlG8v0RtfvlH1lxR vRjrUQK30AuR1PiADmTCFPQ6LS+FGz9Reh5Y5zsVdirsVdirsVdirsVdirEfzJ8gWnnDRDCCsWq2 wL6fct0DHrG9N+D038OvahhOFhxNZpRmjX8Q5PlnUdOvdNvp7C+haC7tnMc0LdVYfLY+xGxzFIp5 acDE0eYZ5YfkZ5s1Dyk+sIVh1BwJLTSpBxkkipuS5ICO37Kn6aZYMZq3YY+zZyhxde55fPBNBNJB PG0U0TFJYnBVlZTRlZTuCD1GVuIRWxU8UJtpXmC4s6RS1mtx+z+0v+qf4YQWuULZTZ31reR84JAw 7r0YfMZJpMSERih2KuxV2KuxV2KuxV2KuxV2KqNzd29snOZwvgO5+QxUBj2oazNc1jjrHD4dz8zk SWYil2Bk7FXov5GX/lLT/NhfWECahMoj0q7kI9KORqhwR+y7g0Vvo75ZjIvd2XZuSEZ+rn0fTGZL 0jsVdirsVdirsVdirsVdirsVdiqR6r5J8s6rrdnrV9ZJNqFl/dSHo1Ps+ovR+B3WvTImIJtonpoS kJEbhPMk3vPPzN/J/SvN6Pf2ZWx19UolzQ+nNxHwrOBX5BwKj3AAyucLcHVaKOXcbSfNGv8Al3Wf L+ovp2r2r2l0m/Fxsy1oGRh8LKadRmORToMmKUDUhRS3AwXxTSwyCSJyjjoymhxQQndl5quEot2g lX+daK33dD+GG2s4u5O7XW9NuAOEwRj+w/wn8dvuw21mBCNBBFR0wsW8VdirsVdirumKoS41WwgB 5Shm/lT4j+GC0gJVdeYZnqtunpj+dt2+7oMFshFKpJJJHLyMXc9WJqcDJZirsVdiqJ07Tr7Ur2Gx sIHubuduMMMYqzHr+A3J7DEC2UIGRobl9ceSdK1vSvLNlY61efXdQhSkkvXiP2Y+XV+A25HrmXEE Dd63TQlGAEjZTzJN7sVdirsVdirsVdirsVdirsVdirsVdiqU+ZfKmgeZdPNjrNotzDuY3O0kbH9q Nx8Sn5de+AxBasuGOQVIPn7zz+QXmLRjJeaCW1jThU+ko/0uNfeMf3nzTf8AyRlEsRHJ0uo7OlDe PqH2vLHR43ZHUo6EqysKEEbEEHKnXLcVdiqtDd3UH9zM8fsrED8MUEAoyPzDq6bevyHgyqfxpXDb HwwrDzTqgH+6z7lT/XG0eGEUfMF+R+wPfj/bjbXwhSfWtSb/AHbxH+SAP4Y2nhCGlubiX+9kZ/Zi TgTSlirsVdirsVdiqc+VvKOu+Z9RFjpFuZXFDNM3wxRKT9qR96Db5nsDhjEluwYJ5TUQ+lvIH5ba J5PtCYQLnVZV43OoOKMR14Rjfgle3fvXamTCAD0ul0ccI23l3suybluxV2KuxV2KuxV2KuxV2Kux V2KuxV2KuxV2KuxVjfmv8uvKHmlSdWsEa6pRb2L91cDag/eL9oCuweo9sjKALj5tNDJ9Q3eQ+ZP+ catUhZ5fLupR3cW5W1vB6UoAGyiRAyOSfEIMqOLudZl7LI+g373muueQPOmhc21TR7mCKMcnnVPV hA95Yucf/DZWYkOBk02SHMFj+RaXYq7FUfi0uxV2KuxV2KuxVdHHJLIsUSl5HIVEUEszE0AAHUnF QLereR/yG1jUjHeeY2bTbE/ELRafWpBTvWoiH+tVvYdctjiJ5u103Zcpbz9I+39j3fRdC0jRLBLD SrVLS0j6RoOpoByZjVmY03ZjXLwKd7jxRgKiKCOwtjsVdirsVdirsVdirsVdirsVdirsVdirsVdi rsVdirsVdirsVSrU/KflfVH9TUtIs7yToJJoI3cV8GK8sBiC1zwwlzALGrn8kPyxndnOjCNm6mOe 4QD5KJOI+7I+HFxzoMJ6fegJP+cfPy5dyyw3UYPRFnag/wCCDH8cHhBgezcXm+a8xnmHYq7FXYqj tK0TWNXn9DS7Ke9lFOSwRs/HkaAsVFFHucIBLOGOUzURb0zyz/zj35gvCk2vXKaZBUFreOk05Fdw Sp9NajoeTfLLBiPV2eHsmZ3meH73sHlT8vvKvleMfouzH1qnF76aklw21D8ZHwg03VAB7ZdGADt8 Gkx4vpG/f1ZHknJdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVdirsVf OKf849eemJrPYJ/rTSfwiOY/hF5wdk5e+P4+CMs/+ccfM7sfrup2UCbUMPqzHvXZki/Xj4JZx7In 1IT/AE7/AJxu0pGJ1LWp7hey28SQEfS5n/VkhhciHY8f4pH8fNl2j/k3+XumFHGmC8mQU9S8dpuX uYyfS/4TJjGA5ePs/DHpfvZhbWtrawJBawpBBGKJFEoRFA7BVAAybmRiAKCril2KuxV2KuxV2Kux V2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV2KuxV 2KuxV2KuxV2KuxV//9k= + + + + 1 + False + False + + 500.000000 + 500.000000 + Points + + + + Cyan + Magenta + Yellow + Black + + + + + + Default Swatch Group + 0 + + + + application/pdf + + + logo_cartographer + + + xmp.did:47f3d73d-a7e1-4ca8-a59d-7cf7f0b5441f + uuid:d0e9fabf-e748-124b-b76c-93f8c9fb3a2d + xmp.did:387ed731-0e0b-4444-9bce-9c2362dc6bce + proof:pdf + + xmp.iid:387ed731-0e0b-4444-9bce-9c2362dc6bce + xmp.did:387ed731-0e0b-4444-9bce-9c2362dc6bce + xmp.did:387ed731-0e0b-4444-9bce-9c2362dc6bce + + + + + saved + xmp.iid:387ed731-0e0b-4444-9bce-9c2362dc6bce + 2015-11-17T16:01:16+01:00 + Adobe Illustrator CC 2015 (Macintosh) + / + + + saved + xmp.iid:47f3d73d-a7e1-4ca8-a59d-7cf7f0b5441f + 2015-11-17T16:08:10+01:00 + Adobe Illustrator CC 2015 (Macintosh) + / + + + + Document + Adobe PDF library 15.00 + + + + + + + + + + + + + + + + + + + + + + + + + endstream endobj 3 0 obj <> endobj 7 0 obj <>/Resources<>/Properties<>>>/Thumb 11 0 R/TrimBox[0.0 0.0 500.0 500.0]/Type/Page>> endobj 8 0 obj <>stream +HtVKn0 em]EE0gHz|$e˓H ?O<_c>>ZjR Q|XTNݷTm }!fwvCJnjaܪa9fT!mx2V 4 \1WQ=*8 +; 6S?ͅY|JdWIRǢ0P9v*5C/6\mdzd~_Ǽh}q{1ؠ!xvSёU +2bViJ;/7.PnM"~ E*N7py6x~fU\"޿O5Hx̝q~z$)>M]L4xƕ&6Nar:۹}"] .gИ i]Rrg4-O\(Ç.߸ WJ|46IZn>ߘ7Z6\< gAGy`:wAH_ш+CBOL%Prvb4ﻖ6ˋ&Y"uUL wYC 4~XB+zᲓ+cSTK3ՎrƱor> +џϳ+v6 endstream endobj 11 0 obj <>stream +8;Z,g_3)VX$q2ZhQU!-A>$$kc>R\Sf5I;(A#pb'mm+cbm1uiSMrHWbkg/@q7\3$'b +B=gS'X)&=#/M="`(EjD:p,J\%j0%R5e-)ckHCi\VD*PsogfTD_dZujj\1.kWTglPc%Y2&I5RbC.^\]I(c-L*.\AYA)SKHpLrL"jasp6?W`6Z^`56DlDMM!Fa% +'DG+g\0KV9WcG+SRi3iYgrJs:$e=h&eo#q,c7YI^(=J+%?YRE6l>2T3-aB9_/c/=d +5%olQc*&'J59mRs.XS_@bHTUQr`7]@TB4$5NV]UE-pR2Z\IS16]1q!al)U"b1T>$X +?L;b[3mZV1h&I4ipT.V<0nWYk42*9]Fo+nChXg\/hIPud]#3>t/]^03hmL)#QIJXq +MpB9u"KT3o0$Cu&Y)]%^qn-=?k01!i7(:lr7#UhW^W-3(Tec)%:0$tdA!acC1jKZ2 +gn-6dD".V1pZMK.N;`R+>TDZ"=46d!p>g^J]ES@qU;(.~> endstream endobj 12 0 obj [/Indexed/DeviceRGB 255 13 0 R] endobj 13 0 obj <>stream +8;X]O>EqN@%''O_@%e@?J;%+8(9e>X=MR6S?i^YgA3=].HDXF.R$lIL@"pJ+EP(%0 +b]6ajmNZn*!='OQZeQ^Y*,=]?C.B+\Ulg9dhD*"iC[;*=3`oP1[!S^)?1)IZ4dup` +E1r!/,*0[*9.aFIR2&b-C#soRZ7Dl%MLY\.?d>Mn +6%Q2oYfNRF$$+ON<+]RUJmC0InDZ4OTs0S!saG>GGKUlQ*Q?45:CI&4J'_2j$XKrcYp0n+Xl_nU*O( +l[$6Nn+Z_Nq0]s7hs]`XX1nZ8&94a\~> endstream endobj 5 0 obj <> endobj 14 0 obj [/View/Design] endobj 15 0 obj <>>> endobj 10 0 obj <> endobj 9 0 obj <> endobj 16 0 obj <> endobj 17 0 obj <>stream +%!PS-Adobe-3.0 %%Creator: Adobe Illustrator(R) 17.0 %%AI8_CreatorVersion: 19.0.0 %%For: (Rachel Simpson) () %%Title: (logo_cartographer_48dp.svg) %%CreationDate: 11/17/15 4:08 PM %%Canvassize: 16383 %%BoundingBox: -240 -240 241 240 %%HiResBoundingBox: -239.999997980893 -240 240.000002019107 240 %%DocumentProcessColors: Cyan Magenta Yellow Black %AI5_FileFormat 13.0 %AI12_BuildNumber: 44 %AI3_ColorUsage: Color %AI7_ImageSettings: 0 %%RGBProcessColor: 0 0 0 ([Registration]) %AI3_Cropmarks: -250 -250 250 250 %AI3_TemplateBox: 24.5 23.5 24.5 23.5 %AI3_TileBox: -288 -356 288 378 %AI3_DocumentPreview: None %AI5_ArtSize: 14400 14400 %AI5_RulerUnits: 2 %AI9_ColorModel: 1 %AI5_ArtFlags: 0 0 0 1 0 0 1 0 0 %AI5_TargetResolution: 800 %AI5_NumLayers: 1 %AI17_Begin_Content_if_version_gt:17 1 %AI9_OpenToView: -1091 649 1 2234 1298 18 0 0 78 133 0 0 0 1 1 0 1 1 0 0 %AI17_Alternate_Content %AI9_OpenToView: -1091 649 1 2234 1298 18 0 0 78 133 0 0 0 1 1 0 1 1 0 0 %AI17_End_Versioned_Content %AI5_OpenViewLayers: 7 %%PageOrigin:-155 -229 %AI7_GridSettings: 72 8 72 8 1 0 0.800000011920929 0.800000011920929 0.800000011920929 0.899999976158142 0.899999976158142 0.899999976158142 %AI9_Flatten: 1 %AI12_CMSettings: 00.MS %%EndComments endstream endobj 18 0 obj <>stream +%%BoundingBox: -240 -240 241 240 %%HiResBoundingBox: -239.999997980893 -240 240.000002019107 240 %AI7_Thumbnail: 128 128 8 %%BeginData: 21150 Hex Bytes %0000330000660000990000CC0033000033330033660033990033CC0033FF %0066000066330066660066990066CC0066FF009900009933009966009999 %0099CC0099FF00CC0000CC3300CC6600CC9900CCCC00CCFF00FF3300FF66 %00FF9900FFCC3300003300333300663300993300CC3300FF333300333333 %3333663333993333CC3333FF3366003366333366663366993366CC3366FF %3399003399333399663399993399CC3399FF33CC0033CC3333CC6633CC99 %33CCCC33CCFF33FF0033FF3333FF6633FF9933FFCC33FFFF660000660033 %6600666600996600CC6600FF6633006633336633666633996633CC6633FF %6666006666336666666666996666CC6666FF669900669933669966669999 %6699CC6699FF66CC0066CC3366CC6666CC9966CCCC66CCFF66FF0066FF33 %66FF6666FF9966FFCC66FFFF9900009900339900669900999900CC9900FF %9933009933339933669933999933CC9933FF996600996633996666996699 %9966CC9966FF9999009999339999669999999999CC9999FF99CC0099CC33 %99CC6699CC9999CCCC99CCFF99FF0099FF3399FF6699FF9999FFCC99FFFF %CC0000CC0033CC0066CC0099CC00CCCC00FFCC3300CC3333CC3366CC3399 %CC33CCCC33FFCC6600CC6633CC6666CC6699CC66CCCC66FFCC9900CC9933 %CC9966CC9999CC99CCCC99FFCCCC00CCCC33CCCC66CCCC99CCCCCCCCCCFF %CCFF00CCFF33CCFF66CCFF99CCFFCCCCFFFFFF0033FF0066FF0099FF00CC %FF3300FF3333FF3366FF3399FF33CCFF33FFFF6600FF6633FF6666FF6699 %FF66CCFF66FFFF9900FF9933FF9966FF9999FF99CCFF99FFFFCC00FFCC33 %FFCC66FFCC99FFCCCCFFCCFFFFFF33FFFF66FFFF99FFFFCC110000001100 %000011111111220000002200000022222222440000004400000044444444 %550000005500000055555555770000007700000077777777880000008800 %000088888888AA000000AA000000AAAAAAAABB000000BB000000BBBBBBBB %DD000000DD000000DDDDDDDDEE000000EE000000EEEEEEEE0000000000FF %00FF0000FFFFFF0000FF00FFFFFF00FFFFFF %524C45FDFCFFFDFCFFFDC8FF59595983598383AFAEFD77FF340B110B110B %330B34345F59A8AFFD72FF0B110B110B330B0B0B110B0B0B34345F83FD6F %FF340B340B340B340B340B340B340B340B34335F83FD6CFF0B330B330B34 %0B330B340B330B340B330B330B0B0B83A8FD64FFAEAF84AE84340B340B34 %0B340B340B340B340B340B340B340B330B3434A8FD5BFFA8A859592E2D0B %2DFD050B110B330B110B330B110B330B110B330B110B330B110B0B0B3483 %FD56FF83833434FD050B330B2D0B340B340B340B340B340B340B340B340B %340B340B340B340B340B340B3334AEFD51FF7D580B0B0AFD040B2D0B0B0B %2D0B0B0B2D0B330B340B330B340B330B340B330B340B330B340B330B340B %330B110B58A8FD4CFF845F0B0B0B330B2D0B340B2D0B340B2D0B340B2D0B %340B340B340B340B340B340B340B340B340B340B340B340B340B340B340B %3459FD49FF842D0B04FD040B2D0B0B0B2D0B0B0B2D0B0B0B2D0B0B0B330B %110B330B110B330B110B330B110B330B110B330B110B330B110B330B0B2D %AEFD45FF83340B0B0B340B330B340B330B340B330B340B330B340B330B34 %0B340B340B340B340B340B340B340B340B340B340B340B340B340B340B34 %0B340B84FD42FFA82D0B0B2D0B0B0B2D0B0B0B2D0B0B0B2D0B0B0B2D0B0B %0B2D0B0B0B330B110B340B330B340B330B340B330B340B330B340B330B34 %0B330B340B330B330B58AEFD3EFFAE590B0B0B2D0B340B2D0B340B2D0B34 %0B2D0B2D0B2D0B340B340B343334113434340B340B340B340B340B340B34 %0B340B340B340B340B340B340B340B340B340B34A8FD3CFF592D0A0B0B2D %0B0B0B2D0B0B0B2DFD050B2D0BFD08343A3434343AFD06340B340B0B0B33 %0B110B330B110B330B110B330B110B330B110B340B1183FD3AFF340B0B33 %0B340B330B340B330B340B2D0B34343A345F345F345F343B345F343B345F %343B345F345F345FFD04340B340B340B340B340B340B340B340B340B340B %340B340B3484FD37FFA80B0B0B2D0B0B0B2D0B0B0B2D0B0B0BFD04343B34 %34343A3434343A3434343A3434343A3434343A3434343BFD043411330B34 %0B330B340B330B340B330B340B330B340B330B1183FD2DFFAFFD07FF830B %2D0B2D0B340B2D0B340B2D0BFD04345F343B345F343B345F343B345F343B %345F343B345F343B345F343B345F343B345F34340B340B340B340B340B34 %0B340B340B340B340B340B3383FD2BFF830B83FD05FF590A0B0B2D0B0B0B %2DFD050B34343BFD23343A343411340B110B330B110B330B110B330B110B %330B330B0B83FD29FFA80B330B84FFFFFF580B330B340B330B340B330B34 %343B345F343B345F343B345F343B345F343B345F343B345F343B345F343B %345F343B345F343B345F343B345F34340B340B340B340B340B340B340B34 %0B340B340B1183FD27FFA80B330B110B84AE340A2D0B0B0B2D0B0B0B2D0B %33343B3434343A3434343A3434343A3434343A3434343A3434343A343434 %3A3434343A3434343A3434343A3434343B34340B330B330B340B330B340B %330B340B330B340B0B84FD26FF0B330B340B340B340B2D0B340B2D0B340B %2D0B34343B345F343B345F343B345F343B345F343B345F343B345F343B34 %5F343B345F343B345F343B345F343B345F343B345F343B34340B340B340B %340B340B340B340B340B340B34A8FD24FF2D0B0B110B330B110B330B0B0B %2DFD050B34343AFD34340B110B330B110B330B110B330B110B340B34A8FD %22FF58110B340B340B340B340B340B340B330B33343B345F343B345F343B %345F343B345F343B345F343B345F343B345F343B345F343B345F343B345F %343B345F343B345F343B345F343B345F34340B340B340B340B340B340B34 %0B340B340B5FFD21FF830B0B340B330B340B330B340B0B0B2D0B0B0B3B34 %34343A3434343A3434343A3434343A3434343A3434343A3434343A343434 %3A3434343A3434343A3434343A3434343A3434343A3434343B34340B340B %330B340B330B340B330B340B330B84FD1FFFA8340B340B340B340B340B34 %0B340B340B33343B345F343B345F343B345F343B345F343B345F343B345F %343A343B343A343A343A343B343B345F343B345F343B345F343B345F343B %345F343B345F34340B340B340B340B340B340B340B340B340BAFFD1EFF34 %0B340B110B330B110B330B110B330B0B0B3BFD163412FD04345F595F585F %585FFD043412FD17340B330B110B330B110B330B110B330B0B2DFD1DFF83 %0B340B340B340B340B340B340B340B34345F343B345F343B345F343B345F %343B345F343B343B343B598484AFAEFD09FFA8AE835F343A345F343B345F %343B345F343B345F343B345F345F34350B340B340B340B340B340B340B34 %0B1183FD1BFFAF0B110B340B330B340B330B340B330B34343A343A343434 %3A3434343A3434343A3434343A34343484A8FD05FFA8FFFFFFA8FFFFFFCA %FFFFFFA8FF835FFD05343A3434343A3434343A3434343A341312340B330B %340B330B340B330B340B330B34A8FD1AFF34110B340B340B340B340B340B %340B34345F343B345F343B345F343B345F343B345F34343483A8FD18FFAF %5F5F343A345F343B345F343B345F343A3435133513340B340B340B340B34 %0B340B340B340B5FFD19FF840B0B330B110B330B110B330B110B330BFD11 %34123459AECBFFCAFFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8 %FFFFFFA883FD0B343A12130C3512130C340B110B330B110B330B110B330B %110BA8FD18FF580B340B340B340B340B340B340B34333B345F343B345F34 %3B345F343B345F343B345FA8FD20FFAF593A343B345F343B345F34351335 %1335133513340B340B340B340B340B340B340B3434FD17FFA80B330B340B %330B340B330B340B330BFD04343A3434343A3434343A3434343A3483A8FF %FFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFF %A8FFFFFF833A3434343A34341235121312351213123512330B340B330B34 %0B330B340B330B0B83FD16FF34330B340B340B340B340B340B340B34345F %343B345F343B345F343B345F343A34AEFD26FFA85F343B345F3435123513 %3512351335123512330B340B340B340B340B340B340B58FD15FF830B0B33 %0B110B330B110B330B110BFD1234AECBFFA8FFA8FFA8FFA8FFA8FFA8FFA8 %FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA85F123412 %3512130C3512130C3512130C350B0B0B330B110B330B110B330B110BA8FD %14FF590B340B340B340B340B340B340B34345F345F343B345F343B345F34 %3B343B5FFD2CFF83123513351335133513351335133513350B340B340B34 %0B340B340B340B3434FD13FFAE0B110B340B330B340B330B340B0B113B34 %34343A3434343A3434343A343434FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8 %FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FF84 %350C3512131235121312351213123513340B340B330B340B330B340B330B %33A8FD12FF59110B340B340B340B340B340B340B3A345F343B345F343B34 %5F343B343A34FD2DFF84846035123513351235133512351335123513340B %340B340B340B340B340B340B83FD11FFAE340B330B110B330B110B330B11 %0BFD1034AFFFFFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8 %FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FF8460598459350C3512130C %3512130C3512130C350C0B0B330B110B330B110B330B0B0BFD11FFA80B34 %0B340B340B340B340B340B34343B345F343B345F343B345F343B34AEFD2C %FF848460848484593513351335133513351335133513350B340B340B340B %340B340B340B3484FD10FF580B0B340B330B340B330B340B0B113B343434 %3A3434343A3434343A3483FD04FFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFF %A8FFA8AE84A884A884AFA8FFCAFFFFFFA8FFFFFFA8FFFFFF848460845F84 %6084351312131235121312351213123513340B340B330B340B330B340B33 %0B5FFD0FFFAF340B340B340B340B340B340B340B3A345F343B345F343B34 %5F343B345FFD14FFA8846084608460846084608484AFAEFD09FF84846084 %608460848484133513351235133512351335123512340B340B340B340B34 %0B340B340BFD0FFFA80B330B110B330B110B330B110BFD0F3483FFA8FFA8 %FFA8FFA8FFA8FFA8FFA8FFA8FFA8A85F845F8459845F8459845F8459845F %8484FFFFFFA8FFCAFF84845F8459845F84598460600C3512130C3512130C %3512130C350B0B0B330B110B330B110B330B0B83FD0EFF59110B340B340B %340B340B340B34345F345F343B345F343B345F343A5FFD11FFA884608484 %84608484846084848460848484608484AFFD04FF848460846084848460FD %04843B13351335133513351335133513350B340B340B340B340B340B340B %83FD0EFF340B340B330B340B330B340B330B3A3434343A3434343A343434 %3A34AEFD04FFA8FFFFFFA8FFFFFFA8FFFFFF848460845F8460845F846084 %5F8460845F8460845F845FA8CAFF84605F8460845F8460845F8460845935 %12131235121312351213123512340B340B330B340B330B340B0B2DFD0EFF %0B340B340B340B340B340B340B34345F343B345F343B345F343B345FFD10 %FF84846084608460846084608460846084608460846084608460AE848460 %84608460846084608460848484353513351235133512351335123512330B %340B340B340B340B340B34A8FD0CFF83110B110B330B110B330B110B33FD %0E3483FFA8FFA8FFA8FFA8FFA8FFA8FFCAFF84605F8459845F8459845F84 %59845F8459845F8459845F8459845F6059845F8459845F8459845F845984 %605F0C3512130C3512130C3512130C340B110B330B110B330B110B330B84 %FD0CFF840B340B340B340B340B340B340B3B345F343B345F343B345F343B %34FD0FFFFD04846084848460848484608484846084848460848484608484 %846084848460848484608484846084848460351335133513351335133513 %3512340B340B340B340B340B340B1159FD0CFF34110B330B340B330B340B %330BFD04343A3434343A3434343A3483FD04FFA8FFFFFFA8FFFFFFA8FFA8 %605F8460845F8460845F8460845F8460845F8460845F8460845F8460845F %8460845F8460845F8460845F84608435131213123512131235121312350B %330B340B330B340B330B340B59FD0CFF340B340B340B340B340B340B3433 %5F343B345F343B345F343B343BA8FD0DFFAE846084608460846084608460 %846084608460846084608460846084608460846084608460846084608460 %846084846012351335123513351235133513340B340B340B340B340B340B %3434FD0CFF0B0B0B330B110B330B110B330BFD0E34FFCBFFA8FFA8FFA8FF %A8FFA8FFFFA859845F8459845F8459845F8459845F8459845F8459845F84 %59845F8459845F8459845F8459845F8459845F8459845F350C3512130C35 %12130C3512130C330B110B330B110B330B110B34A8FD0AFFAE340B340B34 %0B340B340B340B34345F343B345F343B345F343B3483FD0EFFFD04846084 %848460848484608484846084848460848484608484846084848460848484 %60848484608484846084848435351335133513351335133513350B340B34 %0B340B340B340B340BFD0BFFA80B110B340B330B340B330B340B34343A34 %34343A3434343A34345FFFFFFFA8FFFFFFA8FFFFFFA8FFA8605F8460845F %8460845F8460845F8460845F8460845F8460845F8460845F8460845F8460 %845F8460845F8460845F8460350C35121312351213123512130C340B330B %340B330B340B330B3384FD0AFF83110B340B340B340B340B340B34343B34 %5F343B345F343B343B34AEFD0DFF8A608460846084608460846084608460 %846084608460846084608460846084608460846084608460846084608460 %8460845F351235133512351335123513350B340B340B340B340B340B340B %84FD0AFF830B330B110B330B110B330B0B0BFD0D34A8FFA8FFA8FFA8FFA8 %FFA8FFCAFF5F845F8459845F8459845F8459845F8459845F8459845F8459 %845F8459845F8459845F8459845F8459845F8459845F845F6012130C3512 %130C3512130C3512120B330B110B330B110B330B0B59FD0AFF5F110B340B %340B340B340B340B34343B345F343B345F343B345F34FD0DFFA884848460 %848484608484846084848460848484608484846084848460848484608484 %84608484846084848460848484608460351235133513351335133513350C %340B340B340B340B340B340B84FD0AFF830B340B330B340B330B340B0B0B %3B3434343A3434343A3434345FA8FFA8FFFFFFA8FFFFFFA8FFFFAE598460 %845F8460845F8460845F8460845F8460845F8460845F8460845F8460845F %8460845F8460845F8460845F8460845F8412131235121312351213123512 %340B340B330B340B330B340B0B59FD0AFF59110B340B340B340B340B340B %34345F343B345F343B345F343A34FD0DFF84846084608460846084608460 %846084608460846084608460846084608460846084608460846084608460 %84608460846084603513351235133512351335123512340B340B340B340B %340B340B5FFD0AFF5F0B110B330B110B330B110B330BFD0D34A8FFA8FFA8 %FFA8FFA8FFA8FFA8845F8459845F8459845F8459845F8459845F8459845F %8459845F8459845F8459845F8459845F8459845F8459845F845984606012 %3512130C3512130C3512130C340B110B330B110B340B340B1159FD0AFF59 %110B340B340B340B340B340B34345F343B345F343B345F343B34FD0DFF84 %846084848460848484608484846084848460848484608484846084848460 %848484608484846084848460848484608484846035133513351335133513 %3513350C330B340B330B3384FD0FFF5F0B330B340B330B340B330B331134 %343A3434343A3434343A3434A8FFFFFFA8FFFFFFA8FFFFFFCAAE5F845F84 %60845F8460845F8460845F8460845F8460845F8460845F8460845F846084 %5F8460845F8460845F8460845F8460841235121312351213123512131234 %0B0B0B2D0B0B0BA8FD0FFF59110B340B340B340B340B340B34343B345F34 %3B345F343B343B34FD0DFFA8846084608460846084608460846084608460 %846084608460846084608460846084608460846084608460846084608460 %8460351235133512351335123513350B330B2D0B340B0B84FD0FFF830B33 %0B110B330B110B330B0B0B3AFD0B343AA8FFA8FFA8FFA8FFA8FFA8FFFFAF %59845F8459845F8459845F8459845F8459845F8459845F8459845F845984 %5F8459845F8459845F8459845F8459845F845F6012130C3512130C351213 %0C35120C0B2DFD050BAEFD0FFF5F110B340B340B340B340B340B34343B34 %5F343B345F343B345F34AEFD0CFFAF846084608484846084848460848484 %608484846084848460848484608484846084848460848484608484846084 %8484608A5F351235133513351335133513350B340B330B340B34A8FD0FFF %830B340B330B340B330B340B330B3A3434343A3434343AFD043483FFA8FF %FFFFA8FFFFFFA8FFFFFF848460845F8460845F8460845F8460845F846084 %5F8460845F8460845F8460845F8460845F8460845F8460845F8460846060 %121312351213123512131235120B0B2DFD050BFD10FF84330B340B340B34 %0B340B340B34345F343B345F343B345F343B3484FD0DFFAF5F8460846084 %608460846084608460846084608460846084608460846084608460846084 %60846084608460846084848435351335123513351235133512350B2D0B34 %0B2D0B34FD10FFAE0B0B0B330B110B330B110B330BFD0E34FFCAFFA8FFA8 %FFA8FFA8FFA8FFA88459845F8459845F8459845F8459845F8459845F8459 %845F8459845F8459845F8459845F8459845F8459845F8459845F350C3512 %130C3512130C3512130C2D0B0B0B2D0B0B2DFD11FF340B340B340B340B34 %0B340B34345F343B345F343B345F343B345FAEFD0DFFFD04846084848460 %848484608484846084848460848484608484846084848460848484608484 %84608484846084848413351335133513351335133513350B330B340B330B %83FD11FF2D0B0B340B330B340B330B340B34343A3434343A3434343A3434 %1284FFFFA8FFFFFFA8FFFFFFA8FFFFFF5F8460845F8460845F8460845F84 %60845F8460845F8460845F8460845F8460845F8460845F8460845F846084 %5F8435131235121312351213123512130B2D0B0B0B2D0B0B59FD11FF5F0B %340B340B340B340B340B34333B345F343B345F343B345F343A59FD0EFFAF %608460846084608460846084608460846084608460846084608460846084 %6084608460846084608460846084603B1335123513351235133512351334 %0B340B2D0B330BA8FD11FF590B0B110B330B110B330B110BFD0F34A8FFA8 %FFA8FFA8FFA8FFA8FFA8FFFFA859845F8459845F8459845F8459845F8459 %845F8459845F8459845F8459845F8459845F8459845F84598460600C3512 %130C3512130C3512130C350B0B0B2D0B0B0B2DA8FD11FFAE0B340B340B34 %0B340B340B340B34345F343B345F343B345F343B3484FD0FFFAF60846084 %848460848484608484846084848460848484608484846084848460848484 %6084848460848484353513351335133513351335133512330B340B330B33 %34FD12FFA8340B330B340B330B340B330B331134343A3434343A3434343A %343434FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8845F845F8460845F846084 %5F8460845F8460845F8460845F8484AF5F8460845F8460845F8460845F84 %60350C351213123512131235121312340B0B0B2D0B0B0A83FD13FF34340B %340B340B340B340B340B34343B345F343B345F343B345F343483FD10FFAF %6084608460846084608460846084608460846084608484FFFFFF60846084 %6084608460846084846012351235133512351335123513350B330B2D0B34 %0B0B83FD13FF5F0B330B110B330B110B330B110BFD0F34A8FFA8FFA8FFA8 %FFA8FFA8FFA8FFA8FFCAAF606059845F8459845F8459845F8459845F8459 %84A8FFA8FFFFAF59845F8459845F8459846060121312130C3512130C3512 %130C350C0B0B2DFD050BFD14FF84340B340B340B340B340B340B34345F34 %5F343B345F343B345F343B3484FD12FF848A608460848484608484846084 %608484AFFD08FF8484608484846084848435351335133513351335133513 %3513340B340B330B340B59FD15FF0B330B330B340B330B340B330BFD0434 %3A3434343A3434343A343434AEFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFF %FFA8AF84845F845F605F845F845F84A8FFCAFFFFFFA8FFFFFFCBAF60845F %8460845F84591312351213123512131235121312350B0B0B2DFD040B83FD %15FF5F0B340B340B340B340B340B340B34343B345F343B345F343B345F34 %3A34FD15FFAFAFA8AF84AE84AFA8FD0FFF60846084608460351335123513 %3512351335123513350C330B2D0B340B2D0BFD16FF830B0B330B110B330B %110B330B0B0BFD0F3459FFCBFFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8 %FFFFFFCAFFCAFFCAFFCAFFA8FFA8FFA8FFA8FFA8FFA8FFCAAF5F60598460 %350C3512130C3512130C3512130C35120B0B2D0B0B0B2D0A59FD17FF3334 %0B340B340B340B340B340B34345F345F343B345F343B345F343B343A83FD %2DFF60848460123513351335133513351335133513340B340B330B340B0B %A8FD17FF5F0B340B330B340B330B340B330BFD04343A3434343A3434343A %FD043483FFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8 %FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFAF8460121312351213123512 %131235121312350B0B0B2DFD040B2EFD18FFA8330B340B340B340B340B34 %0B340B34343B345F343B345F343B345F343B343B84FD2CFF841235133512 %35133512351335123513350C330B2D0B340B2D0BA8FD19FF340B0B330B11 %0B330B110B330B110BFD113483FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FF %A8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFCBA834340C35 %12130C3512130C3512130C350C0B0B2D0B0B0B2D0B34FD1AFFA80B340B34 %0B340B340B340B340B34345F345F343B345F343B345F343B345F343A83FD %28FFAE343B343B13351335133513351335133513340B340B330B340B0B83 %FD1BFF340B340B330B340B330B340B330B333434343A3434343A3434343A %3434343A343459FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFFA8FFFFFF %A8FFFFFFA8FFFFFFA8FFFFFFA8FFA85F123A343434351213123512131235 %121312340B0B0B2DFD040B2DFD1CFF84110B340B340B340B340B340B340B %34343B345F343B345F343B345F343B345F343434AEFD22FFA85F343B345F %343B343B133512351335123513350B330B2D0B340B2D0BAEFD1DFF340B0B %330B110B330B110B330B110BFD1334125FA8FFCAFFA8FFA8FFA8FFA8FFA8 %FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFA8FFCBFF5F3412FD08343512130C35 %12130C350B0B0B2D0B0B0B2D0A58FD1EFFAE0B340B340B340B340B340B34 %0B340B34345F343B345F343B345F343B345F343B345F343B5FFD1BFFA884 %343A345F343B345F343B345F34351335133513350C2D0B340B330B340B34 %A8FD1FFF580B340B330B340B330B340B330B340B34343A3434343A343434 %3A3434343A3434343A34343483A8FD05FFA8FFFFFFA8FFFFFFA8FFFFFFA8 %FFFFFFA8AE5F34123A3434343A3434343A3434343A34340C3512130C2D0B %0B0B2DFD040B7DFD20FFAE340B340B340B340B340B340B340B340B34345F %343B345F343B345F343B345F343B345F343A343B598484FD0FFFA8AE835F %343A343B345F343B345F343B345F343B345F34351235122D0B340B2D0B34 %0B0B34FD22FF590B0B330B110B330B110B330B110B330BFD17341234345F %5F8483AEA8AEA8AE84A8838358FD1834350C2D0B0B0B2DFD050BFD24FF34 %340B340B340B340B340B340B340B340B34343B345F343B345F343B345F34 %3B345F343B345F343B343B343A343B343B345F343B343A343A345F343B34 %5F343B345F343B345F343B345F343B345F345F34340B330B340B330B340B %A8FD24FFAE0B330B330B340B330B340B330B340B0B0BFD04343A3434343A %3434343A3434343A3434343A3434343A3434343A3434343A3434343A3434 %343A3434343A3434343A3434343A3434343A3434343B34330B330B0B0B2D %0B0B0483FD26FF840B340B340B340B340B340B340B330B2D0B34343B345F %343B345F343B345F343B345F343B345F343B345F343B345F343B345F343B %345F343B345F343B345F343B345F343B345F343B345F343B34340B340B34 %0B2D0B340B59FD28FF580B340B110B330B110B340B0B0B2D0B0B0BFD3934 %0B0B0B330B110B330B0B0458FD2AFF340B340B340B340B340B330B340B33 %0B340B34345F343B345F343B345F343B345F343B345F343B345F343B345F %343B345F343B345F343B345F343B345F343B345F343B345F343B343B3334 %0B340B340B340B340B58FD2BFFA8340B330B340B11FD050B2D0B0B0B2D0B %0B0B3B3434343A3434343A3434343A3434343A3434343A3434343A343434 %3A3434343A3434343A3434343A3434343A3434343B34340B340B330B340B %330B340B34A8FD2CFF84340B340B3383FF2E0B0B2D0B340B2D0B340B2D0B %3A343B345F343B345F343B345F343B345F343B345F343B345F343B345F34 %3B345F343B345F343B345F343B345F343B34340B340B340B340B340B340B %340B84FD2DFF830B0B0B59FFFFFF2EFD040B2D0B0B0B2D0B0B0BFD2D3411 %110B330B110B330B110B330B110B330B83FD2DFF831183FD05FF580B0B34 %0B330B340B330B340B34345F345F345F343B345F343B345F343B345F343B %345F343B345F343B345F343B345F343B345F345F343411340B340B340B34 %0B340B340B340B340B340B84FD2DFFA8FD07FF590B0B2D0B0B0B2D0B0B0B %2D0B0B0BFD04343B3434343A3434343A3434343A3434343A3434343A3434 %343A3434343AFD043411330B340B330B340B330B340B330B340B330B340B %0B0BAFFD35FF84340B330B2D0B340B2D0B340B2D0B343334345F343B345F %343B345F343B345F343B345F343B345F343B345F343B343B34340B340B34 %0B340B340B340B340B340B340B340B340B340B84FD37FFA85804FD040B2D %0B0B0B2DFD070BFD04343B3434343AFD07343A3434343AFD06340B330B11 %0B330B110B330B110B330B110B330B110B330B0B0B84FD3AFF840B0B0B34 %0B330B340B330B340B330B330B330BFD04345F343B345F345F345F343B34 %5FFD04340B340B340B340B340B340B340B340B340B340B340B340B340B33 %0BAEFD3DFF340B0A2D0B0B0B2D0B0B0B2D0B0B0B2D0B0B0B2D0B0B0B340B %340B3411340B340B340B340B0B0B340B330B340B330B340B330B340B330B %340B330B340B330B0B33AEFD3FFFA8340B2D0B2D0B340B2D0B340B2D0B34 %0B2D0B330B2D0B330B2D0B340B340B340B340B340B340B340B340B340B34 %0B340B340B340B340B340B340B340B1134FD43FF832DFD040B2D0B0B0B2D %0B0B0B2D0B0B0B2D0B0B0B2D0B0B0B330B110B330B110B330B110B330B11 %0B330B110B330B110B330B110B330B110B117DFD46FF84580B0B0B330B34 %0B330B340B330B340B330B340B330B340B340B340B340B340B340B340B34 %0B340B340B340B340B340B340B340B340B58A8FD49FFA858330B0B0B2D0B %0B0B2D0B0B0B2D0B0B0B2D0B0B0B340B330B340B330B340B330B340B330B %340B330B340B330B340B330B110B83FD4DFFAE8458340B0B0B2D0B340B2D %0B340B2D0B340B340B340B340B340B340B340B340B340B340B340B340B34 %0B340B340B1134FD52FFA88458340B0B04FD0A0B110B330B110B330B110B %330B110B330B110B330B110B330B0B0B3484FD58FF848359580B340B330B %330B340B340B340B340B340B340B340B340B340B340B340B340B1134AEFD %5FFFA8FFA8A883A80B110B340B330B340B330B340B330B340B330B340B0B %0B5883FD67FF340B340B340B340B340B340B340B340B340B110B3459AFFD %69FF0B110B110B330B110B330B110B330B0B0B345884A8FD6BFF340B340B %340B340B340B110B340B3459AEFD6FFFFD070B330B34588383FD73FF8358 %FD0483AEA8FDFCFFFDFCFFFDBFFFFF %%EndData endstream endobj 19 0 obj <>stream +%AI12_CompressedDatax}iCPQ- ͍TT Yfy?~;{I:swGNUwuum] ڱ`b8eQ'bޏGl.GhTJf;,`3 L:C 㿺t2gb<?Jb\P+afjsL.dsIcd+[[/fh"7]0MFwjÑlxrb8/{*BO f*=tI[ϡۀu^COWP%,'5CZ֘(\*$&df$_^P;?Eqޖ&+bj1ěpУD@A{ڨGeR!a~'scVTnv h046)ƀgDR_Tc=O`x/zuZaej +15ÿq^Q/2u0rl;6;pl.~F m t5sbr@oty: L%M􌢛{pI ::쏄\vVa:QV Tvu]4M[]P @P;AD{ڋ\d{ O;𨕖m ոN#bΪ*'AHc? = %Owԗ@8[n/|ή`]xpc_d'S4x:Kg9@C)86M%T +=1l:p9>iV}_)68_~,??0F FJVw悀GyLf[S|Jjg>y&Ϧ22+iA"0G~ Lm}Upٺ`' L5o9ė{?ߣ1\AEMπX|]'{0p1PF}~#ކdz=Xm>0;Z(MM1xܑ?O_wl4 4Îr*WBwd?̿' ]GoS~|iFw|Z ~@|>n +sɛw82qi92$gE^{`J?סw `fꊳb=/R7ar8@Ķ0?dp+/Bخ&/a@36f԰o/_K5 QJ?LJY-k _W}ʺu_'˷0`VGn~l '?39ܚ ZUFc+aEFxgWHC|GΆh8ݞwrlgr)ֵ*TȦ(awD#ao JDLg$<渤?dnАs;WF?shYr៶Пe=M%L&K>v6g.`TC*%YHA y& @+s;M )ejIW(@Y֯zp6A[U1ϥ-@?n?N:$Iѵ׵((u8^ڪy7jK%+e{KfMƌ]N& gG)Y @?`./F; b/QuJy:ŷ_/A[*7acհ74TxsvAt>f+܇`aH2/.bMHJҩ¯qm1)Fୌˤ3Nꫮ񊣚+YJa)$vn_+NmkݾPHgm2ilDroh~wPL,cPN$ҫ5l&tY1M)83p X΢gsY֡RttLPvR,y +$AsKx[;m|Rfsq򥸊n-Ӻ|ݙ" fUJ[2 +SThZ7}j6(ҳt7;2=ky_\'#z;k<A<I7<~.qELr{$ uu֢ &SOJ{( +"k4*mgE`w5ɱӼYLwc'@OGg]Qv2m5t]ed10~$'WnIOfCUEpwj.wVz3Ao`Y{RutթHCwO@ÑTдU$I4 mQC0fÒ}f{hT8֥OO^^?j$H,I? M}wO^3X30Hi{ύ`ˁ͉`pELt,@A(Qx-t:%;q@)b3"̆c m M5Ù1BFq+'`p;/:kIO"]v0#OkB".cqa"p>5W?|i"?߽qw8+@7)y:TblsO1,&,M}[zrf]ꂶŝ'NH14~単o}陔dSLne|3|јS5p)J4ө(7sh#c t)m+}Z_ *\S'"h2_i"ҳ]q҈VCZꔋ6HL;/HOcϧ!:א:ў-Iiݹϒ67a?z,qMvb쐞2dAC +cѣ= +߾޽HzH;s +8:Lǯ.<~iپא"-G[c:?dTHbi0wLSBݘ׍H\=҈e\l_@z ^mHr0/z6svcaaDg'i*fH 3RȳINB{FMk;җy@Mdqp&eۿi 8,QXFGڸ)1Miӌ=RƄesc@YXo">m)p|>!]tsd!۝6Ioaj@UAo b7Ngj[,z!TQe6c1smY1Cz 8.䯏{'3S yp#Qƈo珬IU<2[LHr,}}j oCݖ8,"5N9}LXuNoϯ>:Ո_Y(Fx3} YۇVo_vT&QsC{`# +ƅ`Gws68?"Ńep +q[t|qY(fCb= Toonp?f [jo}ix9gY5 h.}3 --(B_Ã]4̉xvv@H7Ã^컍*8; 4sЊD 7;+՗&HSgR_PBQ{5N)S+/lab7u⃊4i@ml +#EָLXS-=/ W:M idotsnD6V=ҍc{& |Ү:EOfY$7dHMs +<kw&Ob`jInw)PCr @R\tp$-И/NL "\21aڪ+~qT>?c՗T~5SYCWe~r8 ͢ ЗB%~lvIa0S-&I *V?@2j6In]d$tGi"W2sb;/BA%rK6tQ~ӂ?Q`ޥn Sf>H5*H\8T) ;bOn?= <ɣ3 /z&Ƿ͟S y;rvcbw~Yb)]9JǁXRD!9|M0z*y= ˬH X?ݖ2slJ%~<郮U}j#2l*_ɭC{4 M5dGeFvn\GƣRbbԨ(P%n024p:#F[]?MwȢ/¼CvZReMKG8y/ǟfR=nb^uݍ_V_wݚڄ8h^ըC-i*{ ಙNC8 P;VX[`IگM;aFK!4%_٩iJJ'MvRaj6jI}AQ7QZ 5jY1ZO^@^3b!_r]z(šSvYlIYɠ(F3UeE Nv6V,gl=$q3uQ(kWiZ2q渢nuu ^nfW>nVKAZ 'hp/ҩ#ei7@ ,HwvЦv:ngnBvD MSGyvmٛ<^2LTPNG,-M5ĭ[Wv2w5[w>Y x.鼤 _&` ߶WߖG3%47[ĵ-oڗ'K}LUzgm=_;205i;+2.e' g'G;1/qXF;EWZ"Ga EHl;F?8Բ*a6};xg]r+cbK#:G@|&a`8kH@iƢ3Q5c4Oͦ)檴;ݑ٣" GԠz_^N5 {JjGFg8&\\ʣ9 +Y!s–>N:tl81fo`No䡁&I>$P=]۩x  {P2[42ّW}Q;ё\b`(=.)N^%X6Laz{l73MdHUh0wu :l"rml%Ro~:I]Yi2 aYD#$KXsȬtBx~ӇV)~%Vh 7V_7KJ43ݬEݘ$n_35b8FfzaMB"-0 o +V rGv=5˺omhw7YфYf6Byms^)Ob1ܖsYY(ٯr4Řaá Pt ~帛.D9]6{rLP h#\m4 吰T#}ua[پWuZ־P:_^4vE#_̫6DIQ6r_HRh{>uJK'$:Rj f +L0{}f4;GRl)6Xf\UNf#;SZ9SeqIqKb*Nq'\JDIO:,/"ٶj!*Re`0eǺ9ņ+v/%Ky3ǝv]bVX/&YCW2gSc؇)GNɞRA^8h?\J|9s*6ı>^Lz +UI{En n@҆Y9":`j`,YQsɩerw9ɁΫKL ^LMu|:O·9e3#0ɠda+;s{:O:hA+St*NNl:nJNUUn P7 SϟRgީX X`.%2:PqUb.53^iI Xb.G!Q͈T"{I ע]eP쪟\A +kVZ8?rYݴ[ -͡]ghqmgLvqߡZl(r8CsQn3d!x<>7N 2Po>̃:7I ~%2ōv[UɎKb8]-&wL_K'[1M"Ŋ^m7{N;L䲧mʶ6Ne8t<>zD]JzcSQȕNƈ{23Ed[% *"sĞҜ⒥v )C %#2Aeh`ug[Lj ~UAdqϴ+5GISq2]^aaC_ri-EknؠK +q|n/!3=]0='«Ra}1T9F ZWSDuu%$Jz5niCuj6)7wL;,yI3V^wp>,<+]lz8 uí\JUG[-Z=&qsrq䕳|=x+]g\JEz8]_]J:v5ubDrȰ+qD%Kb/dSX9*N(3!)kË%Geg{BpfLg}@a' :%+卻--ʱ?VTc4M oInWkiᬸ %("\ /t֍ǃiokc q;m!/b=u=\& "gl]?kHe7hn\+RgѕcǾCߙ",>%1n!-<4+Ʇd^PԴ 3S)!u)^WZ +gb?R +tJhvW/.pO{,SNt2 *384,t"<Ƚ@;67,ZTr ݵd`0e7p ipCs*Ӵp]G8)9$e73dvVL?|Af=\ ^jV׃u-'wM]9&o4VoAr6@ՇIp.|k-˞kLn-}JP·cgӃ]۽mE/6}S[*bWXa7M*vZmX{Zuq-3Jc lZEj.vCdlŽv2Es~iz3:Uu+[i{GFbxֵr =jf-7eA¹.Yf(1o!Үe$?W5 c~H Q:-;ܚw|ظCTtQAe[X9&N%^ʶMy*Tsv)dsS#S'zFsp/r9oKtt2QϔK1$;-oT)O]Љtaj[ڦ.UMGJtU,k#Ywq.]MGTիHtvWy|\N&-ڼBڦ|hj:,Th5WQDPMGcYk5}n:HtٶKVӑ|H+Z5KJ-][5ɰWnc\_5iTӑjȷRMj:(c}:Ht+S%(F_MGeYC5mZH|k#m5:j:R-IVӑ$ZHtHiTMG +-99UMg꒪޼TwB|hb3( ^Nֺuzi)$ɺX:Ewo}u5kھدc[+id-w>Q*7{h]ϷKȳX?9YS:z,Kf :%ػ"xyY}3*BM;P+\sg!\t)Q745^mYCU-[1?;EeJ'zqtYޯ?.yt\42=]SQ\jS+ntkyT.EAQnt +:\0.ů;RmBmqѳ>_Ğj{& U>P}عG {M- YG=g'1I{)YXZZ(gZ ^4Q1]v{/a:72cՔC Z؜C \.i"쪟Z+Z 5`x;Yڟ}|O-/%Az:xj g-{p\!,/v- ;?.AwSaJrHh27[qZ=rʕT7ܔn[C] wn[;1q VODe%-PJbrK˹X̑Xoِv9N+ZϹ^X P=p<Re+-I撣轾=f +%R)e-0/Y1VƄދχ_["muFy/bʇ.ׇ%{$kdti{R=F_Z<Ը4gQ0e7pc{+$G9OSK_S/ub$|JP#6(]e*cU,r)/KSٞjU6{rtttD<9\swow exř>l-+ HuizP%fF :]t鐚aGf*RsiZXٖ7־/m/p^o)S\_kH49C*8]"N?m lЌ24e7'ܣjznI ƖRV4Tnm12NBXR ;|9Ԇ>I7?URaom)u 7s騘-nfyPVنs_kS +=LUTB_V`'($Pݝ<-񑳴lJ=ei.TqgjSWݡ(Z֋CVAKz:FqXCt)l뽾VW޾Ns8rJ(vy`pϫ^q-/1%Y7k,WI9w +^"iQ )#hUjt>zNź|d`v׻38?&dgs!$%ݯ "rZ윢L>۵O5Vn!k%a%oָ"$ %l?Fyl)Tzq +u)mݳZc%Em{o]v ?,ňAR/K [5<"@[_ +>ʍz=VUPh̆&*@kETRhVH[U7Xm}d*@e"<*,'.ndGy)Mn͗h3\.3$K R>q)s.W~S3L7TN9{gCQ^ɵ +[ ewle3tr2gCs1Ľ~iZP{L2isΕҪW`~1|N;Y^?|&} +f{ܒ|3n]^?뀂{o\!$V?+is^uUiWО@ʽ~~1^%y~NPn'빏MGm=5g蒥 +(-OY9[Lg-}f9^?UwI)ͷIXVO-"J"Ui{w/9ڿ)[迬^~[rnS5'L]3ϴ /^[*^?gs^Z^?C,yyƗ׏ EYGw}|K-CtSsVeN!|egı^?UikX]e{+q}|||(VzZ/6)5*Qn^9ǵ9'k3;sE_dm^?Lb맟IW.~Άc5O]u|8pxϱ aYǽ~[{I\K\^?.kFOSTܢ)ƩW n$r_ ShQ%!-c kx>z!.PJpEaSx%vfnKiҜ"7+2 淓wg|OGfljɢݣǍ<(֮VvR ~sK?נQz;<ڹsNO۟Cb7S:r$u݊O#lvD1ؘD ?6\,tdqEK,.F~ԘCv}WٯoNΙ(bpg=Jßj {$ޘm#>N6ą@Gt1Q<+bvP>H84[Djq, _ +T˅h\Memu*2'xkI}4Jldv.U+J$%JUQgg's_HGrQ-u5(l6XC*-lrX BJ4l5_v,qc_1DD'4C_<|%n6"7 1 > +3b? 1ϗſ%j)^_ f@~;eo]r_ܰE\{vE?W"~ ?>MaHQ *a&eQ;X¯=vKcH!pNaZWYD<-DFMl#7{b&u^̼ Bj +?2>b8ڃ%T,(C݈ _2 -Tg +56C+I̢KSa4!Y'wzdltZf*'\ +|&G2@2)h C6o_Ef0W:ؠ &*S[}hl澆@Nz#i7蔋R| +l +c+~nkr`S-Su#{:]I+[6r*?d)^_![)& [mTe` +=u/-MjdYUl/UīOlB2Yz6*]3a {'_Wۈ.^ §|3 A]R6MoW@oaX +.=aFrlpۄVR|+[w=΁/5K|'jvE%"&hsIˌ+ +`yX ;?IC|u~ڪ_ήZ^v*ri;$j#9[Vb%]ߏ+n |tZlg}\5ۏV^_OL!1~佞Y7]&ρ3r3~3;3Zb_rip[VLŽpN0ʸt*o_Sf#Q?U'Ggp,3Lr11kq+o1u2'7;;naS_qbDx6)'|+>U z3'ٖ0e-F%j8OKX9+dOzTuB%yE'Upm2舎vhCztcT(U D;'L{H\~ g*j NrJ pII"]beQjU$bYVT(^E ӀAnUVƠ7EڇpRwbtKV9'$Sy_pBzW_z{GmoUvI|ʸ}ź#a?緘Uo +Ou>2o-_EiX[ EzxW-6>+j79>. m=Q$Bz}9OgG?3/^ྠpp]d[m)9әys+lÿI)b&%,sѴZk[+(`8neӂ^m'T+L/cPUΗOc]>rxrn<L'O;xv=?;x}z/'^Txɴ7ή :`JU2BƍO8.eΊFYLOK9YԐ__0Wn֥↺xkE)j)oU5P!?8<+dW?~4. |UT *%l㴼^ %bwU(gN*P`\z2렌έ:sSn<}RE$^<Q[*pڊ^phn@H"RmeUKM7s=#=D2JS2/_7xeQؚ<LʬYOӅJ{<dO!B8xUHH8YO-?dʍVxJI gHE9ŗLnRABc$QH 0 gulᐎuȍY9" ;J8N :""Gp F^NpqRǢUa$>jX>?Vc>o'sǛ#/q 6 إd#_t9ëRs6Vj:j#\+Q.-سKrUgC A%]$q<j@KOQD-8, }Yft糣V( _P~h3Vg34a_jOYzs}i& O)GQcPH}L_U_22յC9R]+Kn/Q_r^X7N#L\nʕH6\Unrxk3B/Uj)#\sM!iTt}WSQ'HTHJ UK@ۆ\^ZtFTKS'Ħh>srkzB:7yt{~*Hzq#;*i_gdAD7l\RJ @/ZO' +BXh|Ҟ8 +!>JrTz?1¨6Wi1!IJJĕ$/˦3,b$c8G}{DeoƃIMkye_| ?O,_7brW=&=IDTsA<3컒2 %E*sa޳=BhxL?j]藟rANslc.^j cXƾN'd'Пç9=‡KEuAlX{K3!maf-3٢>v{Sz"M]mT荞_٩BBRGBwIDH%4"|Q6.;GB]&jlݻ9NBK>ʷ^rN2:|G;WN ʕ- f_%w%jAN"3=2rrnS+U?)J^m]qr@5g+I{t@kxFۦf12v7qGYza㒹xel.R? cdqLzncBz>&'s}[?ST0P+>3uw03/4},y璩Tx Bf|ϧL2ͥ3dLqiGx`bfYO9yeǹ,@L&aO'?0iy(M,9uAi.p@ǚ21&>YhP@'fSY<\3M'Ihk&nL.g,Ǥ鍡yo<s LXV(6q,G:8 B&Je 9\ -ik4l"n>}w>$+P&q)MgS'aIC.1|MªfapɤHDSs|Or~ :'nU"f.BHzLb^LL6@SЬ8]zNgARai3t+e`)fAs +#L<2x‚@?9\*ea4^@m4I 8CAsN򲶑$ +EgvθPXWsI2)<]͈ +S~$Q%3.wHpEB @DT!ŇX!BZقfJsMr<~fFn:g\:'3I&A%Slc8ddR@>I|4E.j:`,$,RFoF:Kg-# [:ӈ8&J1#ٯdX06lfy~i#;vA:b'd@!Y&$L,MSxVI b Re_7"~OY;b6 $F\!F\"+Ğip ,T1 exDX(kYGc1_0~bw8DǬǓyw.LCf(gDi V/jf'L endstream endobj 6 0 obj [5 0 R] endobj 20 0 obj <> endobj xref 0 21 0000000000 65535 f +0000000016 00000 n +0000000144 00000 n +0000021426 00000 n +0000000000 00000 f +0000024082 00000 n +0000068364 00000 n +0000021477 00000 n +0000021801 00000 n +0000024381 00000 n +0000024268 00000 n +0000022674 00000 n +0000023521 00000 n +0000023569 00000 n +0000024152 00000 n +0000024183 00000 n +0000024454 00000 n +0000024628 00000 n +0000025908 00000 n +0000047272 00000 n +0000068387 00000 n +trailer <<3C8C4E798C174B5192D741AFA33E6B2C>]>> startxref 68587 %%EOF \ No newline at end of file diff --git a/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer.svg b/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer.svg new file mode 100644 index 0000000..39c3835 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer.svg @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer_512dp_shaded.svg b/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer_512dp_shaded.svg new file mode 100644 index 0000000..3c52e38 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/t_shirt/logo_cartographer_512dp_shaded.svg @@ -0,0 +1,1242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/docs/assets/t_shirt/pantone_lookup.png b/carto_ws/src/cartographer-master/docs/assets/t_shirt/pantone_lookup.png new file mode 100644 index 0000000..925b42c Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/assets/t_shirt/pantone_lookup.png differ diff --git a/carto_ws/src/cartographer-master/docs/assets/t_shirt/pantone_lookup.txt b/carto_ws/src/cartographer-master/docs/assets/t_shirt/pantone_lookup.txt new file mode 100644 index 0000000..0edff9d --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/assets/t_shirt/pantone_lookup.txt @@ -0,0 +1,6 @@ +1: Light grey: Pantone Solid Uncoated 7541 U +2: Light teal: Pantone Solid Uncoated 564 U +3: Dark teal: Pantone Solid Coated 7473 C +4: Light green: Pantone Solid Coated 2250 C +5: Medium green: Pantone Solid Coated 7731 C +6: Dark green: Pantone Solid Coated 7732 C diff --git a/carto_ws/src/cartographer-master/docs/source/autogenerate_groundtruth.png b/carto_ws/src/cartographer-master/docs/source/autogenerate_groundtruth.png new file mode 100644 index 0000000..191fbd7 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/source/autogenerate_groundtruth.png differ diff --git a/carto_ws/src/cartographer-master/docs/source/conf.py b/carto_ws/src/cartographer-master/docs/source/conf.py new file mode 100644 index 0000000..a3e27db --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/conf.py @@ -0,0 +1,275 @@ +# -*- coding: utf-8 -*- + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cartographer documentation build configuration file, created by +# sphinx-quickstart on Fri Jul 8 10:41:33 2016. +# +# This file is execfile()d with the current directory set to its +# containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys +import os +from datetime import datetime + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.todo', + 'sphinx.ext.mathjax', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'Cartographer' +copyright = u'{year} The Cartographer Authors'.format(year=datetime.now().year) + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# + +# The short X.Y version. +#version = '' +# The full version, including alpha/beta/rc tags. +#release = '' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = [] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +#keep_warnings = False + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = [] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +#html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Output file base name for HTML help builder. +htmlhelp_basename = 'Cartographerdoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + #'papersize': 'letterpaper', + + # The font size ('10pt', '11pt' or '12pt'). + #'pointsize': '10pt', + + # Additional stuff for the LaTeX preamble. + #'preamble': '', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + ('index', 'Cartographer.tex', u'Cartographer Documentation', + u'The Cartographer Authors', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + ('index', 'cartographer', u'Cartographer Documentation', + [u'The Cartographer Authors'], 1) +] + +# If true, show URL addresses after external links. +#man_show_urls = False + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ('index', 'Cartographer', u'Cartographer Documentation', + u'The Cartographer Authors', 'Cartographer', + 'Cartographer is a system that provides real-time simultaneous ' + 'localization and mapping (SLAM) in 2D and 3D across multiple platforms ' + 'and sensor configurations.', 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +#texinfo_no_detailmenu = False diff --git a/carto_ws/src/cartographer-master/docs/source/configuration.rst b/carto_ws/src/cartographer-master/docs/source/configuration.rst new file mode 100644 index 0000000..9fd79f6 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/configuration.rst @@ -0,0 +1,490 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +============= +Configuration +============= + +.. DO NOT EDIT! This documentation is AUTOGENERATED, please edit .proto files as +.. needed and run scripts/update_configuration_doc.py. + +cartographer.common.proto.CeresSolverOptions +============================================ + +bool use_nonmonotonic_steps + Configure the Ceres solver. See the Ceres documentation for more + information: https://code.google.com/p/ceres-solver/ + +int32 max_num_iterations + Not yet documented. + +int32 num_threads + Not yet documented. + + +cartographer.mapping.pose_graph.proto.ConstraintBuilderOptions +============================================================== + +double sampling_ratio + A constraint will be added if the proportion of added constraints to + potential constraints drops below this number. + +double max_constraint_distance + Threshold for poses to be considered near a submap. + +double min_score + 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. + +double global_localization_min_score + Threshold below which global localizations are not trusted. + +double loop_closure_translation_weight + Weight used in the optimization problem for the translational component of + loop closure constraints. + +double loop_closure_rotation_weight + Weight used in the optimization problem for the rotational component of + loop closure constraints. + +bool log_matches + If enabled, logs information of loop-closing constraints for debugging. + +cartographer.mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options + Options for the internally used scan matchers. + +cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options + Not yet documented. + +cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d + Not yet documented. + +cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d + Not yet documented. + + +cartographer.mapping.pose_graph.proto.OptimizationProblemOptions +================================================================ + +double huber_scale + Scaling parameter for Huber loss function. + +double acceleration_weight + Scaling parameter for the IMU acceleration term. + +double rotation_weight + Scaling parameter for the IMU rotation term. + +double local_slam_pose_translation_weight + Scaling parameter for translation between consecutive nodes based on the local SLAM pose. + +double local_slam_pose_rotation_weight + Scaling parameter for rotation between consecutive nodes based on the local SLAM pose. + +double odometry_translation_weight + Scaling parameter for translation between consecutive nodes based on the odometry. + +double odometry_rotation_weight + Scaling parameter for rotation between consecutive nodes based on the odometry. + +double fixed_frame_pose_translation_weight + Scaling parameter for the FixedFramePose translation. + +double fixed_frame_pose_rotation_weight + Scaling parameter for the FixedFramePose rotation. + +bool log_solver_summary + If true, the Ceres solver summary will be logged for every optimization. + +cartographer.common.proto.CeresSolverOptions ceres_solver_options + Not yet documented. + + +cartographer.mapping.proto.MapBuilderOptions +============================================ + +bool use_trajectory_builder_2d + Not yet documented. + +bool use_trajectory_builder_3d + Not yet documented. + +int32 num_background_threads + Number of threads to use for background computations. + +cartographer.mapping.proto.PoseGraphOptions pose_graph_options + Not yet documented. + + +cartographer.mapping.proto.MotionFilterOptions +============================================== + +double max_time_seconds + Threshold above which range data is inserted based on time. + +double max_distance_meters + Threshold above which range data is inserted based on linear motion. + +double max_angle_radians + Threshold above which range data is inserted based on rotational motion. + + +cartographer.mapping.proto.PoseGraphOptions +=========================================== + +int32 optimize_every_n_nodes + Online loop closure: If positive, will run the loop closure while the map + is built. + +cartographer.mapping.pose_graph.proto.ConstraintBuilderOptions constraint_builder_options + Options for the constraint builder. + +double matcher_translation_weight + Weight used in the optimization problem for the translational component of + non-loop-closure scan matcher constraints. + +double matcher_rotation_weight + Weight used in the optimization problem for the rotational component of + non-loop-closure scan matcher constraints. + +cartographer.mapping.pose_graph.proto.OptimizationProblemOptions optimization_problem_options + Options for the optimization problem. + +int32 max_num_final_iterations + Number of iterations to use in 'optimization_problem_options' for the final + optimization. + +double global_sampling_ratio + Rate at which we sample a single trajectory's nodes for global + localization. + +bool log_residual_histograms + Whether to output histograms for the pose residuals. + +double global_constraint_search_after_n_seconds + If for the duration specified by this option no global contraint has been + added between two trajectories, loop closure searches will be performed + globally rather than in a smaller search window. + + +cartographer.mapping.proto.TrajectoryBuilderOptions +=================================================== + +cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options + Not yet documented. + +cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options + Not yet documented. + +bool pure_localization + Not yet documented. + + +cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions +=========================================================== + +float min_range + Rangefinder points outside these ranges will be dropped. + +float max_range + Not yet documented. + +float min_z + Not yet documented. + +float max_z + Not yet documented. + +float missing_data_ray_length + Points beyond 'max_range' will be inserted with this length as empty space. + +int32 num_accumulated_range_data + Number of range data to accumulate into one unwarped, combined range data + to use for scan matching. + +float voxel_filter_size + Voxel filter that gets applied to the range data immediately after + cropping. + +cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options + Voxel filter used to compute a sparser point cloud for matching. + +cartographer.sensor.proto.AdaptiveVoxelFilterOptions loop_closure_adaptive_voxel_filter_options + Voxel filter used to compute a sparser point cloud for finding loop + closures. + +bool use_online_correlative_scan_matching + Whether to solve the online scan matching first using the correlative scan + matcher to generate a good starting point for Ceres. + +cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options + Not yet documented. + +cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options + Not yet documented. + +cartographer.mapping.proto.MotionFilterOptions motion_filter_options + Not yet documented. + +double imu_gravity_time_constant + Time constant in seconds for the orientation moving average based on + observed gravity via the IMU. It should be chosen so that the error + 1. from acceleration measurements not due to gravity (which gets worse when + the constant is reduced) and + 2. from integration of angular velocities (which gets worse when the + constant is increased) is balanced. + +cartographer.mapping_2d.proto.SubmapsOptions submaps_options + Not yet documented. + +bool use_imu_data + True if IMU data should be expected and used. + + +cartographer.mapping_2d.proto.RangeDataInserterOptions +====================================================== + +double hit_probability + Probability change for a hit (this will be converted to odds and therefore + must be greater than 0.5). + +double miss_probability + Probability change for a miss (this will be converted to odds and therefore + must be less than 0.5). + +bool insert_free_space + If 'false', free space will not change the probabilities in the occupancy + grid. + + +cartographer.mapping_2d.proto.SubmapsOptions +============================================ + +double resolution + Resolution of the map in meters. + +int32 num_range_data + Number of range data before adding a new submap. Each submap will get twice + the number of range data inserted: First for initialization without being + matched against, then while being matched. + +cartographer.mapping_2d.proto.RangeDataInserterOptions range_data_inserter_options + Not yet documented. + + +cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions +=================================================================== + +double occupied_space_weight + Scaling parameters for each cost functor. + +double translation_weight + Not yet documented. + +double rotation_weight + Not yet documented. + +cartographer.common.proto.CeresSolverOptions ceres_solver_options + Configure the Ceres solver. See the Ceres documentation for more + information: https://code.google.com/p/ceres-solver/ + + +cartographer.mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions +============================================================================= + +double linear_search_window + Minimum linear search window in which the best possible scan alignment + will be found. + +double angular_search_window + Minimum angular search window in which the best possible scan alignment + will be found. + +int32 branch_and_bound_depth + Number of precomputed grids to use. + + +cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions +================================================================================= + +double linear_search_window + Minimum linear search window in which the best possible scan alignment + will be found. + +double angular_search_window + Minimum angular search window in which the best possible scan alignment + will be found. + +double translation_delta_cost_weight + Weights applied to each part of the score. + +double rotation_delta_cost_weight + Not yet documented. + + +cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions +=========================================================== + +float min_range + Rangefinder points outside these ranges will be dropped. + +float max_range + Not yet documented. + +int32 num_accumulated_range_data + Number of range data to accumulate into one unwarped, combined range data + to use for scan matching. + +float voxel_filter_size + Voxel filter that gets applied to the range data immediately after + cropping. + +cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options + Voxel filter used to compute a sparser point cloud for matching. + +cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options + Not yet documented. + +bool use_online_correlative_scan_matching + Whether to solve the online scan matching first using the correlative scan + matcher to generate a good starting point for Ceres. + +cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options + Not yet documented. + +cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options + Not yet documented. + +cartographer.mapping.proto.MotionFilterOptions motion_filter_options + Not yet documented. + +double imu_gravity_time_constant + Time constant in seconds for the orientation moving average based on + observed gravity via the IMU. It should be chosen so that the error + 1. from acceleration measurements not due to gravity (which gets worse when + the constant is reduced) and + 2. from integration of angular velocities (which gets worse when the + constant is increased) is balanced. + +int32 rotational_histogram_size + Number of histogram buckets for the rotational scan matcher. + +cartographer.mapping_3d.proto.SubmapsOptions submaps_options + Not yet documented. + + +cartographer.mapping_3d.proto.RangeDataInserterOptions +====================================================== + +double hit_probability + Probability change for a hit (this will be converted to odds and therefore + must be greater than 0.5). + +double miss_probability + Probability change for a miss (this will be converted to odds and therefore + must be less than 0.5). + +int32 num_free_space_voxels + Up to how many free space voxels are updated for scan matching. + 0 disables free space. + + +cartographer.mapping_3d.proto.SubmapsOptions +============================================ + +double high_resolution + Resolution of the 'high_resolution' map in meters used for local SLAM and + loop closure. + +double high_resolution_max_range + Maximum range to filter the point cloud to before insertion into the + 'high_resolution' map. + +double low_resolution + Resolution of the 'low_resolution' version of the map in meters used for + local SLAM only. + +int32 num_range_data + Number of range data before adding a new submap. Each submap will get twice + the number of range data inserted: First for initialization without being + matched against, then while being matched. + +cartographer.mapping_3d.proto.RangeDataInserterOptions range_data_inserter_options + Not yet documented. + + +cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions +=================================================================== + +double occupied_space_weight + Scaling parameters for each cost functor. + +double translation_weight + Not yet documented. + +double rotation_weight + Not yet documented. + +bool only_optimize_yaw + Whether only to allow changes to yaw, keeping roll/pitch constant. + +cartographer.common.proto.CeresSolverOptions ceres_solver_options + Configure the Ceres solver. See the Ceres documentation for more + information: https://code.google.com/p/ceres-solver/ + + +cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions +============================================================================= + +int32 branch_and_bound_depth + Number of precomputed grids to use. + +int32 full_resolution_depth + Number of full resolution grids to use, additional grids will reduce the + resolution by half each. + +double min_rotational_score + Minimum score for the rotational scan matcher. + +double min_low_resolution_score + Threshold for the score of the low resolution grid below which a match is + not considered. Only used for 3D. + +double linear_xy_search_window + Linear search window in the plane orthogonal to gravity in which the best + possible scan alignment will be found. + +double linear_z_search_window + Linear search window in the gravity direction in which the best possible + scan alignment will be found. + +double angular_search_window + Minimum angular search window in which the best possible scan alignment + will be found. + + +cartographer.sensor.proto.AdaptiveVoxelFilterOptions +==================================================== + +float max_length + 'max_length' of a voxel edge. + +float min_num_points + If there are more points and not at least 'min_num_points' remain, the + voxel length is reduced trying to get this minimum number of points. + +float max_range + Points further away from the origin are removed. + + diff --git a/carto_ws/src/cartographer-master/docs/source/cost_functions.rst b/carto_ws/src/cartographer-master/docs/source/cost_functions.rst new file mode 100644 index 0000000..58ad253 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/cost_functions.rst @@ -0,0 +1,130 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +============== +Cost functions +============== + +Relative Transform Error 2D +=========================== + +Given two poses +:math:`\mathbf{p}_i = [\mathbf{x}_i; \theta_i] = [x_i, y_i, \theta_i]^T` +and :math:`\mathbf{p}_j = [\mathbf{x}_j; \theta_j] = [x_j, y_j, \theta_j]^T` +the transformation :math:`\mathbf T` from the coordinate frame :math:`j` to the +coordinate frame :math:`i` has the following form + +.. math:: + \mathbf{T}( \mathbf{p}_i,\mathbf{p}_j) = + \left[ + \begin{array}{c} + R(\theta_i)^T (\mathbf x_j - \mathbf x_i) \\ + \theta_j-\theta_i + \end{array} + \right] + +where :math:`R(\theta_i)^T` is the rotation matrix of :math:`\theta_i`. + +The weighted error :math:`f:\mathbb R^6 \mapsto \mathbb R^3` between +:math:`\mathbf T` and the measured transformation :math:`\mathbf T_{ij}^m = +[\mathbf x_{ij}^m; \theta_j^m]` from the coordinate frame :math:`j` to the +coordinate frame :math:`i` can be computed as + +.. math:: + \mathbf f_{\text{relative}}( \mathbf{p}_i,\mathbf{p}_j) = + \left[ + w_{\text{t}} \; w_{\text{r}} + \right] + \left( + \mathbf T_{ij}^m - \mathbf T( \mathbf{p}_i,\mathbf{p}_j) + \right) = + \left[ + \begin{array}{c} + w_{\text{t}}\left( + \mathbf x_{ij}^m - R(\theta_i)^T (\mathbf x_j - \mathbf x_i) + \right) \\ + w_{\text{r}}\left( + \mathrm{clamp}(\theta_{ij}^m - (\theta_j-\theta_i)) + \right) + \end{array} + \right] + +where :math:`w_t` and :math:`w_r` are weights for translation and rotation +respectively and :math:`\mathrm{clamp}: \mathbb R \mapsto [-\pi, \pi]` +normalizes the angle difference. + +Jacobian matrix :math:`J_f` is given by: + +.. math:: + \begin{align} + J_f( \mathbf{p}_i,\mathbf{p}_j) &= + \left[ + \frac{\partial\mathbf f}{\partial x_i} \quad + \frac{\partial\mathbf f}{\partial y_i} \quad + \frac{\partial\mathbf f}{\partial \theta_i} \quad + \frac{\partial\mathbf f}{\partial x_j} \quad + \frac{\partial\mathbf f}{\partial y_j} \quad + \frac{\partial\mathbf f}{\partial \theta_j} + \right] \\ + &\mathstrut \\ + &= + \left[ + \begin{array}{cccc} + w_{\text{t}} R^T(\theta_i) + & -w_{\text{t}} {\frac{\mathrm d R^T(\theta_i)}{\mathrm d \theta}}(\mathbf x_j - \mathbf x_i) + & -w_{\text{t}} R^T(\theta_i) + & \mathbf{0} \\ + \mathbf{0}^T + & w_{\text{r}} + & \mathbf{0}^T + & -w_{\text{r}} + \end{array} + \right] + \end{align} + +Landmark Cost Function +====================== + +Let :math:`\mathbf{p}_o` denote the global pose of the SLAM tracking frame at +which a landmark with the global pose :math:`\mathbf{p}_l` is observed. +The landmark observation itself is the measured transformation +:math:`\mathbf{T}^m_{ol}` that was observed at time :math:`t_o`. + +As the landmark can be observed asynchronously, the pose of observation +:math:`\mathbf{p}_o` is modeled in between two regular, consecutive trajectory +nodes :math:`\mathbf{p}_i, \mathbf{p}_j`. +It is interpolated between :math:`\mathbf{p}_i` and +:math:`\mathbf{p}_j` at the observation time :math:`t_o` using a linear +interpolation for the translation and a quaternion SLERP for the rotation: + +.. math:: + \mathbf{p}_o = \text{interpolate}(\mathbf{p}_i, \mathbf{p}_j, t_o) + +Then, the full weighted landmark cost function can be written as: + +.. math:: + \begin{align} + \mathbf f_{\text{landmark}}(\mathbf{p}_l, \mathbf{p}_i, \mathbf{p}_j) &= + \mathbf f_{\text{relative}}(\mathbf{p}_l, \mathbf{p}_o) \\ + &= + \left[ + w_{\text{t}} \; w_{\text{r}} + \right] + \left( + \mathbf T_{ol}^m - \mathbf T( \mathbf{p}_o,\mathbf{p}_l) + \right) + \end{align} + +The translation and rotation weights :math:`w_{\text{t}}, w_{\text{r}}` are +part of the landmark observation data that is fed into Cartographer. diff --git a/carto_ws/src/cartographer-master/docs/source/evaluation.rst b/carto_ws/src/cartographer-master/docs/source/evaluation.rst new file mode 100644 index 0000000..6cad217 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/evaluation.rst @@ -0,0 +1,109 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +========== +Evaluation +========== + +Performing evaluation is a crucial part of developing a SLAM system. +For this purpose, Cartographer offers built-in tools that can aid the tuning process or can be used for quality assurance purposes. + +These tools can be used to assess the SLAM result even when no dedicated ground truth is available. +This is in contrast to public SLAM benchmarks like e.g the KITTI dataset [1]_ or the TUM RGB-D dataset [2]_, where highly-precise ground truth states (GPS-RTK, motion capture) are available as a reference. + + +Concept +======= + +The process comprises two steps: + +1. auto-generation of "ground truth" relations +2. evaluation of the test data against the generated ground truth + +The evaluation is based on the pose relations metric proposed in [3]_. +Rather than comparing the pose of a trajectory node directly to the corresponding ground truth pose, it compares the relative poses between two trajectory nodes in the probe data to the corresponding relation of two trajectory nodes in the ground truth trajectory. + +In Cartographer, we can generate such ground truth relations from trajectories with loop closures. +Let an optimized trajectory with loop closures be the input for the ground truth generation. +We select the relations from loop closure constraints that satisfy the following criteria: + +* ``min_covered_distance``: Minimum covered distance in meters before a loop closure is considered a candidate for autogenerated ground truth. +* ``outlier_threshold_meters``: Distance in meters beyond which constraints are considered outliers. +* ``outlier_threshold_radians``: Distance in radians beyond which constraints are considered outliers. + +We can assume the pose relations of neighboring trajectory nodes fulfilling these requirements to be locally correct in a fully optimized trajectory. +Although this is not a ground truth in the sense of an independent input from another source, we can now use it to evaluate the quality of local SLAM results that were generated without loop closure optimization. + +The following figure illustrates the concept. +On the left side, the ground truth relations are visualized as green connections between trajectory nodes of a fully optimized trajectory. +On the right side, the corresponding relations in a non-optimized trajectory are shown in red. + +The actual metric that is computed is the difference between the ground truth (green) and the probe (red) relations. + +.. editable version: https://drive.google.com/file/d/1riJCj8KK4tQZArssGRtoXt7aBr1SWEXM/view?usp=sharing + +.. image:: autogenerate_groundtruth.png + + +Advantages & Limitations +======================== + +The first obvious advantage is the easier data collection process compared to a cumbersome ground truth setup. +Another great advantage of this methodology is that the SLAM system can be evaluated in any custom sensor configuration (compared to public benchmarks where we are restricted to the data and the sensor configuration of the authors). + +However, this type of self-evaluation is not suitable for measuring the accuracy of the full SLAM system with all optimizations enabled - only an evaluation with *real* ground truth states can provide that. +Furthermore, trajectory nodes outside of loop closure areas can't be considered. + + +How-To +====== + +Given a serialized state of a fully optimized trajectory (here: ``optimized.pbstream`` file), the ground truth relations can be generated with the following command: + +.. code-block:: bash + + cd # (directory where Cartographer's binaries are located) + ./cartographer_autogenerate_ground_truth -pose_graph_filename optimized.pbstream -output_filename relations.pbstream -min_covered_distance 100 -outlier_threshold_meters 0.15 -outlier_threshold_radians 0.02 + +Then, a non-optimized trajectory ``test.pbstream`` can be evaluated against the generated relations with: + +.. code-block:: bash + + ./cartographer_compute_relations_metrics -relations_filename relations.pbstream -pose_graph_filename test.pbstream + +This will produce output in this form: + +.. code-block:: none + + Abs translational error 0.01944 +/- 0.01819 m + Sqr translational error 0.00071 +/- 0.00189 m^2 + Abs rotational error 0.11197 +/- 0.12432 deg + Sqr rotational error 0.02799 +/- 0.07604 deg^2 + + +---- + + +References +========== + +.. [1] Andreas Geiger, Philip Lenz and Raquel Urtasun. + *Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite*. + CVPR, 2012. +.. [2] Jürgen Sturm, Nikolas Engelhard, Felix Endres, Wolfram Burgard and Daniel Cremers. + *A Benchmark for the Evaluation of RGB-D SLAM Systems*. + IROS, 2012. +.. [3] Rainer Kümmerle, Bastian Steder, Christian Dornhege, Michael Ruhnke, Giorgio Grisetti, Cyrill Stachniss and Alexander Kleiner. + *On measuring the accuracy of SLAM algorithms.* + Autonomous Robots 27(4), pp.387-407, 2009. diff --git a/carto_ws/src/cartographer-master/docs/source/high_level_system_overview.png b/carto_ws/src/cartographer-master/docs/source/high_level_system_overview.png new file mode 100644 index 0000000..6c87737 Binary files /dev/null and b/carto_ws/src/cartographer-master/docs/source/high_level_system_overview.png differ diff --git a/carto_ws/src/cartographer-master/docs/source/index.rst b/carto_ws/src/cartographer-master/docs/source/index.rst new file mode 100644 index 0000000..4871233 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/index.rst @@ -0,0 +1,121 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +============ +Cartographer +============ + +.. toctree:: + :maxdepth: 2 + :hidden: + + configuration + evaluation + terminology + cost_functions + pbstream_migration + +`Cartographer`_ is a system that provides real-time simultaneous localization +and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor +configurations. + +.. _Cartographer: https://github.com/cartographer-project/cartographer +.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping + +Technical Overview +================== +* High level system overview of Cartographer + +.. image:: high_level_system_overview.png + :target: https://github.com/cartographer-project/cartographer/blob/master/docs/source/high_level_system_overview.png + +.. To make modifications, edit the original Google Sketch and export a png. +.. https://docs.google.com/drawings/d/1kCJ_dEbSvV83THCUfMikCPw7xFrTkrvRw5r6Ji8C90c/edit?usp=sharing + +Getting started +=============== + +Cartographer is a standalone C++ library. To get started quickly, use our `ROS +`_ integration. + +Getting started with ROS +------------------------ + +ROS integration is provided by the `Cartographer ROS repository`_. You will find +complete documentation for using Cartographer with ROS at the +`Cartographer ROS Read the Docs site`_. + +.. _Cartographer ROS repository: https://github.com/cartographer-project/cartographer_ros +.. _Cartographer ROS Read the Docs site: https://google-cartographer-ros.readthedocs.io + +Getting started without ROS +--------------------------- + +Please see our ROS integration as a starting point for integrating your system +with the standalone library. Currently, it is the best available reference. + +On Ubuntu 18.04 (Bionic): + +.. literalinclude:: ../../scripts/install_debs_cmake.sh + :language: bash + :lines: 20- + +.. literalinclude:: ../../scripts/install_abseil.sh + :language: bash + :lines: 20- + +.. literalinclude:: ../../scripts/install_ceres.sh + :language: bash + :lines: 20- + +.. literalinclude:: ../../scripts/install_proto3.sh + :language: bash + :lines: 20- + +.. literalinclude:: ../../scripts/install_cartographer_cmake.sh + :language: bash + :lines: 20- + +.. _system-requirements: + +System Requirements +=================== + +Although Cartographer may run on other systems, it is confirmed to be working +on systems that meet the following requirements: + +* 64-bit, modern CPU (e.g. 3rd generation i7) +* 16 GB RAM +* Ubuntu 18.04 (Bionic), 20.04 (Focal) +* gcc version 6.3.0, 7.5.0, 9.3.0 + +Known Issues +------------ + +* 32-bit builds have libeigen alignment problems which cause crashes and/or + memory corruptions. + +How to cite us +============== + +Background about the algorithms developed for Cartographer can be found in the +following publication. If you use Cartographer for your research, we would +appreciate it if you cite our paper. + +W. Hess, D. Kohler, H. Rapp, and D. Andor, +`Real-Time Loop Closure in 2D LIDAR SLAM`_, in +*Robotics and Automation (ICRA), 2016 IEEE International Conference on*. +IEEE, 2016. pp. 1271–1278. + +.. _Real-Time Loop Closure in 2D LIDAR SLAM: https://research.google.com/pubs/pub45466.html diff --git a/carto_ws/src/cartographer-master/docs/source/pbstream_migration.rst b/carto_ws/src/cartographer-master/docs/source/pbstream_migration.rst new file mode 100644 index 0000000..20ce543 --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/pbstream_migration.rst @@ -0,0 +1,45 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +================================= +Migration tool for pbstream files +================================= + +The pbstream serialization format for 3D has changed to include additional +data (histograms) in each submap. Code to load old data by migrating +on-the-fly will be removed soon. Once this happened, users who wish to +migrate old pbstream files can use a migration tool. + +The tool is shipped as part of Cartographer's pbstream tool (`source`_) and once +built can be invoked as follows::: + + cartographer_pbstream migrate old.pbstream new.pbstream + +The tool assumes 3D data in the old submap format as input and converts it +to the currently used format version. + +Migrating pre-1.0 pbstream files +================================ + +With the update of the pbstream serialization format as discussed in +`RFC-0021`_, previously serialized pbstream files are not loadable in +Cartographer 1.0 anymore. + +In order to enable users to reuse previously generated pbstream files, +migration using an older version of the migration tool is necessary. +The current tool does not support this migration anymore. Please use +the version at Git SHA 6c889490e245cc5d9da15023249c6fc7119def3f. + +.. _RFC-0021: https://github.com/cartographer-project/rfcs/blob/master/text/0021-serialization-format.md +.. _source: https://github.com/cartographer-project/cartographer/blob/master/cartographer/io/pbstream_main.cc diff --git a/carto_ws/src/cartographer-master/docs/source/terminology.rst b/carto_ws/src/cartographer-master/docs/source/terminology.rst new file mode 100644 index 0000000..575266f --- /dev/null +++ b/carto_ws/src/cartographer-master/docs/source/terminology.rst @@ -0,0 +1,71 @@ +.. Copyright 2017 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +=========== +Terminology +=========== + +This documents a few common patterns that exist in the Cartographer codebase. + +Frames +====== + +global map frame + This is the frame in which global SLAM results are expressed. It is the fixed + map frame including all loop closure and optimization results. The transform + between this frame and any other frame can jump when new optimization results + are available. Its z-axis points upwards, i.e. the gravitational acceleration + vector points in the -z direction, i.e. the gravitational component measured + by an accelerometer is in the +z direction. + +local map frame + This is the frame in which local SLAM results are expressed. It is the fixed + map frame excluding loop closures and the pose graph optimization. For a given + point in time, the transform between this and the global map frame may change, + but the transform between this and all other frames does not change. + +submap frame + Each submap has a separate fixed frame. + +tracking frame + The frame in which sensor data is expressed. It is not fixed, i.e. it changes + over time. It is also different for different trajectories. + +gravity-aligned frame + Only used in 2D. A frame colocated with the tracking frame but with a + different orientation that is approximately aligned with gravity, i.e. the + gravitational acceleration vector points approximately in the -z direction. No + assumption about yaw (rotation around the z axis between this and the tracking + frame) should be made. A different gravity-aligned frame is used for different + trajectory nodes, e.g. yaw can change arbitrarily between gravity-aligned + frames of consecutive nodes. + + + +Transforms +========== + +local_pose + Transforms data from the tracking frame (or a submap frame, depending on + context) to the local map frame. + +global_pose + Transforms data from the tracking frame (or a submap frame, depending on + context) to the global map frame. + +local_submap_pose + Transforms data from a submap frame to the local map frame. + +global_submap_pose + Transforms data from a submap frame to the global map frame. diff --git a/carto_ws/src/cartographer-master/package.xml b/carto_ws/src/cartographer-master/package.xml new file mode 100644 index 0000000..8c9f120 --- /dev/null +++ b/carto_ws/src/cartographer-master/package.xml @@ -0,0 +1,56 @@ + + + + + cartographer + 2.0.0 + + Cartographer is a system that provides real-time simultaneous localization + and mapping (SLAM) in 2D and 3D across multiple platforms and sensor + configurations. + + + The Cartographer Authors + + Apache 2.0 + + https://github.com/cartographer-project/cartographer + + + The Cartographer Authors + + + cmake + + git + google-mock + gtest + python3-sphinx + + libboost-iostreams-dev + eigen + libcairo2-dev + libceres-dev + libgflags-dev + libgoogle-glog-dev + lua5.2-dev + protobuf-dev + + + cmake + + diff --git a/carto_ws/src/cartographer-master/scripts/build_test_asan.sh b/carto_ws/src/cartographer-master/scripts/build_test_asan.sh new file mode 100644 index 0000000..7bf6592 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/build_test_asan.sh @@ -0,0 +1,38 @@ +#!/bin/sh + +# Copyright 2017 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Installing the following packages may be required: +# sudo apt-get install clang-3.8 llvm-3.8 + +cd cartographer +mkdir -p build-asan +cd build-asan +cmake .. -G Ninja \ + -DCMAKE_BUILD_TYPE=Debug \ + -DFORCE_DEBUG_BUILD=True \ + -DCMAKE_CXXFLAGS="-fno-omit-frame-pointer -fsanitize=address \ + -fsanitize-address-use-after-scope -O1" \ + -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=address" \ + -DCMAKE_C_COMPILER=clang-3.8 \ + -DCMAKE_CXX_COMPILER=clang++-3.8 +ninja +PATH="/usr/lib/llvm-3.8/bin/:$PATH" \ + ASAN_OPTIONS="detect_leaks=1 detect_stack_use_after_return=true" \ + ctest --output-on-failure -j20 --timeout 60 \ + --test-arguments="--gtest_color=yes" diff --git a/carto_ws/src/cartographer-master/scripts/ctest_to_junit.py b/carto_ws/src/cartographer-master/scripts/ctest_to_junit.py new file mode 100644 index 0000000..a009912 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/ctest_to_junit.py @@ -0,0 +1,33 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from lxml import etree +import StringIO +import sys + +TAGfile = open(sys.argv[1]+"/Testing/TAG", 'r') +dirname = TAGfile.readline().strip() + +xmlfile = open(sys.argv[1]+"/Testing/"+dirname+"/Test.xml", 'r') +xslfile = open(sys.path[0] + "/ctest_to_junit.xsl", 'r') + +xmlcontent = xmlfile.read() +xslcontent = xslfile.read() + +xmldoc = etree.parse(StringIO.StringIO(xmlcontent)) +xslt_root = etree.XML(xslcontent) +transform = etree.XSLT(xslt_root) + +result_tree = transform(xmldoc) +result_tree.write(sys.argv[1]+"/Testing/"+dirname+"/jUnit.xml") \ No newline at end of file diff --git a/carto_ws/src/cartographer-master/scripts/ctest_to_junit.xsl b/carto_ws/src/cartographer-master/scripts/ctest_to_junit.xsl new file mode 100644 index 0000000..194823b --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/ctest_to_junit.xsl @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer-master/scripts/install_abseil.sh b/carto_ws/src/cartographer-master/scripts/install_abseil.sh new file mode 100644 index 0000000..4962d6e --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_abseil.sh @@ -0,0 +1,33 @@ +#!/bin/sh + +# Copyright 2019 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +git clone https://github.com/abseil/abseil-cpp.git +cd abseil-cpp +git checkout d902eb869bcfacc1bad14933ed9af4bed006d481 +mkdir build +cd build +cmake -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=/usr/local/stow/absl \ + .. +ninja +sudo ninja install +cd /usr/local/stow +sudo stow absl diff --git a/carto_ws/src/cartographer-master/scripts/install_async_grpc.sh b/carto_ws/src/cartographer-master/scripts/install_async_grpc.sh new file mode 100644 index 0000000..73c8bbd --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_async_grpc.sh @@ -0,0 +1,29 @@ +#!/bin/sh + +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +git clone https://github.com/cartographer-project/async_grpc +cd async_grpc +git checkout 771af45374af7f7bfc3b622ed7efbe29a4aba403 +mkdir build +cd build +cmake -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + .. +ninja +sudo ninja install diff --git a/carto_ws/src/cartographer-master/scripts/install_cartographer_bazel.sh b/carto_ws/src/cartographer-master/scripts/install_cartographer_bazel.sh new file mode 100644 index 0000000..e3889f0 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_cartographer_bazel.sh @@ -0,0 +1,23 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Build and install Cartographer. +cd cartographer +bazel build //... +bazel test --jobs 1 //... diff --git a/carto_ws/src/cartographer-master/scripts/install_cartographer_cmake.sh b/carto_ws/src/cartographer-master/scripts/install_cartographer_cmake.sh new file mode 100644 index 0000000..49c783c --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_cartographer_cmake.sh @@ -0,0 +1,27 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Build and install Cartographer. +cd cartographer +mkdir build +cd build +cmake .. -G Ninja +ninja +CTEST_OUTPUT_ON_FAILURE=1 ninja test +sudo ninja install diff --git a/carto_ws/src/cartographer-master/scripts/install_cartographer_cmake_with_grpc.sh b/carto_ws/src/cartographer-master/scripts/install_cartographer_cmake_with_grpc.sh new file mode 100644 index 0000000..c73af23 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_cartographer_cmake_with_grpc.sh @@ -0,0 +1,27 @@ +#!/bin/sh + +# Copyright 2017 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Build and install Cartographer. +cd cartographer +mkdir build +cd build +cmake .. -DBUILD_GRPC=ON -DBUILD_PROMETHEUS=ON -G Ninja +ninja +CTEST_OUTPUT_ON_FAILURE=1 ninja test +sudo ninja install diff --git a/carto_ws/src/cartographer-master/scripts/install_ceres.sh b/carto_ws/src/cartographer-master/scripts/install_ceres.sh new file mode 100644 index 0000000..56999d0 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_ceres.sh @@ -0,0 +1,31 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +VERSION="1.13.0" + +# Build and install Ceres. +git clone https://ceres-solver.googlesource.com/ceres-solver +cd ceres-solver +git checkout tags/${VERSION} +mkdir build +cd build +cmake .. -G Ninja -DCXX11=ON +ninja +CTEST_OUTPUT_ON_FAILURE=1 ninja test +sudo ninja install diff --git a/carto_ws/src/cartographer-master/scripts/install_debs_bazel.sh b/carto_ws/src/cartographer-master/scripts/install_debs_bazel.sh new file mode 100644 index 0000000..f3b6883 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_debs_bazel.sh @@ -0,0 +1,33 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Install the required libraries that are available as debs. +sudo apt-get update +sudo apt-get install -y \ + wget \ + pkg-config \ + zip \ + g++ \ + zlib1g-dev \ + unzip \ + python +wget https://github.com/bazelbuild/bazel/releases/download/0.9.0/bazel-0.9.0-installer-linux-x86_64.sh +chmod +x bazel-0.9.0-installer-linux-x86_64.sh +./bazel-0.9.0-installer-linux-x86_64.sh +export PATH="$PATH:$HOME/bin" diff --git a/carto_ws/src/cartographer-master/scripts/install_debs_cmake.sh b/carto_ws/src/cartographer-master/scripts/install_debs_cmake.sh new file mode 100644 index 0000000..7eda82a --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_debs_cmake.sh @@ -0,0 +1,53 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Install the required libraries that are available as debs. +sudo apt-get update +sudo apt-get install -y \ + clang \ + cmake \ + g++ \ + git \ + google-mock \ + libboost-all-dev \ + libcairo2-dev \ + libcurl4-openssl-dev \ + libeigen3-dev \ + libgflags-dev \ + libgoogle-glog-dev \ + liblua5.2-dev \ + libsuitesparse-dev \ + lsb-release \ + ninja-build \ + stow + +# Install Ceres Solver and Protocol Buffers support if available. +# No need to build it ourselves. +if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]] +then + sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler +else + sudo apt-get install -y python-sphinx + if [[ "$(lsb_release -sc)" = "bionic" ]] + then + sudo apt-get install -y libceres-dev + fi +fi + + diff --git a/carto_ws/src/cartographer-master/scripts/install_grpc.sh b/carto_ws/src/cartographer-master/scripts/install_grpc.sh new file mode 100644 index 0000000..9bc081a --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_grpc.sh @@ -0,0 +1,28 @@ +#!/bin/sh + +# Copyright 2017 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +VERSION="v1.10.0" +# Digest: 474c5950686e3962bd339c93d27e369bf64f568f + +# Build and install gRPC. +git clone --branch ${VERSION} --depth 1 https://github.com/grpc/grpc +cd grpc +git submodule update --init +make +sudo make install diff --git a/carto_ws/src/cartographer-master/scripts/install_prometheus_cpp.sh b/carto_ws/src/cartographer-master/scripts/install_prometheus_cpp.sh new file mode 100644 index 0000000..3fe88c1 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_prometheus_cpp.sh @@ -0,0 +1,30 @@ +#!/bin/sh + +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +COMMIT="4e0814ee3f93b796356a51a4795a332568940a72" + +git clone https://github.com/jupp0r/prometheus-cpp.git +cd prometheus-cpp +git checkout ${COMMIT} +git submodule update --init +mkdir build +cd build +cmake -DCMAKE_BUILD_TYPE=Release .. +make +sudo make install diff --git a/carto_ws/src/cartographer-master/scripts/install_proto3.sh b/carto_ws/src/cartographer-master/scripts/install_proto3.sh new file mode 100644 index 0000000..ef82711 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/install_proto3.sh @@ -0,0 +1,34 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +VERSION="v3.4.1" + +# Build and install proto3. +git clone https://github.com/google/protobuf.git +cd protobuf +git checkout tags/${VERSION} +mkdir build +cd build +cmake -G Ninja \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_BUILD_TYPE=Release \ + -Dprotobuf_BUILD_TESTS=OFF \ + ../cmake +ninja +sudo ninja install diff --git a/carto_ws/src/cartographer-master/scripts/load_docker_cache.sh b/carto_ws/src/cartographer-master/scripts/load_docker_cache.sh new file mode 100644 index 0000000..4c37247 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/load_docker_cache.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cache intermediate Docker layers. For a description of how this works, see: +# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html + +set -o errexit +set -o verbose +set -o pipefail + +if [ -f ${DOCKER_CACHE_FILE} ]; then + gunzip -c ${DOCKER_CACHE_FILE} | docker load; +fi diff --git a/carto_ws/src/cartographer-master/scripts/remove_mingw_cygwin_from_path.bat b/carto_ws/src/cartographer-master/scripts/remove_mingw_cygwin_from_path.bat new file mode 100644 index 0000000..d6663c2 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/remove_mingw_cygwin_from_path.bat @@ -0,0 +1,20 @@ +rem Copyright 2018 The Cartographer Authors +rem +rem Licensed under the Apache License, Version 2.0 (the "License"); +rem you may not use this file except in compliance with the License. +rem You may obtain a copy of the License at +rem +rem http://www.apache.org/licenses/LICENSE-2.0 +rem +rem Unless required by applicable law or agreed to in writing, software +rem distributed under the License is distributed on an "AS IS" BASIS, +rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +rem See the License for the specific language governing permissions and +rem limitations under the License. + +rem Remove git bash's MinGW/Cygwin stuff from PATH, as it interferes with CMake's +rem detection of Boost when using ninja. +set PATH=%PATH:C:\tools\mingw64\bin;=% +set PATH=%PATH:C:\Program Files\Git\bin;=% +set PATH=%PATH:C:\Program Files\Git\usr\bin;=% +set PATH=%PATH:C:\Program Files\Git\mingw64\bin;=% \ No newline at end of file diff --git a/carto_ws/src/cartographer-master/scripts/save_docker_cache.sh b/carto_ws/src/cartographer-master/scripts/save_docker_cache.sh new file mode 100644 index 0000000..7b2000e --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/save_docker_cache.sh @@ -0,0 +1,30 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cache intermediate Docker layers. For a description of how this works, see: +# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html + +set -o errexit +set -o verbose +set -o pipefail + +if [[ ${TRAVIS_BRANCH} == "master" ]] && + [[ ${TRAVIS_PULL_REQUEST} == "false" ]]; then + mkdir -p $(dirname ${DOCKER_CACHE_FILE}) + IMAGE_NAMES=$(docker history -q cartographer:${LSB_RELEASE} | grep -v '') + docker save ${IMAGE_NAMES} | gzip > ${DOCKER_CACHE_FILE}.new + mv ${DOCKER_CACHE_FILE}.new ${DOCKER_CACHE_FILE} +fi diff --git a/carto_ws/src/cartographer-master/scripts/update_configuration_doc.py b/carto_ws/src/cartographer-master/scripts/update_configuration_doc.py new file mode 100644 index 0000000..8468594 --- /dev/null +++ b/carto_ws/src/cartographer-master/scripts/update_configuration_doc.py @@ -0,0 +1,216 @@ +#!/usr/bin/python3 +# -*- coding: utf-8 -*- + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""A dumb configuration.rst generator that relies on source comments.""" + +import io +import os + +TARGET = 'docs/source/configuration.rst' +ROOT = 'cartographer' +PREFIX = """.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +============= +Configuration +============= + +.. DO NOT EDIT! This documentation is AUTOGENERATED, please edit .proto files as +.. needed and run scripts/update_configuration_doc.py. + +""" +SUFFIX = """ +""" +NODOC = 'Not yet documented.' + + +class Message(object): + def __init__(self, name, package, preceding_comments): + self.name = name + self.package = package + self.preceding_comments = preceding_comments + self.trailing_comments = None + self.options = [] + + def AddTrailingComments(self, comments): + self.trailing_comments = comments + + def AddOption(self, option_type, name, comments): + self.options.append((option_type, name, comments)) + + +def ParseProtoFile(proto_file): + """Computes the list of Message objects of the option messages in a file.""" + line_iter = iter(proto_file) + + # We ignore the license header and search for the 'package' line. + for line in line_iter: + line = line.strip() + if line.startswith('package'): + assert line[-1] == ';' + package = line[7:-1].strip() + break + else: + assert '}' not in line + + message_list = [] + while True: + # Search for the next options message and capture preceding comments. + message_comments = [] + for line in line_iter: + line = line.strip() + if '}' in line: + # The preceding comments were for a different message it seems. + message_comments = [] + elif line.startswith('//'): + # We keep comments preceding an options message. + comment = line[2:].strip() + if not comment.startswith('NEXT ID:'): + message_comments.append(comment) + elif line.startswith('message') and line.endswith('Options {'): + message_name = package + '.' + line[7:-1].strip() + break + else: + # We reached the end of file. + break + print(" Found '%s'." % message_name) + message = Message(message_name, package, message_comments) + message_list.append(message) + + # We capture the contents of this message. + option_comments = [] + multiline = '' + for line in line_iter: + line = line.strip() + if '}' in line: + # We reached the end of this message. + message.AddTrailingComments(option_comments) + break + elif line.startswith('//'): + comment = line[2:].strip() + if not comment.startswith('NEXT ID:'): + option_comments.append(comment) + else: + assert not line.startswith('required') + multiline += ' ' + line + if not multiline.endswith(';'): + continue + assert len(multiline) < 200 + option = multiline[:-1].strip().rstrip('0123456789').strip() + assert option.endswith('=') + if option.startswith('repeated'): + option = option[8:] + option_type, option_name = option[:-1].strip().split(); + print(" Option '%s'." % option_name) + multiline = '' + message.AddOption(option_type, option_name, option_comments) + option_comments = [] + + return message_list + + +def ParseProtoFilesRecursively(root): + """Recursively parses all proto files into a list of Message objects.""" + message_list = [] + for dirpath, dirnames, filenames in os.walk(root): + for name in filenames: + if name.endswith('.proto'): + path = os.path.join(dirpath, name) + print("Found '%s'..." % path) + assert not os.path.islink(path) + message_list.extend(ParseProtoFile(io.open(path, encoding='UTF-8'))) + return message_list + + +class ResolutionError(Exception): + """Raised when resolving a message name fails.""" + + +class Resolver(object): + def __init__(self, name_set): + self.name_set = set(iter(name_set)) + + def Resolve(self, message_name, package_name): + if message_name in ('bool', 'double', 'float', 'int32'): + return message_name + if message_name.startswith('.'): + return message_name[1:] + package = package_name.split('.') + for levels in range(len(package), -1, -1): + candidate = '.'.join(package[0:levels]) + '.' + message_name + if candidate in self.name_set: + return candidate + raise ResolutionError( + 'Resolving %s in %s failed.' % (message_name, package_name)) + + +def GenerateDocumentation(output_file, root): + """Recursively generates documentation, sorts and writes it.""" + message_list = ParseProtoFilesRecursively(root) + resolver = Resolver(message.name for message in message_list) + + output_dict = {} + for message in message_list: + content = [message.name, '=' * len(message.name), ''] + assert message.name not in output_dict + output_dict[message.name] = content + if message.preceding_comments: + content.extend(message.preceding_comments) + content.append('') + for option_type, option_name, option_comments in message.options: + # TODO(whess): For now we exclude InitialTrajectoryPose from the + # documentation. It is documented itself (since it has no Options suffix) + # and is not parsed from the Lua files. + if option_type in ('InitialTrajectoryPose',): + continue + content.append( + resolver.Resolve(option_type, message.package) + ' ' + option_name) + if not option_comments: + option_comments.append(NODOC) + for comment in option_comments: + content.append(' ' + comment) + content.append('') + if message.trailing_comments: + content.extend(message.trailing_comments) + content.append('') + + output = ['\n'.join(doc) for key, doc in sorted(list(output_dict.items()))] + print('\n\n'.join(output), file=output_file) + + +def main(): + assert not os.path.islink(TARGET) and os.path.isfile(TARGET) + assert not os.path.islink(ROOT) and os.path.isdir(ROOT) + output_file = io.open(TARGET, mode='w', encoding='UTF-8', newline='\n') + output_file.write(PREFIX) + GenerateDocumentation(output_file, ROOT) + output_file.write(SUFFIX) + output_file.close() + + +if __name__ == "__main__": + main() diff --git a/carto_ws/src/cartographer_ros-master/.dockerignore b/carto_ws/src/cartographer_ros-master/.dockerignore new file mode 100644 index 0000000..6647fc9 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/.dockerignore @@ -0,0 +1,4 @@ +**/Dockerfile* +**/.dockerignore +**/.git +**/.travis.yml diff --git a/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/bug-report.md b/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/bug-report.md new file mode 100644 index 0000000..b1d8c54 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/bug-report.md @@ -0,0 +1,19 @@ +--- +name: Bug report +about: Report a bug that you've found in this repository or in a release. + +--- + +Please provide information for your bug report: + +- if you're using cartographer_ros from source, provide the Git commit hash + (via `git log -1 --format="%H"`): + +- if you're using a release package, provide the version + (e.g. via `apt show `): + +- provide all information that is needed to analyze and reproduce the bug + (logs, commands that have been used, ...) + +PLEASE NOTE: we don't support custom forks or extensions. +If you need tuning advice, you can open a tuning issue instead. diff --git a/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/general-issue.md b/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/general-issue.md new file mode 100644 index 0000000..24180eb --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/general-issue.md @@ -0,0 +1,5 @@ +--- +name: General issue +about: Use this issue for general questions. + +--- diff --git a/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/tuning-advice.md b/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/tuning-advice.md new file mode 100644 index 0000000..48442f6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/.github/ISSUE_TEMPLATE/tuning-advice.md @@ -0,0 +1,30 @@ +--- +name: Help request for tuning +about: Describe tuning problems with your custom Cartographer setup to request help from the community. + +--- + +To improve the chances that other Cartographer users can help you, +here are some guidelines for describing your tuning problem: + +1. check the tuning guide on the documentation page and previous issues - + some problems might have been solved already by other people. + +2. run `rosbag_validate` which does some checks on your sensor data. This + tool often finds issues that can explain poor performance and must be fixed + at recording time. Please post the full output of the tool into a + GitHub Gist at https://gist.github.com/, then link it in the issue even if + it does not report anything. You can run the tool like this: + + rosrun cartographer_ros cartographer_rosbag_validate -bag_filename + +3. post a link to a Git repository containing a branch of + `cartographer_ros` containing all the configuration, launch, and URDF files + required to reproduce your issue. +4. post a link to a bag file we can use to reproduce your issue. Put it on + Google Drive, Dropbox, any webserver or wherever it is publicly + downloadable. +5. remove this boilerplate text before submitting your issue. + +PLEASE NOTE: tuning issues are not necessarily handled by the maintainers and +can be closed after a period of inactivity. diff --git a/carto_ws/src/cartographer_ros-master/.github/PULL_REQUEST_TEMPLATE.md b/carto_ws/src/cartographer_ros-master/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000..bf6c78e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,2 @@ +Want to contribute? Great! Make sure you've read and understood +[CONTRIBUTING.md](https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md). diff --git a/carto_ws/src/cartographer_ros-master/.travis.yml b/carto_ws/src/cartographer_ros-master/.travis.yml new file mode 100644 index 0000000..41286c6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/.travis.yml @@ -0,0 +1,38 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +sudo: required +services: docker + +# Cache intermediate Docker layers. For a description of how this works, see: +# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html +cache: + directories: + - /home/travis/docker/ + +env: + - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz + - ROS_RELEASE=melodic DOCKER_CACHE_FILE=/home/travis/docker/melodic-cache.tar.gz + - ROS_RELEASE=noetic DOCKER_CACHE_FILE=/home/travis/docker/noetic-cache.tar.gz + +before_install: + # $GITHUB_TOKEN must be a valid GitHub access token without access rights (https://github.com/settings/tokens). + # Either add your token to the 'env' section above or add it as an unencrypted variable in the Travis settings. + - scripts/check_access_token.sh $GITHUB_TOKEN + - scripts/load_docker_cache.sh + +install: true +script: + - docker build ${TRAVIS_BUILD_DIR} -t cartographer_ros:${ROS_RELEASE} -f Dockerfile.${ROS_RELEASE} --build-arg github_token=${GITHUB_TOKEN} + - scripts/save_docker_cache.sh diff --git a/carto_ws/src/cartographer_ros-master/AUTHORS b/carto_ws/src/cartographer_ros-master/AUTHORS new file mode 100644 index 0000000..663aac4 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/AUTHORS @@ -0,0 +1,7 @@ +# This is the list of Cartographer authors for copyright purposes. +# +# This does not necessarily list everyone who has contributed code, since in +# some cases, their employer may be the copyright holder. To see the full list +# of contributors, see the revision history in source control. +Google Inc. +and other contributors diff --git a/carto_ws/src/cartographer_ros-master/CONTRIBUTING.md b/carto_ws/src/cartographer_ros-master/CONTRIBUTING.md new file mode 100644 index 0000000..42d2869 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/CONTRIBUTING.md @@ -0,0 +1,51 @@ +Want to contribute? Great! First, read this page. + +### Before you contribute + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0): + +``` +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. +``` + +### Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). +You can sign-off a commit via `git commit -s`. + +### Code reviews + +All submissions, including submissions by project members, require review. +We use GitHub pull requests for this purpose. Make sure you've read, +understood and considered all the points below before creating your PR. + +#### Style guide + +C++ code should adhere to the +[Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). +You can handle the formatting part of the style guide via `git clang-format`. + +#### Best practices + +When preparing your PR and also during the code review make sure to follow +[best practices](https://google.github.io/eng-practices/review/developer/). +Most importantly, keep your PR under 200 lines of code and address a single +concern. + +#### Testing + +- Add unit tests and documentation (these do not count toward your 200 lines). +- Run tests as appropriate, e.g. `docker build . -t cartographer:noetic -f Dockerfile.noetic`. +- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis + runs for every update which blocks merging of other changes. diff --git a/carto_ws/src/cartographer_ros-master/Dockerfile.kinetic b/carto_ws/src/cartographer_ros-master/Dockerfile.kinetic new file mode 100644 index 0000000..7e2bb8d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/Dockerfile.kinetic @@ -0,0 +1,80 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM osrf/ros:kinetic-desktop + +ARG CARTOGRAPHER_VERSION=master + +# We require a GitHub access token to be passed. +ARG github_token + +# Xenial's base image doesn't ship with sudo. +RUN apt-get update && apt-get install -y sudo + +# First, we invalidate the entire cache if cartographer-project/cartographer has +# changed. This file's content changes whenever master changes. See: +# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ + cartographer_ros/cartographer_version.json + +# wstool needs the updated rosinstall file to clone the correct repos. +COPY cartographer_ros.rosinstall cartographer_ros/ +COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ +RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ + cartographer_ros/scripts/prepare_catkin_workspace.sh + +# rosdep needs the updated package.xml files to install the correct debs. +COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ +COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ +COPY scripts/install_debs.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install_debs.sh + +# Install Abseil and proto3. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh +RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh + +# Build, install, and test all packages individually to allow caching. The +# ordering of these steps must match the topological package ordering as +# determined by Catkin. +COPY scripts/install.sh cartographer_ros/scripts/ +COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ + +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + +COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs + +COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros + +COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ + cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz + +COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* +# A BTRFS bug may prevent us from cleaning up these directories. +# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory +RUN rm -rf cartographer_ros catkin_ws || true diff --git a/carto_ws/src/cartographer_ros-master/Dockerfile.melodic b/carto_ws/src/cartographer_ros-master/Dockerfile.melodic new file mode 100644 index 0000000..35e8399 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/Dockerfile.melodic @@ -0,0 +1,79 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM osrf/ros:melodic-desktop + +ARG CARTOGRAPHER_VERSION=master + +# We require a GitHub access token to be passed. +ARG github_token + +# Bionic's base image doesn't ship with sudo. +RUN apt-get update && apt-get install -y sudo + +# First, we invalidate the entire cache if cartographer-project/cartographer has +# changed. This file's content changes whenever master changes. See: +# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ + cartographer_ros/cartographer_version.json + +# wstool needs the updated rosinstall file to clone the correct repos. +COPY cartographer_ros.rosinstall cartographer_ros/ +COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ +RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ + cartographer_ros/scripts/prepare_catkin_workspace.sh + +# rosdep needs the updated package.xml files to install the correct debs. +COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ +COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ +COPY scripts/install_debs.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install_debs.sh + +# Install Abseil. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh + +# Build, install, and test all packages individually to allow caching. The +# ordering of these steps must match the topological package ordering as +# determined by Catkin. +COPY scripts/install.sh cartographer_ros/scripts/ +COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ + +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + +COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs + +COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros + +COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ + cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz + +COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* +# A BTRFS bug may prevent us from cleaning up these directories. +# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory +RUN rm -rf cartographer_ros catkin_ws || true diff --git a/carto_ws/src/cartographer_ros-master/Dockerfile.noetic b/carto_ws/src/cartographer_ros-master/Dockerfile.noetic new file mode 100644 index 0000000..7168d10 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/Dockerfile.noetic @@ -0,0 +1,83 @@ +# Copyright 2020 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM osrf/ros:noetic-desktop + +ARG CARTOGRAPHER_VERSION=master + +# We require a GitHub access token to be passed. +ARG github_token + +# Prevent any interaction required by apt-get. +# https://stackoverflow.com/questions/22466255 +ARG DEBIAN_FRONTEND=noninteractive + +# ROS Noetic's base image doesn't ship with sudo and git. +RUN apt-get update && apt-get install -y sudo git + +# First, we invalidate the entire cache if cartographer-project/cartographer has +# changed. This file's content changes whenever master changes. See: +# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ + cartographer_ros/cartographer_version.json + +# wstool needs the updated rosinstall file to clone the correct repos. +COPY cartographer_ros.rosinstall cartographer_ros/ +COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ +RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ + cartographer_ros/scripts/prepare_catkin_workspace.sh + +# rosdep needs the updated package.xml files to install the correct debs. +COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ +COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ +COPY scripts/install_debs.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install_debs.sh + +# Install Abseil. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh + +# Build, install, and test all packages individually to allow caching. The +# ordering of these steps must match the topological package ordering as +# determined by Catkin. +COPY scripts/install.sh cartographer_ros/scripts/ +COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ + +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + +COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs + +COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros + +COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ + cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz + +COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* +# A BTRFS bug may prevent us from cleaning up these directories. +# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory +RUN rm -rf cartographer_ros catkin_ws || true diff --git a/carto_ws/src/cartographer_ros-master/LICENSE b/carto_ws/src/cartographer_ros-master/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/carto_ws/src/cartographer_ros-master/README.rst b/carto_ws/src/cartographer_ros-master/README.rst new file mode 100644 index 0000000..fb72eb6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/README.rst @@ -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. + +============================ +Cartographer ROS Integration +============================ + +|build| |docs| |license| + +Purpose +======= + +`Cartographer`_ is a system that provides real-time simultaneous localization +and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor +configurations. This project provides Cartographer's ROS integration. + +.. _Cartographer: https://github.com/cartographer-project/cartographer +.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping + +Getting started +=============== + +* Learn to use Cartographer with ROS at `our Read the Docs site`_. +* You can ask a question by `creating an issue`_. + +.. _our Read the Docs site: https://google-cartographer-ros.readthedocs.io +.. _creating an issue: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question + +Contributing +============ + +You can find information about contributing to Cartographer's ROS integration +at `our Contribution page`_. + +.. _our Contribution page: https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md + +.. |build| image:: https://travis-ci.org/cartographer-project/cartographer_ros.svg?branch=master + :alt: Build Status + :scale: 100% + :target: https://travis-ci.org/cartographer-project/cartographer_ros +.. |docs| image:: https://readthedocs.org/projects/google-cartographer-ros/badge/?version=latest + :alt: Documentation Status + :scale: 100% + :target: https://google-cartographer-ros.readthedocs.io/en/latest/?badge=latest +.. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg + :alt: Apache 2 license. + :scale: 100% + :target: https://github.com/cartographer-project/cartographer_ros/blob/master/LICENSE + diff --git a/carto_ws/src/cartographer_ros-master/azure-pipelines.yml b/carto_ws/src/cartographer_ros-master/azure-pipelines.yml new file mode 100644 index 0000000..a778158 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/azure-pipelines.yml @@ -0,0 +1,70 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +jobs: +- job: Build + pool: + vmImage: 'vs2017-win2016' + timeoutInMinutes: 360 + steps: + - script: | + choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1 + rem Azure VM runs out of space on C:, so use D: for ros and rosdeps + mkdir D:\opt && mklink /J C:\opt D:\opt + choco upgrade %ROS_METAPACKAGE% -y + choco upgrade ros-melodic-perception -y + robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL + git clone https://github.com/cartographer-project/cartographer src\cartographer + call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y + env: + ROS_METAPACKAGE: 'ros-melodic-desktop' + displayName: Install prerequisites + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + catkin_make_isolated --use-ninja --install --cmake-args -DCMAKE_BUILD_TYPE=Release + displayName: Build + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + cd build_isolated\cartographer\install && ctest --no-compress-output -T Test + displayName: Run cartographer tests + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + cd build_isolated\cartographer_ros && ninja tests && ctest --no-compress-output -T Test + displayName: Build and run cartographer_ros tests + condition: always() + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer_ros + displayName: Convert tests to jUnit + condition: always() + + - task: PublishTestResults@2 + displayName: Publish test results + inputs: + testRunner: 'jUnit' + testResultsFiles: '**\jUnit.xml' + searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer_ros\Testing' + condition: always() diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros.rosinstall b/carto_ws/src/cartographer_ros-master/cartographer_ros.rosinstall new file mode 100644 index 0000000..430ce84 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros.rosinstall @@ -0,0 +1,2 @@ +- git: {local-name: cartographer, uri: 'https://github.com/cartographer-project/cartographer.git', version: 'master'} +- git: {local-name: cartographer_ros, uri: 'https://github.com/cartographer-project/cartographer_ros.git', version: 'master'} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/CHANGELOG.rst b/carto_ws/src/cartographer_ros-master/cartographer_ros/CHANGELOG.rst new file mode 100644 index 0000000..b81daa2 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/CHANGELOG.rst @@ -0,0 +1,19 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cartographer_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2018-06-01) +---------------------- +* https://github.com/googlecartographer/cartographer_ros/compare/0.3.0...1.0.0 + +0.3.0 (2017-11-23) +------------------ +* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0 + +0.2.0 (2017-06-19) +------------------ +* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0 + +0.1.0 (2017-05-18) +------------------ +* First unstable development release diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/CMakeLists.txt b/carto_ws/src/cartographer_ros-master/cartographer_ros/CMakeLists.txt new file mode 100644 index 0000000..a7e483c --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/CMakeLists.txt @@ -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. + +cmake_minimum_required(VERSION 2.8.12) # Ships with Ubuntu 14.04 (Trusty) + +project(cartographer_ros) + +set(PACKAGE_DEPENDENCIES + cartographer_ros_msgs + geometry_msgs + message_runtime + nav_msgs + pcl_conversions + rosbag + roscpp + roslib + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_ros + urdf + visualization_msgs +) + +if(WIN32) + set(Boost_USE_STATIC_LIBS FALSE) +endif() +# For yet unknown reason, if Boost is find_packaged() after find_package(cartographer), +# some Boost libraries including Thread are set to static, despite Boost_USE_STATIC_LIBS, +# which causes linking problems on windows due to shared/static Thread clashing. +# PCL also finds Boost. Work around by moving before find_package(cartographer). +find_package(Boost REQUIRED COMPONENTS system iostreams) +find_package(PCL REQUIRED COMPONENTS common) + +find_package(cartographer REQUIRED) +include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") +option(BUILD_GRPC "build features that require Cartographer gRPC support" false) +google_initialize_cartographer_project() +google_enable_testing() +set(CARTOGRAPHER_GMOCK_LIBRARIES ${GMOCK_LIBRARIES}) + +find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) + +include(FindPkgConfig) + +find_package(absl REQUIRED) +find_package(LuaGoogle REQUIRED) +find_package(Eigen3 REQUIRED) + +find_package(urdfdom_headers REQUIRED) +if(DEFINED urdfdom_headers_VERSION) + if(${urdfdom_headers_VERSION} GREATER 0.4.1) + add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS) + endif() +endif() + +include_directories( + ${urdfdom_headers_INCLUDE_DIRS} +) + +# Override Catkin's GTest configuration to use GMock. +set(GTEST_FOUND TRUE) +set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS}) +set(GTEST_LIBRARIES ${CARTOGRAPHER_GMOCK_LIBRARIES}) + +catkin_package( + CATKIN_DEPENDS + ${PACKAGE_DEPENDENCIES} + DEPENDS + # TODO(damonkohler): This should be here but causes Catkin to abort because + # protobuf specifies a library '-lpthread' instead of just 'pthread'. + # CARTOGRAPHER + PCL + EIGEN3 + Boost + urdfdom_headers + INCLUDE_DIRS "." + LIBRARIES ${PROJECT_NAME} +) + +file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h") +file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc") +file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc") +file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*") +list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS}) +list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES}) +if (NOT ${BUILD_GRPC}) + list(REMOVE_ITEM ALL_SRCS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES}) + list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES}) +endif() +add_library(${PROJECT_NAME} STATIC ${ALL_SRCS}) +add_subdirectory("cartographer_ros") + +target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) + +# Lua +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) + +# PCL +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES}) +set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ") +foreach(DEFINITION ${PCL_DEFINITIONS}) + list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX) + if (${DEFINITIONS_INDEX} GREATER -1) + continue() + endif() + set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}") +endforeach() + +# Eigen +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${EIGEN3_INCLUDE_DIR}") + +# Boost +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${Boost_INCLUDE_DIRS}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) + +# Catkin +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + +# Add the binary directory first, so that port.h is included after it has +# been generated. +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + $ +) + +set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") +set_target_properties(${PROJECT_NAME} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + +if (CATKIN_ENABLE_TESTING) + foreach(TEST_SOURCE_FILENAME ${ALL_TESTS}) + get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE) + catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) + # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to + # target_link_libraries. That forces us to do the same. + target_link_libraries(${TEST_NAME} ${GMOCK_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) + target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) + target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) + target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES}) + add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${TEST_NAME} cartographer) + target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) + set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + # Needed for dynamically linked GTest on Windows. + if (WIN32) + target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY) + endif() + endforeach() +endif() + +install(DIRECTORY launch urdf configuration_files + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(PROGRAMS scripts/tf_remove_frames.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Install source headers. +file(GLOB_RECURSE HDRS "cartographer_ros/*.h") +foreach(HDR ${HDRS}) + file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR}) + get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY) + install( + FILES + ${HDR} + DESTINATION + include/${INSTALL_DIR} + ) +endforeach() diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.clang-format b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.clang-format new file mode 100644 index 0000000..5650f22 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: Google +DerivePointerAlignment: false diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.vscode/launch.json b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.vscode/launch.json new file mode 100644 index 0000000..69737dc --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.vscode/launch.json @@ -0,0 +1,27 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "g++ build and debug active file", + "type": "cppdbg", + "request": "launch", + "program": "${fileDirname}/${fileBasenameNoExtension}", + "args": [], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ], + "preLaunchTask": "g++ build active file", + "postDebugTask": "delete output file", + "miDebuggerPath": "/usr/bin/gdb" + } + ] + } \ No newline at end of file diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.vscode/tasks.json b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.vscode/tasks.json new file mode 100644 index 0000000..c959d9a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/.vscode/tasks.json @@ -0,0 +1,39 @@ +{ + "tasks": [ + { + //编译源文件 + "type": "shell", + "label": "g++ build active file", + "command": "/usr/bin/g++", + "args": [ + "-std=c++11", //C++版本， 可不加 + "-g", + "${file}", + "-o", + "${fileDirname}/${fileBasenameNoExtension}" + ], + "options": { + "cwd": "/usr/bin" + }, + "problemMatcher": [ + "$gcc" + ], + "group": { + "kind": "build", + "isDefault": true + } + }, + { //删除二进制文件 + "type": "shell", + "label": "delete output file", + "command": "rm", + "args": [ + "${fileDirname}/${fileBasenameNoExtension}" + ], + "presentation": { + "reveal": "silent", //删除过程不切换终端（专注程序输出） + } + } + ], + "version": "2.0.0" +} \ No newline at end of file diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/CMakeLists.txt b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/CMakeLists.txt new file mode 100644 index 0000000..f2616d1 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -0,0 +1,155 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +google_binary(cartographer_assets_writer + SRCS + assets_writer_main.cc + ros_map_writing_points_processor.h + ros_map_writing_points_processor.cc +) + +install(TARGETS cartographer_assets_writer + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_node + SRCS + node_main.cc +) + +install(TARGETS cartographer_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_offline_node + SRCS + offline_node_main.cc +) + +install(TARGETS cartographer_offline_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_occupancy_grid_node + SRCS + occupancy_grid_node_main.cc +) + +install(TARGETS cartographer_occupancy_grid_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_rosbag_validate + SRCS + rosbag_validate_main.cc +) + +install(TARGETS cartographer_rosbag_validate + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_pbstream_to_ros_map + SRCS + pbstream_to_ros_map_main.cc +) + +install(TARGETS cartographer_pbstream_to_ros_map + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_pbstream_map_publisher + SRCS + pbstream_map_publisher_main.cc +) + +install(TARGETS cartographer_pbstream_map_publisher + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_dev_pbstream_trajectories_to_rosbag + SRCS + dev/pbstream_trajectories_to_rosbag_main.cc +) + +install(TARGETS cartographer_dev_pbstream_trajectories_to_rosbag + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_dev_rosbag_publisher + SRCS + dev/rosbag_publisher_main.cc +) + +install(TARGETS cartographer_dev_rosbag_publisher + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +google_binary(cartographer_dev_trajectory_comparison + SRCS + dev/trajectory_comparison_main.cc +) + +install(TARGETS cartographer_dev_trajectory_comparison + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# TODO(cschuet): Add support for shared library case. +if (${BUILD_GRPC}) + google_binary(cartographer_grpc_node + SRCS + cartographer_grpc/node_grpc_main.cc + ) + + install(TARGETS cartographer_grpc_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + google_binary(cartographer_grpc_offline_node + SRCS + cartographer_grpc/offline_node_grpc_main.cc + ) + + install(TARGETS cartographer_grpc_offline_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(PROGRAMS + ../scripts/cartographer_grpc_server.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +endif() diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer.cc new file mode 100644 index 0000000..1ca3e09 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer.cc @@ -0,0 +1,272 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/assets_writer.h" + +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/math.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/ros_map_writing_points_processor.h" +#include "cartographer_ros/time_conversion.h" +#include "cartographer_ros/urdf_reader.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ros/ros.h" +#include "ros/time.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_msgs/TFMessage.h" +#include "tf2_ros/buffer.h" +#include "urdf/model.h" + +namespace cartographer_ros { +namespace { + +constexpr char kTfStaticTopic[] = "/tf_static"; +namespace carto = ::cartographer; + +std::unique_ptr +CreatePipelineBuilder( + const std::vector& trajectories, + const std::string file_prefix) { + const auto file_writer_factory = + AssetsWriter::CreateFileWriterFactory(file_prefix); + auto builder = absl::make_unique(); + carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory, + builder.get()); + builder->Register(RosMapWritingPointsProcessor::kConfigurationFileActionName, + [file_writer_factory]( + carto::common::LuaParameterDictionary* const dictionary, + carto::io::PointsProcessor* const next) + -> std::unique_ptr { + return RosMapWritingPointsProcessor::FromDictionary( + file_writer_factory, dictionary, next); + }); + return builder; +} + +std::unique_ptr LoadLuaDictionary( + const std::string& configuration_directory, + const std::string& configuration_basename) { + auto file_resolver = + absl::make_unique( + std::vector{configuration_directory}); + + const std::string code = + file_resolver->GetFileContentOrDie(configuration_basename); + auto lua_parameter_dictionary = + absl::make_unique( + code, std::move(file_resolver)); + return lua_parameter_dictionary; +} + +template +std::unique_ptr HandleMessage( + const T& message, const std::string& tracking_frame, + const tf2_ros::Buffer& tf_buffer, + const carto::transform::TransformInterpolationBuffer& + transform_interpolation_buffer) { + const carto::common::Time start_time = FromRos(message.header.stamp); + + auto points_batch = absl::make_unique(); + points_batch->start_time = start_time; + points_batch->frame_id = message.header.frame_id; + + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time point_cloud_time; + std::tie(point_cloud, point_cloud_time) = + ToPointCloudWithIntensities(message); + CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); + + for (size_t i = 0; i < point_cloud.points.size(); ++i) { + const carto::common::Time time = + point_cloud_time + + carto::common::FromSeconds(point_cloud.points[i].time); + if (!transform_interpolation_buffer.Has(time)) { + continue; + } + const carto::transform::Rigid3d tracking_to_map = + transform_interpolation_buffer.Lookup(time); + const carto::transform::Rigid3d sensor_to_tracking = + ToRigid3d(tf_buffer.lookupTransform( + tracking_frame, message.header.frame_id, ToRos(time))); + const carto::transform::Rigid3f sensor_to_map = + (tracking_to_map * sensor_to_tracking).cast(); + points_batch->points.push_back( + sensor_to_map * + carto::sensor::ToRangefinderPoint(point_cloud.points[i])); + points_batch->intensities.push_back(point_cloud.intensities[i]); + // We use the last transform for the origin, which is approximately correct. + points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); + } + if (points_batch->points.empty()) { + return nullptr; + } + return points_batch; +} + +} // namespace + +AssetsWriter::AssetsWriter(const std::string& pose_graph_filename, + const std::vector& bag_filenames, + const std::string& output_file_prefix) + : bag_filenames_(bag_filenames), + pose_graph_( + carto::io::DeserializePoseGraphFromFile(pose_graph_filename)) { + CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size()) + << "Pose graphs contains " << pose_graph_.trajectory_size() + << " trajectories while " << bag_filenames_.size() + << " bags were provided. This tool requires one bag for each " + "trajectory in the same order as the correponding trajectories in the " + "pose graph proto."; + + // This vector must outlive the pipeline. + all_trajectories_ = std::vector<::cartographer::mapping::proto::Trajectory>( + pose_graph_.trajectory().begin(), pose_graph_.trajectory().end()); + + const std::string file_prefix = !output_file_prefix.empty() + ? output_file_prefix + : bag_filenames_.front() + "_"; + point_pipeline_builder_ = + CreatePipelineBuilder(all_trajectories_, file_prefix); +} + +void AssetsWriter::RegisterPointsProcessor( + const std::string& name, + cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction factory) { + point_pipeline_builder_->Register(name, factory); +} + +void AssetsWriter::Run(const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, + const bool use_bag_transforms) { + const auto lua_parameter_dictionary = + LoadLuaDictionary(configuration_directory, configuration_basename); + + std::vector> pipeline = + point_pipeline_builder_->CreatePipeline( + lua_parameter_dictionary->GetDictionary("pipeline").get()); + const std::string tracking_frame = + lua_parameter_dictionary->GetString("tracking_frame"); + + do { + for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size(); + ++trajectory_id) { + const carto::mapping::proto::Trajectory& trajectory_proto = + pose_graph_.trajectory(trajectory_id); + const std::string& bag_filename = bag_filenames_[trajectory_id]; + LOG(INFO) << "Processing " << bag_filename << "..."; + if (trajectory_proto.node_size() == 0) { + continue; + } + tf2_ros::Buffer tf_buffer; + if (!urdf_filename.empty()) { + ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); + } + + const carto::transform::TransformInterpolationBuffer + transform_interpolation_buffer(trajectory_proto); + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag); + const ::ros::Time begin_time = view.getBeginTime(); + const double duration_in_seconds = + (view.getEndTime() - begin_time).toSec(); + + // We need to keep 'tf_buffer' small because it becomes very inefficient + // otherwise. We make sure that tf_messages are published before any data + // messages, so that tf lookups always work. + std::deque delayed_messages; + // We publish tf messages one second earlier than other messages. Under + // the assumption of higher frequency tf this should ensure that tf can + // always interpolate. + const ::ros::Duration kDelay(1.); + for (const rosbag::MessageInstance& message : view) { + if (use_bag_transforms && message.isType()) { + auto tf_message = message.instantiate(); + for (const auto& transform : tf_message->transforms) { + try { + tf_buffer.setTransform(transform, "unused_authority", + message.getTopic() == kTfStaticTopic); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + } + } + + while (!delayed_messages.empty() && delayed_messages.front().getTime() < + message.getTime() - kDelay) { + const rosbag::MessageInstance& delayed_message = + delayed_messages.front(); + + std::unique_ptr points_batch; + if (delayed_message.isType()) { + points_batch = HandleMessage( + *delayed_message.instantiate(), + tracking_frame, tf_buffer, transform_interpolation_buffer); + } else if (delayed_message + .isType()) { + points_batch = HandleMessage( + *delayed_message.instantiate(), + tracking_frame, tf_buffer, transform_interpolation_buffer); + } else if (delayed_message.isType()) { + points_batch = HandleMessage( + *delayed_message.instantiate(), + tracking_frame, tf_buffer, transform_interpolation_buffer); + } + if (points_batch != nullptr) { + points_batch->trajectory_id = trajectory_id; + pipeline.back()->Process(std::move(points_batch)); + } + delayed_messages.pop_front(); + } + delayed_messages.push_back(message); + LOG_EVERY_N(INFO, 10000) + << "Processed " << (message.getTime() - begin_time).toSec() + << " of " << duration_in_seconds << " bag time seconds..."; + } + bag.close(); + } + } while (pipeline.back()->Flush() == + carto::io::PointsProcessor::FlushResult::kRestartStream); +} + +::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory( + const std::string& file_path) { + const auto file_writer_factory = [file_path](const std::string& filename) { + return absl::make_unique(file_path + filename); + }; + return file_writer_factory; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer.h new file mode 100644 index 0000000..c12a87f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer.h @@ -0,0 +1,63 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H + +namespace cartographer_ros { + +class AssetsWriter { + public: + AssetsWriter(const std::string& pose_graph_filename, + const std::vector& bag_filenames, + const std::string& output_file_prefix); + + // Registers a new PointsProcessor type uniquly identified by 'name' which + // will be created using 'factory'. + void RegisterPointsProcessor( + const std::string& name, + cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction + factory); + + // Configures a points processing pipeline and pushes the points from the + // bag through the pipeline. + void Run(const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, bool use_bag_transforms); + + // Creates a FileWriterFactory which creates a FileWriter for storing assets. + static ::cartographer::io::FileWriterFactory CreateFileWriterFactory( + const std::string& file_path); + + private: + std::vector bag_filenames_; + std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_; + ::cartographer::mapping::proto::PoseGraph pose_graph_; + std::unique_ptr<::cartographer::io::PointsProcessorPipelineBuilder> + point_pipeline_builder_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer_main.cc new file mode 100644 index 0000000..238f0df --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -0,0 +1,64 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "absl/strings/str_split.h" +#include "cartographer_ros/assets_writer.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); +DEFINE_string( + urdf_filename, "", + "URDF file that contains static links for your sensor configuration."); +DEFINE_string(bag_filenames, "", + "Bags to process, must be in the same order as the trajectories " + "in 'pose_graph_filename'."); +DEFINE_string(pose_graph_filename, "", + "Proto stream file containing the pose graph."); +DEFINE_bool(use_bag_transforms, true, + "Whether to read and use the transforms from the bag."); +DEFINE_string(output_file_prefix, "", + "Will be prefixed to all output file names and can be used to " + "define the output directory. If empty, the first bag filename " + "will be used."); + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basename.empty()) + << "-configuration_basename is missing."; + CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing."; + CHECK(!FLAGS_pose_graph_filename.empty()) + << "-pose_graph_filename is missing."; + + ::cartographer_ros::AssetsWriter asset_writer( + FLAGS_pose_graph_filename, + absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()), + FLAGS_output_file_prefix); + + asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename, + FLAGS_urdf_filename, FLAGS_use_bag_transforms); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc new file mode 100644 index 0000000..20ee55d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc @@ -0,0 +1,117 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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 "absl/memory/memory.h" +#include "cartographer/cloud/client/map_builder_stub.h" +#include "cartographer_ros/node.h" +#include "cartographer_ros/node_options.h" +#include "cartographer_ros/ros_log_sink.h" +#include "gflags/gflags.h" +#include "tf2_ros/transform_listener.h" + +DEFINE_bool(collect_metrics, false, + "Activates the collection of runtime metrics. If activated, the " + "metrics can be accessed via a ROS service."); +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); +DEFINE_string(server_address, "localhost:50051", + "gRPC server address to stream the sensor data to."); +DEFINE_bool( + start_trajectory_with_default_topics, true, + "Enable to immediately start the first trajectory with default topics."); +DEFINE_string( + save_map_filename, "", + "If non-empty, serialize state and write it to disk before shutting down."); +DEFINE_string(load_state_filename, "", + "If non-empty, filename of a .pbstream file " + "to load, containing a saved SLAM state. " + "Unless --upload_load_state_file is set, the filepath refers " + "to the gRPC server's file system."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); +DEFINE_bool(upload_load_state_file, false, + "Upload the .pbstream file from a local path to the (remote) gRPC " + "server instead of loading it from the server file system."); +DEFINE_string(client_id, "", + "Cartographer client ID to use when connecting to the server."); + +namespace cartographer_ros { +namespace { + +void Run() { + constexpr double kTfBufferCacheTimeInSeconds = 10.; + tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; + tf2_ros::TransformListener tf(tf_buffer); + NodeOptions node_options; + TrajectoryOptions trajectory_options; + std::tie(node_options, trajectory_options) = + LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); + + auto map_builder = absl::make_unique<::cartographer::cloud::MapBuilderStub>( + FLAGS_server_address, FLAGS_client_id); + + if (!FLAGS_load_state_filename.empty() && !FLAGS_upload_load_state_file) { + map_builder->LoadStateFromFile(FLAGS_load_state_filename, + FLAGS_load_frozen_state); + } + + Node node(node_options, std::move(map_builder), &tf_buffer, + FLAGS_collect_metrics); + + if (!FLAGS_load_state_filename.empty() && FLAGS_upload_load_state_file) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); + } + + if (FLAGS_start_trajectory_with_default_topics) { + node.StartTrajectoryWithDefaultTopics(trajectory_options); + } + + ::ros::spin(); + + node.FinishAllTrajectories(); + node.RunFinalOptimization(); + + if (!FLAGS_save_map_filename.empty()) { + node.SerializeState(FLAGS_save_map_filename, + false /* include_unfinished_submaps */); + } +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basename.empty()) + << "-configuration_basename is missing."; + CHECK(!FLAGS_client_id.empty()) << "-client_id is missing."; + + ::ros::init(argc, argv, "cartographer_grpc_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + cartographer_ros::Run(); + ::ros::shutdown(); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc new file mode 100644 index 0000000..3b89997 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc @@ -0,0 +1,49 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/cloud/client/map_builder_stub.h" +#include "cartographer_ros/offline_node.h" +#include "cartographer_ros/ros_log_sink.h" +#include "gflags/gflags.h" +#include "ros/ros.h" + +DEFINE_string(server_address, "localhost:50051", + "gRPC server address to " + "stream the sensor data to."); +DEFINE_string(client_id, "", + "Cartographer client ID to use when connecting to the server."); + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_client_id.empty()) << "-client_id is missing."; + + ::ros::init(argc, argv, "cartographer_grpc_offline_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + + const cartographer_ros::MapBuilderFactory map_builder_factory = + [](const ::cartographer::mapping::proto::MapBuilderOptions&) { + return absl::make_unique< ::cartographer::cloud::MapBuilderStub>( + FLAGS_server_address, FLAGS_client_id); + }; + + cartographer_ros::RunOfflineNode(map_builder_factory); + + ::ros::shutdown(); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/configuration_files_test.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/configuration_files_test.cc new file mode 100644 index 0000000..232960a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/configuration_files_test.cc @@ -0,0 +1,44 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer_ros/node_options.h" +#include "gtest/gtest.h" +#include "ros/package.h" + +namespace cartographer_ros { +namespace { + +class ConfigurationFilesTest : public ::testing::TestWithParam {}; + +TEST_P(ConfigurationFilesTest, ValidateNodeOptions) { + EXPECT_NO_FATAL_FAILURE({ + LoadOptions( + ::ros::package::getPath("cartographer_ros") + "/configuration_files", + GetParam()); + }); +} + +INSTANTIATE_TEST_CASE_P( + ValidateAllNodeOptions, ConfigurationFilesTest, + ::testing::Values("backpack_2d.lua", "backpack_2d_localization.lua", + "backpack_3d.lua", "backpack_3d_localization.lua", + "pr2.lua", "revo_lds.lua", "taurob_tracker.lua")); + +} // namespace +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc new file mode 100644 index 0000000..63d2c5f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc @@ -0,0 +1,102 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "absl/strings/str_cat.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" +#include "geometry_msgs/TransformStamped.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "rosbag/bag.h" +#include "tf2_msgs/TFMessage.h" + +DEFINE_string(input, "", "pbstream file to process"); +DEFINE_string(output, "", "Bag file to write to."); +DEFINE_string(parent_frame, "map", "Frame id to use as parent frame."); + +namespace cartographer_ros { +namespace { + +geometry_msgs::TransformStamped ToTransformStamped( + int64_t timestamp_uts, const std::string& parent_frame_id, + const std::string& child_frame_id, + const cartographer::transform::proto::Rigid3d& parent_T_child) { + static int64_t seq = 0; + geometry_msgs::TransformStamped transform_stamped; + transform_stamped.header.seq = ++seq; + transform_stamped.header.frame_id = parent_frame_id; + transform_stamped.header.stamp = cartographer_ros::ToRos( + ::cartographer::common::FromUniversal(timestamp_uts)); + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform = cartographer_ros::ToGeometryMsgTransform( + ::cartographer::transform::ToRigid3(parent_T_child)); + return transform_stamped; +} + +void pbstream_trajectories_to_bag(const std::string& pbstream_filename, + const std::string& output_bag_filename, + const std::string& parent_frame_id) { + const auto pose_graph = + cartographer::io::DeserializePoseGraphFromFile(FLAGS_input); + + rosbag::Bag bag(output_bag_filename, rosbag::bagmode::Write); + for (const auto trajectory : pose_graph.trajectory()) { + const auto child_frame_id = + absl::StrCat("trajectory_", trajectory.trajectory_id()); + LOG(INFO) + << "Writing tf and geometry_msgs/TransformStamped for trajectory id " + << trajectory.trajectory_id() << " with " << trajectory.node_size() + << " nodes."; + for (const auto& node : trajectory.node()) { + tf2_msgs::TFMessage tf_msg; + geometry_msgs::TransformStamped transform_stamped = ToTransformStamped( + node.timestamp(), parent_frame_id, child_frame_id, node.pose()); + tf_msg.transforms.push_back(transform_stamped); + bag.write(child_frame_id, transform_stamped.header.stamp, + transform_stamped); + bag.write("/tf", transform_stamped.header.stamp, tf_msg); + } + } +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char* argv[]) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "\n\n" + "Extracts all trajectories from the pbstream and creates a bag file with " + "the trajectory poses stored in /tf.\nAdditionally, each trajectory is " + "also written separately to a geometry_msgs/TransformStamped topic named " + "after the TF child_frame_id of the trajectory.\n For each trajectory, " + "the tool will write transforms with the tf parent_frame_id set " + "according to the `parent_frame` commandline flag and child_frame_id to " + "`trajectory_i`, with `i` corresponding to the `trajectory_id`."); + google::ParseCommandLineFlags(&argc, &argv, true); + CHECK(!FLAGS_input.empty()) << "-input pbstream is missing."; + CHECK(!FLAGS_output.empty()) << "-output is missing."; + + cartographer_ros::pbstream_trajectories_to_bag(FLAGS_input, FLAGS_output, + FLAGS_parent_frame); + return 0; +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc new file mode 100644 index 0000000..e069482 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc @@ -0,0 +1,148 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/time.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/time_conversion.h" +#include "gflags/gflags.h" +#include "nav_msgs/Odometry.h" +#include "ros/ros.h" +#include "ros/time.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" +#include "tf2_msgs/TFMessage.h" + +DEFINE_string(bag_filename, "", "Bag to publish."); + +const int kQueueSize = 1; + +template +void PublishWithModifiedTimestamp(MessagePtrType message, + const ros::Publisher& publisher, + ros::Duration bag_to_current) { + ros::Time& stamp = message->header.stamp; + stamp += bag_to_current; + publisher.publish(message); +} + +template <> +void PublishWithModifiedTimestamp( + tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher, + ros::Duration bag_to_current) { + for (const auto& transform : message->transforms) { + ros::Time& stamp = const_cast(transform.header.stamp); + stamp += bag_to_current; + } + publisher.publish(message); +} + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "\n\n" + "This replays and publishes messages from a given bag file, modifying " + "their header timestamps to match current ROS time.\n\n" + "Messages are published in the same sequence and with the same delay " + "they were recorded." + "Contrary to rosbag play, it does not publish a clock, so time is" + "hopefully smoother and it should be possible to reproduce timing" + "issues.\n" + "It only plays message types related to Cartographer.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; + + ros::init(argc, argv, "rosbag_publisher"); + ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + + rosbag::Bag bag; + bag.open(FLAGS_bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag); + ros::NodeHandle node_handle; + bool use_sim_time; + node_handle.getParam("/use_sim_time", use_sim_time); + if (use_sim_time) { + LOG(ERROR) << "use_sim_time is true but not supported. Expect conflicting " + "ros::Time and message header times or weird behavior."; + } + std::map topic_to_publisher; + for (const rosbag::ConnectionInfo* c : view.getConnections()) { + const std::string& topic = c->topic; + if (topic_to_publisher.count(topic) == 0) { + ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum, + c->datatype, c->msg_def); + topic_to_publisher[topic] = node_handle.advertise(options); + } + } + ros::Duration(1).sleep(); + CHECK(ros::ok()); + + ros::Time current_start = ros::Time::now(); + ros::Time bag_start = view.getBeginTime(); + ros::Duration bag_to_current = current_start - bag_start; + for (const rosbag::MessageInstance& message : view) { + ros::Duration after_bag_start = message.getTime() - bag_start; + if (!::ros::ok()) { + break; + } + ros::Time planned_publish_time = current_start + after_bag_start; + ros::Time::sleepUntil(planned_publish_time); + + ros::Publisher& publisher = topic_to_publisher.at(message.getTopic()); + if (message.isType()) { + PublishWithModifiedTimestamp( + message.instantiate(), publisher, + bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp( + message.instantiate(), publisher, + bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp( + message.instantiate(), publisher, + bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), + publisher, bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), + publisher, bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), + publisher, bag_to_current); + } else { + LOG(WARNING) << "Skipping message with type " << message.getDataType(); + } + + ros::Time current_time = ros::Time::now(); + double simulation_delay = cartographer::common::ToSeconds( + cartographer_ros::FromRos(current_time) - + cartographer_ros::FromRos(planned_publish_time)); + if (std::abs(simulation_delay) > 0.001) { + LOG(WARNING) << "Playback delayed by " << simulation_delay + << " s. planned_publish_time: " << planned_publish_time + << " current_time: " << current_time; + } + } + bag.close(); + + ros::shutdown(); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc new file mode 100644 index 0000000..c29ba00 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc @@ -0,0 +1,143 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ros/ros.h" +#include "ros/time.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_msgs/TFMessage.h" + +DEFINE_string(bag_filename, "", + "Bag file containing TF messages of the trajectory that will be " + "compared against the trajectory in the .pbstream file."); +DEFINE_string(tf_parent_frame, "map", + "The parent frame ID of the TF trajectory from the bag file."); +DEFINE_string(tf_child_frame, "base_link", + "The child frame ID of the TF trajectory from the bag file."); +DEFINE_string(pbstream_filename, "", + "Proto stream file containing the pose graph. The last " + "trajectory will be used for comparison."); + +namespace cartographer_ros { +namespace { + +double FractionSmallerThan(const std::vector& v, double x) { + return static_cast(std::count_if( + v.begin(), v.end(), [=](double value) { return value < x; })) / + v.size(); +} + +std::string QuantilesToString(std::vector* v) { + if (v->empty()) return "(empty vector)"; + std::sort(v->begin(), v->end()); + std::stringstream result; + const int kNumQuantiles = 10; + for (int i = 0; i < kNumQuantiles; ++i) { + auto value = v->at(v->size() * i / kNumQuantiles); + auto percentage = 100 * i / kNumQuantiles; + result << percentage << "%: " << value << "\n"; + } + result << "100%: " << v->back() << "\n"; + return result.str(); +} + +void Run(const std::string& pbstream_filename, + const std::string& bag_filename) { + cartographer::mapping::proto::PoseGraph pose_graph_proto = + cartographer::io::DeserializePoseGraphFromFile(pbstream_filename); + const cartographer::mapping::proto::Trajectory& last_trajectory_proto = + *pose_graph_proto.mutable_trajectory()->rbegin(); + const cartographer::transform::TransformInterpolationBuffer + transform_interpolation_buffer(last_trajectory_proto); + + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag); + std::vector deviation_translation, deviation_rotation; + const double signal_maximum = std::numeric_limits::max(); + for (const rosbag::MessageInstance& message : view) { + if (!message.isType()) { + continue; + } + auto tf_message = message.instantiate(); + for (const auto& transform : tf_message->transforms) { + if (transform.header.frame_id != FLAGS_tf_parent_frame || + transform.child_frame_id != FLAGS_tf_child_frame) { + continue; + } + const cartographer::common::Time transform_time = + FromRos(message.getTime()); + if (!transform_interpolation_buffer.Has(transform_time)) { + deviation_translation.push_back(signal_maximum); + deviation_rotation.push_back(signal_maximum); + continue; + } + auto optimized_transform = + transform_interpolation_buffer.Lookup(transform_time); + auto published_transform = ToRigid3d(transform); + deviation_translation.push_back((published_transform.translation() - + optimized_transform.translation()) + .norm()); + deviation_rotation.push_back( + published_transform.rotation().angularDistance( + optimized_transform.rotation())); + } + } + bag.close(); + LOG(INFO) << "Distribution of translation difference:\n" + << QuantilesToString(&deviation_translation); + LOG(INFO) << "Distribution of rotation difference:\n" + << QuantilesToString(&deviation_rotation); + LOG(INFO) << "Fraction of translation difference smaller than 1m: " + << FractionSmallerThan(deviation_translation, 1); + LOG(INFO) << "Fraction of translation difference smaller than 0.1m: " + << FractionSmallerThan(deviation_translation, 0.1); + LOG(INFO) << "Fraction of translation difference smaller than 0.05m: " + << FractionSmallerThan(deviation_translation, 0.05); + LOG(INFO) << "Fraction of translation difference smaller than 0.01m: " + << FractionSmallerThan(deviation_translation, 0.01); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "\n\n" + "This compares a trajectory from a bag file against the " + "last trajectory in a pbstream file.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; + CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; + ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_bag_filename); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/map_builder_bridge.cc new file mode 100644 index 0000000..75784c8 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -0,0 +1,542 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/map_builder_bridge.h" + +#include "absl/memory/memory.h" +#include "absl/strings/str_cat.h" +#include "cartographer/io/color.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" +#include "cartographer_ros_msgs/StatusCode.h" +#include "cartographer_ros_msgs/StatusResponse.h" + +namespace cartographer_ros { +namespace { + +using ::cartographer::transform::Rigid3d; + +constexpr double kTrajectoryLineStripMarkerScale = 0.07; +constexpr double kLandmarkMarkerScale = 0.2; +constexpr double kConstraintMarkerScale = 0.025; + +::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { + ::std_msgs::ColorRGBA result; + result.r = color[0]; + result.g = color[1]; + result.b = color[2]; + result.a = 1.f; + return result; +} + +visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id, + const std::string& frame_id) { + visualization_msgs::Marker marker; + marker.ns = absl::StrCat("Trajectory ", trajectory_id); + marker.id = 0; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.header.stamp = ::ros::Time::now(); + marker.header.frame_id = frame_id; + marker.color = ToMessage(cartographer::io::GetColor(trajectory_id)); + marker.scale.x = kTrajectoryLineStripMarkerScale; + marker.pose.orientation.w = 1.; + marker.pose.position.z = 0.05; + return marker; +} + +int GetLandmarkIndex( + const std::string& landmark_id, + std::unordered_map* landmark_id_to_index) { + auto it = landmark_id_to_index->find(landmark_id); + if (it == landmark_id_to_index->end()) { + const int new_index = landmark_id_to_index->size(); + landmark_id_to_index->emplace(landmark_id, new_index); + return new_index; + } + return it->second; +} + +visualization_msgs::Marker CreateLandmarkMarker(int landmark_index, + const Rigid3d& landmark_pose, + const std::string& frame_id) { + visualization_msgs::Marker marker; + marker.ns = "Landmarks"; + marker.id = landmark_index; + marker.type = visualization_msgs::Marker::SPHERE; + marker.header.stamp = ::ros::Time::now(); + marker.header.frame_id = frame_id; + marker.scale.x = kLandmarkMarkerScale; + marker.scale.y = kLandmarkMarkerScale; + marker.scale.z = kLandmarkMarkerScale; + marker.color = ToMessage(cartographer::io::GetColor(landmark_index)); + marker.pose = ToGeometryMsgPose(landmark_pose); + return marker; +} + +void PushAndResetLineMarker(visualization_msgs::Marker* marker, + std::vector* markers) { + markers->push_back(*marker); + ++marker->id; + marker->points.clear(); +} + +} // namespace + +MapBuilderBridge::MapBuilderBridge( + const NodeOptions& node_options, + std::unique_ptr map_builder, + tf2_ros::Buffer* const tf_buffer) + : node_options_(node_options), + map_builder_(std::move(map_builder)), + tf_buffer_(tf_buffer) {} + +void MapBuilderBridge::LoadState(const std::string& state_filename, + bool load_frozen_state) { + // Check if suffix of the state file is ".pbstream". + const std::string suffix = ".pbstream"; + CHECK_EQ(state_filename.substr( + std::max(state_filename.size() - suffix.size(), 0)), + suffix) + << "The file containing the state to be loaded must be a " + ".pbstream file."; + LOG(INFO) << "Loading saved state '" << state_filename << "'..."; + cartographer::io::ProtoStreamReader stream(state_filename); + map_builder_->LoadState(&stream, load_frozen_state); +} + +int MapBuilderBridge::AddTrajectory( + const std::set& + expected_sensor_ids, + const TrajectoryOptions& trajectory_options) { + const int trajectory_id = map_builder_->AddTrajectoryBuilder( + expected_sensor_ids, trajectory_options.trajectory_builder_options, + [this](const int trajectory_id, const ::cartographer::common::Time time, + const Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local, + const std::unique_ptr< + const ::cartographer::mapping::TrajectoryBuilderInterface:: + InsertionResult>) { + OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); + }); + LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; + + // Make sure there is no trajectory with 'trajectory_id' yet. + CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); + sensor_bridges_[trajectory_id] = absl::make_unique( + trajectory_options.num_subdivisions_per_laser_scan, + trajectory_options.tracking_frame, + node_options_.lookup_transform_timeout_sec, tf_buffer_, + map_builder_->GetTrajectoryBuilder(trajectory_id)); + auto emplace_result = + trajectory_options_.emplace(trajectory_id, trajectory_options); + CHECK(emplace_result.second == true); + return trajectory_id; +} + +void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { + LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'..."; + + // Make sure there is a trajectory with 'trajectory_id'. + CHECK(GetTrajectoryStates().count(trajectory_id)); + map_builder_->FinishTrajectory(trajectory_id); + sensor_bridges_.erase(trajectory_id); +} + +void MapBuilderBridge::RunFinalOptimization() { + LOG(INFO) << "Running final trajectory optimization..."; + map_builder_->pose_graph()->RunFinalOptimization(); +} + +bool MapBuilderBridge::SerializeState(const std::string& filename, + const bool include_unfinished_submaps) { + return map_builder_->SerializeStateToFile(include_unfinished_submaps, + filename); +} + +void MapBuilderBridge::HandleSubmapQuery( + cartographer_ros_msgs::SubmapQuery::Request& request, + cartographer_ros_msgs::SubmapQuery::Response& response) { + cartographer::mapping::proto::SubmapQuery::Response response_proto; + cartographer::mapping::SubmapId submap_id{request.trajectory_id, + request.submap_index}; + const std::string error = + map_builder_->SubmapToProto(submap_id, &response_proto); + if (!error.empty()) { + LOG(ERROR) << error; + response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; + response.status.message = error; + return; + } + + response.submap_version = response_proto.submap_version(); + for (const auto& texture_proto : response_proto.textures()) { + response.textures.emplace_back(); + auto& texture = response.textures.back(); + texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(), + texture_proto.cells().end()); + texture.width = texture_proto.width(); + texture.height = texture_proto.height(); + texture.resolution = texture_proto.resolution(); + texture.slice_pose = ToGeometryMsgPose( + cartographer::transform::ToRigid3(texture_proto.slice_pose())); + } + response.status.message = "Success."; + response.status.code = cartographer_ros_msgs::StatusCode::OK; +} + +std::map +MapBuilderBridge::GetTrajectoryStates() { + auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates(); + // Add active trajectories that are not yet in the pose graph, but are e.g. + // waiting for input sensor data and thus already have a sensor bridge. + for (const auto& sensor_bridge : sensor_bridges_) { + trajectory_states.insert(std::make_pair( + sensor_bridge.first, + ::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE)); + } + return trajectory_states; +} + +cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { + cartographer_ros_msgs::SubmapList submap_list; + submap_list.header.stamp = ::ros::Time::now(); + submap_list.header.frame_id = node_options_.map_frame; + for (const auto& submap_id_pose : + map_builder_->pose_graph()->GetAllSubmapPoses()) { + cartographer_ros_msgs::SubmapEntry submap_entry; + submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen( + submap_id_pose.id.trajectory_id); + submap_entry.trajectory_id = submap_id_pose.id.trajectory_id; + submap_entry.submap_index = submap_id_pose.id.submap_index; + submap_entry.submap_version = submap_id_pose.data.version; + submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose); + submap_list.submap.push_back(submap_entry); + } + return submap_list; +} + +std::unordered_map +MapBuilderBridge::GetLocalTrajectoryData() { + std::unordered_map local_trajectory_data; + for (const auto& entry : sensor_bridges_) { + const int trajectory_id = entry.first; + const SensorBridge& sensor_bridge = *entry.second; + + std::shared_ptr local_slam_data; + { + absl::MutexLock lock(&mutex_); + if (local_slam_data_.count(trajectory_id) == 0) { + continue; + } + local_slam_data = local_slam_data_.at(trajectory_id); + } + + // Make sure there is a trajectory with 'trajectory_id'. + CHECK_EQ(trajectory_options_.count(trajectory_id), 1); + local_trajectory_data[trajectory_id] = { + local_slam_data, + map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id), + sensor_bridge.tf_bridge().LookupToTracking( + local_slam_data->time, + trajectory_options_[trajectory_id].published_frame), + trajectory_options_[trajectory_id]}; + } + return local_trajectory_data; +} + +void MapBuilderBridge::HandleTrajectoryQuery( + cartographer_ros_msgs::TrajectoryQuery::Request& request, + cartographer_ros_msgs::TrajectoryQuery::Response& response) { + // This query is safe if the trajectory doesn't exist (returns 0 poses). + // However, we can filter unwanted states at the higher level in the node. + const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); + for (const auto& node_id_data : + node_poses.trajectory(request.trajectory_id)) { + if (!node_id_data.data.constant_pose_data.has_value()) { + continue; + } + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.frame_id = node_options_.map_frame; + pose_stamped.header.stamp = + ToRos(node_id_data.data.constant_pose_data.value().time); + pose_stamped.pose = ToGeometryMsgPose(node_id_data.data.global_pose); + response.trajectory.push_back(pose_stamped); + } + response.status.code = cartographer_ros_msgs::StatusCode::OK; + response.status.message = absl::StrCat( + "Retrieved ", response.trajectory.size(), + " trajectory nodes from trajectory ", request.trajectory_id, "."); +} + +visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { + visualization_msgs::MarkerArray trajectory_node_list; + const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); + // Find the last node indices for each trajectory that have either + // inter-submap or inter-trajectory constraints. + std::map + trajectory_to_last_inter_submap_constrained_node; + std::map + trajectory_to_last_inter_trajectory_constrained_node; + for (const int trajectory_id : node_poses.trajectory_ids()) { + trajectory_to_last_inter_submap_constrained_node[trajectory_id] = 0; + trajectory_to_last_inter_trajectory_constrained_node[trajectory_id] = 0; + } + const auto constraints = map_builder_->pose_graph()->constraints(); + for (const auto& constraint : constraints) { + if (constraint.tag == + cartographer::mapping::PoseGraphInterface::Constraint::INTER_SUBMAP) { + if (constraint.node_id.trajectory_id == + constraint.submap_id.trajectory_id) { + trajectory_to_last_inter_submap_constrained_node[constraint.node_id + .trajectory_id] = + std::max(trajectory_to_last_inter_submap_constrained_node.at( + constraint.node_id.trajectory_id), + constraint.node_id.node_index); + } else { + trajectory_to_last_inter_trajectory_constrained_node + [constraint.node_id.trajectory_id] = + std::max(trajectory_to_last_inter_submap_constrained_node.at( + constraint.node_id.trajectory_id), + constraint.node_id.node_index); + } + } + } + + for (const int trajectory_id : node_poses.trajectory_ids()) { + visualization_msgs::Marker marker = + CreateTrajectoryMarker(trajectory_id, node_options_.map_frame); + int last_inter_submap_constrained_node = std::max( + node_poses.trajectory(trajectory_id).begin()->id.node_index, + trajectory_to_last_inter_submap_constrained_node.at(trajectory_id)); + int last_inter_trajectory_constrained_node = std::max( + node_poses.trajectory(trajectory_id).begin()->id.node_index, + trajectory_to_last_inter_trajectory_constrained_node.at(trajectory_id)); + last_inter_submap_constrained_node = + std::max(last_inter_submap_constrained_node, + last_inter_trajectory_constrained_node); + + if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) { + last_inter_submap_constrained_node = + (--node_poses.trajectory(trajectory_id).end())->id.node_index; + last_inter_trajectory_constrained_node = + last_inter_submap_constrained_node; + } + + marker.color.a = 1.0; + for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) { + if (!node_id_data.data.constant_pose_data.has_value()) { + PushAndResetLineMarker(&marker, &trajectory_node_list.markers); + continue; + } + const ::geometry_msgs::Point node_point = + ToGeometryMsgPoint(node_id_data.data.global_pose.translation()); + marker.points.push_back(node_point); + + if (node_id_data.id.node_index == + last_inter_trajectory_constrained_node) { + PushAndResetLineMarker(&marker, &trajectory_node_list.markers); + marker.points.push_back(node_point); + + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 165. / 255.; + } + if (node_id_data.id.node_index == last_inter_submap_constrained_node) { + PushAndResetLineMarker(&marker, &trajectory_node_list.markers); + marker.points.push_back(node_point); + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 165. / 255.; + } + // Work around the 16384 point limit in RViz by splitting the + // trajectory into multiple markers. + if (marker.points.size() == 16384) { + PushAndResetLineMarker(&marker, &trajectory_node_list.markers); + // Push back the last point, so the two markers appear connected. + marker.points.push_back(node_point); + } + } + PushAndResetLineMarker(&marker, &trajectory_node_list.markers); + size_t current_last_marker_id = static_cast(marker.id - 1); + if (trajectory_to_highest_marker_id_.count(trajectory_id) == 0) { + trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id; + } else { + marker.action = visualization_msgs::Marker::DELETE; + while (static_cast(marker.id) <= + trajectory_to_highest_marker_id_[trajectory_id]) { + trajectory_node_list.markers.push_back(marker); + ++marker.id; + } + trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id; + } + } + return trajectory_node_list; +} + +visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() { + visualization_msgs::MarkerArray landmark_poses_list; + const std::map landmark_poses = + map_builder_->pose_graph()->GetLandmarkPoses(); + for (const auto& id_to_pose : landmark_poses) { + landmark_poses_list.markers.push_back(CreateLandmarkMarker( + GetLandmarkIndex(id_to_pose.first, &landmark_to_index_), + id_to_pose.second, node_options_.map_frame)); + } + return landmark_poses_list; +} + +visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { + visualization_msgs::MarkerArray constraint_list; + int marker_id = 0; + visualization_msgs::Marker constraint_intra_marker; + constraint_intra_marker.id = marker_id++; + constraint_intra_marker.ns = "Intra constraints"; + constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST; + constraint_intra_marker.header.stamp = ros::Time::now(); + constraint_intra_marker.header.frame_id = node_options_.map_frame; + constraint_intra_marker.scale.x = kConstraintMarkerScale; + constraint_intra_marker.pose.orientation.w = 1.0; + + visualization_msgs::Marker residual_intra_marker = constraint_intra_marker; + residual_intra_marker.id = marker_id++; + residual_intra_marker.ns = "Intra residuals"; + // This and other markers which are less numerous are set to be slightly + // above the intra constraints marker in order to ensure that they are + // visible. + residual_intra_marker.pose.position.z = 0.1; + + visualization_msgs::Marker constraint_inter_same_trajectory_marker = + constraint_intra_marker; + constraint_inter_same_trajectory_marker.id = marker_id++; + constraint_inter_same_trajectory_marker.ns = + "Inter constraints, same trajectory"; + constraint_inter_same_trajectory_marker.pose.position.z = 0.1; + + visualization_msgs::Marker residual_inter_same_trajectory_marker = + constraint_intra_marker; + residual_inter_same_trajectory_marker.id = marker_id++; + residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory"; + residual_inter_same_trajectory_marker.pose.position.z = 0.1; + + visualization_msgs::Marker constraint_inter_diff_trajectory_marker = + constraint_intra_marker; + constraint_inter_diff_trajectory_marker.id = marker_id++; + constraint_inter_diff_trajectory_marker.ns = + "Inter constraints, different trajectories"; + constraint_inter_diff_trajectory_marker.pose.position.z = 0.1; + + visualization_msgs::Marker residual_inter_diff_trajectory_marker = + constraint_intra_marker; + residual_inter_diff_trajectory_marker.id = marker_id++; + residual_inter_diff_trajectory_marker.ns = + "Inter residuals, different trajectories"; + residual_inter_diff_trajectory_marker.pose.position.z = 0.1; + + const auto trajectory_node_poses = + map_builder_->pose_graph()->GetTrajectoryNodePoses(); + const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses(); + const auto constraints = map_builder_->pose_graph()->constraints(); + + for (const auto& constraint : constraints) { + visualization_msgs::Marker *constraint_marker, *residual_marker; + std_msgs::ColorRGBA color_constraint, color_residual; + if (constraint.tag == + cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP) { + constraint_marker = &constraint_intra_marker; + residual_marker = &residual_intra_marker; + // Color mapping for submaps of various trajectories - add trajectory id + // to ensure different starting colors. Also add a fixed offset of 25 + // to avoid having identical colors as trajectories. + color_constraint = ToMessage( + cartographer::io::GetColor( 25)); + color_residual.a = 1.0; + color_residual.r = 1.0; + } else { + if (constraint.node_id.trajectory_id == + constraint.submap_id.trajectory_id) { + constraint_marker = &constraint_inter_same_trajectory_marker; + residual_marker = &residual_inter_same_trajectory_marker; + // Bright yellow + color_constraint.a = 1.0; + color_constraint.r = color_constraint.g = 1.0; + } else { + constraint_marker = &constraint_inter_diff_trajectory_marker; + residual_marker = &residual_inter_diff_trajectory_marker; + // Bright orange + color_constraint.a = 1.0; + color_constraint.r = 1.0; + color_constraint.g = 165. / 255.; + } + // Bright cyan + color_residual.a = 1.0; + color_residual.b = color_residual.g = 1.0; + } + + for (int i = 0; i < 2; ++i) { + constraint_marker->colors.push_back(color_constraint); + residual_marker->colors.push_back(color_residual); + } + + const auto submap_it = submap_poses.find(constraint.submap_id); + if (submap_it == submap_poses.end()) { + continue; + } + const auto& submap_pose = submap_it->data.pose; + const auto node_it = trajectory_node_poses.find(constraint.node_id); + if (node_it == trajectory_node_poses.end()) { + continue; + } + const auto& trajectory_node_pose = node_it->data.global_pose; + const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij; + + constraint_marker->points.push_back( + ToGeometryMsgPoint(submap_pose.translation())); + constraint_marker->points.push_back( + ToGeometryMsgPoint(constraint_pose.translation())); + + residual_marker->points.push_back( + ToGeometryMsgPoint(constraint_pose.translation())); + residual_marker->points.push_back( + ToGeometryMsgPoint(trajectory_node_pose.translation())); + } + + constraint_list.markers.push_back(constraint_intra_marker); + constraint_list.markers.push_back(residual_intra_marker); + constraint_list.markers.push_back(constraint_inter_same_trajectory_marker); + constraint_list.markers.push_back(residual_inter_same_trajectory_marker); + constraint_list.markers.push_back(constraint_inter_diff_trajectory_marker); + constraint_list.markers.push_back(residual_inter_diff_trajectory_marker); + return constraint_list; +} + +SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { + return sensor_bridges_.at(trajectory_id).get(); +} + +void MapBuilderBridge::OnLocalSlamResult( + const int trajectory_id, const ::cartographer::common::Time time, + const Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local) { + std::shared_ptr local_slam_data = + std::make_shared( + LocalTrajectoryData::LocalSlamData{time, local_pose, + std::move(range_data_in_local)}); + absl::MutexLock lock(&mutex_); + local_slam_data_[trajectory_id] = std::move(local_slam_data); +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/map_builder_bridge.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/map_builder_bridge.h new file mode 100644 index 0000000..1a54e65 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/map_builder_bridge.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. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H + +#include +#include +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer_ros/node_options.h" +#include "cartographer_ros/sensor_bridge.h" +#include "cartographer_ros/tf_bridge.h" +#include "cartographer_ros/trajectory_options.h" +#include "cartographer_ros_msgs/SubmapEntry.h" +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/TrajectoryQuery.h" +#include "geometry_msgs/TransformStamped.h" +#include "nav_msgs/OccupancyGrid.h" + +// Abseil unfortunately pulls in winnt.h, which #defines DELETE. +// Clean up to unbreak visualization_msgs::Marker::DELETE. +#ifdef DELETE +#undef DELETE +#endif +#include "visualization_msgs/MarkerArray.h" + +namespace cartographer_ros { + +class MapBuilderBridge { + public: + struct LocalTrajectoryData { + // Contains the trajectory data received from local SLAM, after + // it had processed accumulated 'range_data_in_local' and estimated + // current 'local_pose' at 'time'. + struct LocalSlamData { + ::cartographer::common::Time time; + ::cartographer::transform::Rigid3d local_pose; + ::cartographer::sensor::RangeData range_data_in_local; + }; + std::shared_ptr local_slam_data; + cartographer::transform::Rigid3d local_to_map; + std::unique_ptr published_to_tracking; + TrajectoryOptions trajectory_options; + }; + + MapBuilderBridge( + const NodeOptions& node_options, + std::unique_ptr map_builder, + tf2_ros::Buffer* tf_buffer); + + MapBuilderBridge(const MapBuilderBridge&) = delete; + MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; + + void LoadState(const std::string& state_filename, bool load_frozen_state); + int AddTrajectory( + const std::set< + ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>& + expected_sensor_ids, + const TrajectoryOptions& trajectory_options); + void FinishTrajectory(int trajectory_id); + void RunFinalOptimization(); + bool SerializeState(const std::string& filename, + const bool include_unfinished_submaps); + + void HandleSubmapQuery( + cartographer_ros_msgs::SubmapQuery::Request& request, + cartographer_ros_msgs::SubmapQuery::Response& response); + void HandleTrajectoryQuery( + cartographer_ros_msgs::TrajectoryQuery::Request& request, + cartographer_ros_msgs::TrajectoryQuery::Response& response); + + std::map + GetTrajectoryStates(); + cartographer_ros_msgs::SubmapList GetSubmapList(); + std::unordered_map GetLocalTrajectoryData() + LOCKS_EXCLUDED(mutex_); + visualization_msgs::MarkerArray GetTrajectoryNodeList(); + visualization_msgs::MarkerArray GetLandmarkPosesList(); + visualization_msgs::MarkerArray GetConstraintList(); + + SensorBridge* sensor_bridge(int trajectory_id); + + private: + void OnLocalSlamResult(const int trajectory_id, + const ::cartographer::common::Time time, + const ::cartographer::transform::Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local) + LOCKS_EXCLUDED(mutex_); + + absl::Mutex mutex_; + const NodeOptions node_options_; + std::unordered_map> + local_slam_data_ GUARDED_BY(mutex_); + std::unique_ptr map_builder_; + tf2_ros::Buffer* const tf_buffer_; + + std::unordered_map landmark_to_index_; + + // These are keyed with 'trajectory_id'. + std::unordered_map trajectory_options_; + std::unordered_map> sensor_bridges_; + std::unordered_map trajectory_to_highest_marker_id_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/family_factory.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/family_factory.cc new file mode 100644 index 0000000..96df381 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/family_factory.cc @@ -0,0 +1,69 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/metrics/family_factory.h" + +#include "absl/memory/memory.h" + +namespace cartographer_ros { +namespace metrics { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +::cartographer::metrics::Family<::cartographer::metrics::Counter>* +FamilyFactory::NewCounterFamily(const std::string& name, + const std::string& description) { + auto wrapper = absl::make_unique(name, description); + auto* ptr = wrapper.get(); + counter_families_.emplace_back(std::move(wrapper)); + return ptr; +} + +::cartographer::metrics::Family<::cartographer::metrics::Gauge>* +FamilyFactory::NewGaugeFamily(const std::string& name, + const std::string& description) { + auto wrapper = absl::make_unique(name, description); + auto* ptr = wrapper.get(); + gauge_families_.emplace_back(std::move(wrapper)); + return ptr; +} + +::cartographer::metrics::Family<::cartographer::metrics::Histogram>* +FamilyFactory::NewHistogramFamily(const std::string& name, + const std::string& description, + const BucketBoundaries& boundaries) { + auto wrapper = + absl::make_unique(name, description, boundaries); + auto* ptr = wrapper.get(); + histogram_families_.emplace_back(std::move(wrapper)); + return ptr; +} + +void FamilyFactory::ReadMetrics( + ::cartographer_ros_msgs::ReadMetrics::Response* response) const { + for (const auto& counter_family : counter_families_) { + response->metric_families.push_back(counter_family->ToRosMessage()); + } + for (const auto& gauge_family : gauge_families_) { + response->metric_families.push_back(gauge_family->ToRosMessage()); + } + for (const auto& histogram_family : histogram_families_) { + response->metric_families.push_back(histogram_family->ToRosMessage()); + } +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/family_factory.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/family_factory.h new file mode 100644 index 0000000..a05fbd5 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/family_factory.h @@ -0,0 +1,61 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H +#define CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H + +#include +#include + +#include "cartographer/metrics/family_factory.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/family.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" +#include "cartographer_ros_msgs/ReadMetrics.h" + +namespace cartographer_ros { +namespace metrics { + +// Realizes the factory / registry interface for the metrics in libcartographer +// and provides a wrapper to collect ROS messages from the metrics it owns. +class FamilyFactory : public ::cartographer::metrics::FamilyFactory { + public: + ::cartographer::metrics::Family<::cartographer::metrics::Counter>* + + NewCounterFamily(const std::string& name, + const std::string& description) override; + ::cartographer::metrics::Family<::cartographer::metrics::Gauge>* + NewGaugeFamily(const std::string& name, + const std::string& description) override; + ::cartographer::metrics::Family<::cartographer::metrics::Histogram>* + NewHistogramFamily(const std::string& name, const std::string& description, + const ::cartographer::metrics::Histogram::BucketBoundaries& + boundaries) override; + + void ReadMetrics( + ::cartographer_ros_msgs::ReadMetrics::Response* response) const; + + private: + std::vector> counter_families_; + std::vector> gauge_families_; + std::vector> histogram_families_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/counter.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/counter.h new file mode 100644 index 0000000..52f35fe --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/counter.h @@ -0,0 +1,51 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H + +#include "cartographer/metrics/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros_msgs/Metric.h" + +namespace cartographer_ros { +namespace metrics { + +class Counter : public ::cartographer::metrics::Counter { + public: + explicit Counter(const std::map& labels) + : gauge_(labels) {} + + void Increment(const double value) override { gauge_.Increment(value); } + + void Increment() override { gauge_.Increment(); } + + double Value() { return gauge_.Value(); } + + cartographer_ros_msgs::Metric ToRosMessage() { + cartographer_ros_msgs::Metric msg = gauge_.ToRosMessage(); + msg.type = cartographer_ros_msgs::Metric::TYPE_COUNTER; + return msg; + } + + private: + Gauge gauge_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/family.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/family.cc new file mode 100644 index 0000000..42f81c6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/family.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/metrics/internal/family.h" + +#include "absl/memory/memory.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" + +namespace cartographer_ros { +namespace metrics { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +Counter* CounterFamily::Add(const std::map& labels) { + auto wrapper = absl::make_unique(labels); + auto* ptr = wrapper.get(); + wrappers_.emplace_back(std::move(wrapper)); + return ptr; +} + +cartographer_ros_msgs::MetricFamily CounterFamily::ToRosMessage() { + cartographer_ros_msgs::MetricFamily family_msg; + family_msg.name = name_; + family_msg.description = description_; + for (const auto& wrapper : wrappers_) { + family_msg.metrics.push_back(wrapper->ToRosMessage()); + } + return family_msg; +} + +Gauge* GaugeFamily::Add(const std::map& labels) { + auto wrapper = absl::make_unique(labels); + auto* ptr = wrapper.get(); + wrappers_.emplace_back(std::move(wrapper)); + return ptr; +} + +cartographer_ros_msgs::MetricFamily GaugeFamily::ToRosMessage() { + cartographer_ros_msgs::MetricFamily family_msg; + family_msg.name = name_; + family_msg.description = description_; + for (const auto& wrapper : wrappers_) { + family_msg.metrics.push_back(wrapper->ToRosMessage()); + } + return family_msg; +} + +Histogram* HistogramFamily::Add( + const std::map& labels) { + auto wrapper = absl::make_unique(labels, boundaries_); + auto* ptr = wrapper.get(); + wrappers_.emplace_back(std::move(wrapper)); + return ptr; +} + +cartographer_ros_msgs::MetricFamily HistogramFamily::ToRosMessage() { + cartographer_ros_msgs::MetricFamily family_msg; + family_msg.name = name_; + family_msg.description = description_; + for (const auto& wrapper : wrappers_) { + family_msg.metrics.push_back(wrapper->ToRosMessage()); + } + return family_msg; +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/family.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/family.h new file mode 100644 index 0000000..7e21f58 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/family.h @@ -0,0 +1,81 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H + +#include +#include + +#include "cartographer/metrics/family_factory.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" +#include "cartographer_ros_msgs/MetricFamily.h" + +namespace cartographer_ros { +namespace metrics { +class CounterFamily + : public ::cartographer::metrics::Family<::cartographer::metrics::Counter> { + public: + CounterFamily(const std::string& name, const std::string& description) + : name_(name), description_(description) {} + Counter* Add(const std::map& labels) override; + cartographer_ros_msgs::MetricFamily ToRosMessage(); + + private: + std::string name_; + std::string description_; + std::vector> wrappers_; +}; + +class GaugeFamily + : public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> { + public: + GaugeFamily(const std::string& name, const std::string& description) + : name_(name), description_(description) {} + Gauge* Add(const std::map& labels) override; + + cartographer_ros_msgs::MetricFamily ToRosMessage(); + + private: + std::string name_; + std::string description_; + std::vector> wrappers_; +}; + +class HistogramFamily : public ::cartographer::metrics::Family< + ::cartographer::metrics::Histogram> { + public: + HistogramFamily(const std::string& name, const std::string& description, + const BucketBoundaries& boundaries) + : name_(name), description_(description), boundaries_(boundaries) {} + + Histogram* Add(const std::map& labels) override; + + cartographer_ros_msgs::MetricFamily ToRosMessage(); + + private: + std::string name_; + std::string description_; + std::vector> wrappers_; + const BucketBoundaries boundaries_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/gauge.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/gauge.h new file mode 100644 index 0000000..fc0a693 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/gauge.h @@ -0,0 +1,80 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H + +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer_ros_msgs/Metric.h" + +namespace cartographer_ros { +namespace metrics { + +class Gauge : public ::cartographer::metrics::Gauge { + public: + explicit Gauge(const std::map& labels) + : labels_(labels), value_(0.) {} + + void Decrement(const double value) override { Add(-1. * value); } + + void Decrement() override { Decrement(1.); } + + void Increment(const double value) override { Add(value); } + + void Increment() override { Increment(1.); } + + void Set(double value) override { + absl::MutexLock lock(&mutex_); + value_ = value; + } + + double Value() { + absl::MutexLock lock(&mutex_); + return value_; + } + + cartographer_ros_msgs::Metric ToRosMessage() { + cartographer_ros_msgs::Metric msg; + msg.type = cartographer_ros_msgs::Metric::TYPE_GAUGE; + for (const auto& label : labels_) { + cartographer_ros_msgs::MetricLabel label_msg; + label_msg.key = label.first; + label_msg.value = label.second; + msg.labels.push_back(label_msg); + } + msg.value = Value(); + return msg; + } + + private: + void Add(const double value) { + absl::MutexLock lock(&mutex_); + value_ += value; + } + + absl::Mutex mutex_; + const std::map labels_; + double value_ GUARDED_BY(mutex_); +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc new file mode 100644 index 0000000..27646cc --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc @@ -0,0 +1,90 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/metrics/internal/histogram.h" + +#include +#include + +#include "glog/logging.h" + +namespace cartographer_ros { +namespace metrics { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +Histogram::Histogram(const std::map& labels, + const BucketBoundaries& bucket_boundaries) + : labels_(labels), + bucket_boundaries_(bucket_boundaries), + bucket_counts_(bucket_boundaries.size() + 1) { + absl::MutexLock lock(&mutex_); + CHECK(std::is_sorted(std::begin(bucket_boundaries_), + std::end(bucket_boundaries_))); +} + +void Histogram::Observe(double value) { + auto bucket_index = + std::distance(bucket_boundaries_.begin(), + std::upper_bound(bucket_boundaries_.begin(), + bucket_boundaries_.end(), value)); + absl::MutexLock lock(&mutex_); + sum_ += value; + bucket_counts_[bucket_index] += 1; +} + +std::map Histogram::CountsByBucket() { + absl::MutexLock lock(&mutex_); + std::map counts_by_bucket; + // Add the finite buckets. + for (size_t i = 0; i < bucket_boundaries_.size(); ++i) { + counts_by_bucket[bucket_boundaries_.at(i)] = bucket_counts_.at(i); + } + // Add the "infinite" bucket. + counts_by_bucket[kInfiniteBoundary] = bucket_counts_.back(); + return counts_by_bucket; +} + +double Histogram::Sum() { + absl::MutexLock lock(&mutex_); + return sum_; +} + +double Histogram::CumulativeCount() { + absl::MutexLock lock(&mutex_); + return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.); +} + +cartographer_ros_msgs::Metric Histogram::ToRosMessage() { + cartographer_ros_msgs::Metric msg; + msg.type = cartographer_ros_msgs::Metric::TYPE_HISTOGRAM; + for (const auto& label : labels_) { + cartographer_ros_msgs::MetricLabel label_msg; + label_msg.key = label.first; + label_msg.value = label.second; + msg.labels.push_back(label_msg); + } + for (const auto& bucket : CountsByBucket()) { + cartographer_ros_msgs::HistogramBucket bucket_msg; + bucket_msg.bucket_boundary = bucket.first; + bucket_msg.count = bucket.second; + msg.counts_by_bucket.push_back(bucket_msg); + } + return msg; +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/histogram.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/histogram.h new file mode 100644 index 0000000..ec68815 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/histogram.h @@ -0,0 +1,60 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H + +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/metrics/histogram.h" +#include "cartographer_ros_msgs/Metric.h" + +namespace cartographer_ros { +namespace metrics { + +constexpr double kInfiniteBoundary = std::numeric_limits::infinity(); + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +class Histogram : public ::cartographer::metrics::Histogram { + public: + explicit Histogram(const std::map& labels, + const BucketBoundaries& bucket_boundaries); + + void Observe(double value) override; + + std::map CountsByBucket(); + + double Sum(); + + double CumulativeCount(); + + cartographer_ros_msgs::Metric ToRosMessage(); + + private: + absl::Mutex mutex_; + const std::map labels_; + const BucketBoundaries bucket_boundaries_; + std::vector bucket_counts_ GUARDED_BY(mutex_); + double sum_ GUARDED_BY(mutex_); +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc new file mode 100644 index 0000000..d0c8681 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc @@ -0,0 +1,104 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "cartographer/metrics/histogram.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" +#include "gtest/gtest.h" + +namespace cartographer_ros { +namespace metrics { + +TEST(Metrics, GaugeTest) { + Gauge gauge({}); + EXPECT_EQ(gauge.Value(), 0.); + gauge.Increment(1.2); + EXPECT_EQ(gauge.Value(), 1.2); + gauge.Increment(); + EXPECT_EQ(gauge.Value(), 2.2); + gauge.Decrement(2.2); + EXPECT_EQ(gauge.Value(), 0.); + gauge.Decrement(); + EXPECT_EQ(gauge.Value(), -1.); +} + +TEST(Metrics, CounterTest) { + Counter counter({}); + EXPECT_EQ(counter.Value(), 0.); + counter.Increment(1.2); + EXPECT_EQ(counter.Value(), 1.2); + counter.Increment(0.8); + EXPECT_EQ(counter.Value(), 2.); + counter.Increment(); + EXPECT_EQ(counter.Value(), 3.); +} + +TEST(Metrics, HistogramFixedWidthTest) { + auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(1, 3); + Histogram histogram({}, boundaries); + + // Observe some values that fit in finite buckets. + std::array values = {{0., 2, 2.5}}; + for (const auto& value : values) { + histogram.Observe(value); + } + // 1 2 3 inf + // 1 | 0 | 2 | 0 | + EXPECT_EQ(histogram.CountsByBucket()[1], 1); + EXPECT_EQ(histogram.CountsByBucket()[2], 0); + EXPECT_EQ(histogram.CountsByBucket()[3], 2); + EXPECT_EQ(histogram.CumulativeCount(), values.size()); + EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.)); + + // Values above the last bucket boundary should go to the "infinite" bucket. + histogram.Observe(3.5); + // 1 2 3 inf + // 1 | 0 | 2 | 1 | + EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1); +} + +TEST(Metrics, HistogramScaledPowersOfTest) { + auto boundaries = + ::cartographer::metrics::Histogram::ScaledPowersOf(2, 1, 2048); + Histogram histogram({}, boundaries); + + // Observe some values that fit in finite buckets. + std::array values = {{256, 512, 666}}; + for (const auto& value : values) { + histogram.Observe(value); + } + // ... 256 512 1024 inf + // ... | 1 | 2 | 0 | + EXPECT_EQ(histogram.CountsByBucket()[256], 0); + EXPECT_EQ(histogram.CountsByBucket()[512], 1); + EXPECT_EQ(histogram.CountsByBucket()[1024], 2); + EXPECT_EQ(histogram.CumulativeCount(), values.size()); + EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.)); + + // Values above the last bucket boundary should go to the "infinite" bucket. + histogram.Observe(2048); + // ... 256 512 1024 inf + // ... | 1 | 2 | 1 | + EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1); +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/msg_conversion.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/msg_conversion.cc new file mode 100644 index 0000000..e0eb4a5 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -0,0 +1,633 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/msg_conversion.h" + +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/time_conversion.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/TransformStamped.h" +#include "geometry_msgs/Vector3.h" +#include "glog/logging.h" +#include "nav_msgs/OccupancyGrid.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "ros/ros.h" +#include "ros/serialization.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" +#include + +namespace { + +// Sizes of PCL point types have to be 4n floats for alignment, as described in +// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php +// 自定义的pcl点云格式 +struct PointXYZT { + float x; + float y; + float z; + float time; +}; + +struct PointXYZIT { + PCL_ADD_POINT4D; + float intensity; + float time; + float unused_padding[2]; +}; + +} // namespace + +// 将自定义的点云格式在pcl中注册 +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, + intensity)(float, time, time)) + +namespace cartographer_ros { +namespace { + +// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each +// point. The last one must be this value or RViz is not showing the point cloud +// properly. +constexpr float kPointCloudComponentFourMagic = 1.; + +using ::cartographer::sensor::LandmarkData; +using ::cartographer::sensor::LandmarkObservation; +using ::cartographer::sensor::PointCloudWithIntensities; +using ::cartographer::transform::Rigid3d; +using ::cartographer_ros_msgs::LandmarkEntry; +using ::cartographer_ros_msgs::LandmarkList; + +/** + * @brief 点云格式的设置与数组的初始化 + * + * @param[in] timestamp 时间戳 + * @param[in] frame_id 坐标系 + * @param[in] num_points 点云的个数 + * @return sensor_msgs::PointCloud2 + */ +sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, + const std::string& frame_id, + const int num_points) { + sensor_msgs::PointCloud2 msg; + msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); + msg.header.frame_id = frame_id; + msg.height = 1; + msg.width = num_points; + msg.fields.resize(3); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.is_bigendian = false; + msg.point_step = 16; + msg.row_step = 16 * msg.width; + msg.is_dense = true; + msg.data.resize(16 * num_points); + return msg; +} + +// For sensor_msgs::LaserScan. +bool HasEcho(float) { return true; } + +float GetFirstEcho(float range) { return range; } + +// For sensor_msgs::MultiEchoLaserScan. +bool HasEcho(const sensor_msgs::LaserEcho& echo) { + return !echo.echoes.empty(); +} + +// 通过函数重载, 使得函数可以同时适用LaserScan与LaserEcho +float GetFirstEcho(const sensor_msgs::LaserEcho& echo) { + return echo.echoes[0]; +} + +// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan. +// 将LaserScan与MultiEchoLaserScan转成carto格式的点云数据 +template +std::tuple +LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { + CHECK_GE(msg.range_min, 0.f); + CHECK_GE(msg.range_max, msg.range_min); + if (msg.angle_increment > 0.f) { + CHECK_GT(msg.angle_max, msg.angle_min); + } else { + CHECK_GT(msg.angle_min, msg.angle_max); + } + + PointCloudWithIntensities point_cloud; + float angle = msg.angle_min; + static double timestamp_current = 0 ; + static double timestamp_last = 0 ; + double time_increment_vary; + timestamp_last = timestamp_current; + timestamp_current = msg.header.stamp.toSec(); + if(timestamp_last != 0) { + time_increment_vary = (timestamp_current - timestamp_last)/(6.2831852/msg.angle_increment); + } + else time_increment_vary = msg.time_increment; + + + for (size_t i = 0; i < msg.ranges.size(); ++i) { + // c++11: 使用auto可以适应不同的数据类型 + const auto& echoes = msg.ranges[i]; + if (HasEcho(echoes)) { + + const float first_echo = GetFirstEcho(echoes); + // 满足范围才进行使用 + if (msg.range_min <= first_echo && first_echo <= msg.range_max) { + const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); + const cartographer::sensor::TimedRangefinderPoint point{ + rotation * (first_echo * Eigen::Vector3f::UnitX()), // position + i * msg.time_increment}; // time + // 保存点云坐标与时间信息 + point_cloud.points.push_back(point); + //point_cloud.intensities.push_back(127); + // 如果存在强度信息 + + if (msg.intensities.size() > 0) { + CHECK_EQ(msg.intensities.size(), msg.ranges.size()); + // 使用auto可以适应不同的数据类型 + const auto& echo_intensities = msg.intensities[i]; + CHECK(HasEcho(echo_intensities)); + point_cloud.intensities.push_back(GetFirstEcho(echo_intensities)); + //point_cloud.intensities.push_back(msg.intensities[i]); + } else { + + point_cloud.intensities.push_back(0.f); + } + + } + } + angle += msg.angle_increment; + } + + ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); + if (!point_cloud.points.empty()) { + const double duration = point_cloud.points.back().time; + // 以点云最后一个点的时间为点云的时间戳 + timestamp += cartographer::common::FromSeconds(duration); + + // 让点云的时间变成相对值, 最后一个点的时间为0 + for (auto& point : point_cloud.points) { + point.time -= duration; + } + } + return std::make_tuple(point_cloud, timestamp); +} + +// 检查点云是否存在 field_name 字段 +bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, + const std::string& field_name) { + for (const auto& field : pc2.fields) { + if (field.name == field_name) { + return true; + } + } + return false; +} + +} // namespace + +/** + * @brief 将cartographer格式的点云数据 转换成 ROS格式的点云数据 + * + * @param[in] timestamp 时间戳 + * @param[in] frame_id 坐标系 + * @param[in] point_cloud 地图坐标系下的点云数据 + * @return sensor_msgs::PointCloud2 ROS格式的点云数据 + */ +sensor_msgs::PointCloud2 ToPointCloud2Message( + const int64_t timestamp, const std::string& frame_id, + const ::cartographer::sensor::TimedPointCloud& point_cloud) { + // 点云格式的设置与数组的初始化 + auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); + + // note: 通过ros::serialization将msg放进内存中 + ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); + for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) { + // 通过使用next()函数,将点的坐标 序列化 到stream输出流, 将point存入msg + stream.next(point.position.x()); + stream.next(point.position.y()); + stream.next(point.position.z()); + stream.next(kPointCloudComponentFourMagic); // kPointCloudComponentFourMagic = 1 + } + return msg; +} + +// 由ros格式的LaserScan转成carto格式的PointCloudWithIntensities +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) { + return LaserScanToPointCloudWithIntensities(msg); +} + +// 由ros格式的MultiEchoLaserScan转成carto格式的PointCloudWithIntensities +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) { + return LaserScanToPointCloudWithIntensities(msg); +} + +// 由ros格式的PointCloud2转成carto格式的PointCloudWithIntensities +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { + PointCloudWithIntensities point_cloud; + // We check for intensity field here to avoid run-time warnings if we pass in + // a PointCloud2 without intensity. + + // 有强度数据 + if (PointCloud2HasField(msg, "intensity")) { + + // 有强度字段, 有时间字段 + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); + point_cloud.intensities.push_back(point.intensity); + } + } + // 有强度字段, 没时间字段 + else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); // 没有时间信息就把时间填0 + point_cloud.intensities.push_back(point.intensity); + } + } + } + // 没有强度数据 + else { + // If we don't have an intensity field, just copy XYZ and fill in 1.0f. + // 没强度字段, 有时间字段 + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); + point_cloud.intensities.push_back(1.0f); + } + } + // 没强度字段, 没时间字段 + else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); // 没有时间信息就把时间填0 + point_cloud.intensities.push_back(1.0f); + } + } + } + + ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); + if (!point_cloud.points.empty()) { + const double duration = point_cloud.points.back().time; + // 点云开始的时间 加上 第一个点到最后一个点的时间 + // 点云最后一个点的时间 作为整个点云的时间戳 + timestamp += cartographer::common::FromSeconds(duration); + + for (auto& point : point_cloud.points) { + // 将每个点的时间减去整个点云的时间, 所以每个点的时间都应该小于0 + point.time -= duration; + + // 对每个点进行时间检查, 看是否有数据点的时间大于0, 大于0就报错 + CHECK_LE(point.time, 0.f) + << "Encountered a point with a larger stamp than " + "the last point in the cloud."; + } + } + + return std::make_tuple(point_cloud, timestamp); +} + +// 将在ros中自定义的LandmarkList类型的数据, 转成LandmarkData +LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { + LandmarkData landmark_data; + landmark_data.time = FromRos(landmark_list.header.stamp); + for (const LandmarkEntry& entry : landmark_list.landmarks) { + landmark_data.landmark_observations.push_back( + {entry.id, ToRigid3d(entry.tracking_from_landmark_transform), + entry.translation_weight, entry.rotation_weight}); + } + return landmark_data; +} + +Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { + return Rigid3d(ToEigen(transform.transform.translation), + ToEigen(transform.transform.rotation)); +} + +Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) { + return Rigid3d({pose.position.x, pose.position.y, pose.position.z}, + ToEigen(pose.orientation)); +} + +Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3) { + return Eigen::Vector3d(vector3.x, vector3.y, vector3.z); +} + +Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion) { + return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y, + quaternion.z); +} + +geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { + geometry_msgs::Transform transform; + transform.translation.x = rigid3d.translation().x(); + transform.translation.y = rigid3d.translation().y(); + transform.translation.z = rigid3d.translation().z(); + transform.rotation.w = rigid3d.rotation().w(); + transform.rotation.x = rigid3d.rotation().x(); + transform.rotation.y = rigid3d.rotation().y(); + transform.rotation.z = rigid3d.rotation().z(); + return transform; +} + +/** + * @brief 将cartographer的Rigid3d位姿格式,转换成ROS的 geometry_msgs::Pose 格式 + * + * @param[in] rigid3d Rigid3d格式的位姿 + * @return geometry_msgs::Pose ROS格式的位姿 + */ +geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { + geometry_msgs::Pose pose; + pose.position = ToGeometryMsgPoint(rigid3d.translation()); + pose.orientation.w = rigid3d.rotation().w(); + pose.orientation.x = rigid3d.rotation().x(); + pose.orientation.y = rigid3d.rotation().y(); + pose.orientation.z = rigid3d.rotation().z(); + return pose; +} + +geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { + geometry_msgs::Point point; + point.x = vector3d.x(); + point.y = vector3d.y(); + point.z = vector3d.z(); + return point; +} + +// 将经纬度数据转换成ecef坐标系下的坐标 +Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude, + const double altitude) { + // note: 地固坐标系(Earth-Fixed Coordinate System)也称地球坐标系, + // 是固定在地球上与地球一起旋转的坐标系. + // 如果忽略地球潮汐和板块运动, 地面上点的坐标值在地固坐标系中是固定不变的. + + // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates + + constexpr double a = 6378137.; // semi-major axis, equator to center. + constexpr double f = 1. / 298.257223563; + constexpr double b = a * (1. - f); // semi-minor axis, pole to center. + constexpr double a_squared = a * a; + constexpr double b_squared = b * b; + constexpr double e_squared = (a_squared - b_squared) / a_squared; + const double sin_phi = std::sin(cartographer::common::DegToRad(latitude)); + const double cos_phi = std::cos(cartographer::common::DegToRad(latitude)); + const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude)); + const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude)); + const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi); + const double x = (N + altitude) * cos_phi * cos_lambda; + const double y = (N + altitude) * cos_phi * sin_lambda; + const double z = (b_squared / a_squared * N + altitude) * sin_phi; + + return Eigen::Vector3d(x, y, z); +} + +/** + * @brief 计算第一帧GPS数据指向ECEF坐标系下原点的坐标变换, 用这个坐标变换乘以之后的GPS数据 + * 就得到了之后的GPS数据相对于第一帧GPS数据的相对坐标变换 + * + * @param[in] latitude 维度数据 + * @param[in] longitude 经度数据 + * @return cartographer::transform::Rigid3d 计算第一帧GPS数据指向ECEF坐标系下原点的坐标变换 + */ +cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( + const double latitude, const double longitude) { + const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.); + const Eigen::Quaterniond rotation = + Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.), + Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude), + Eigen::Vector3d::UnitZ()); + return cartographer::transform::Rigid3d(rotation * -translation, rotation); +} + +/** + * @brief 由cairo的图像生成ros格式的地图 + * + * @param[in] painted_slices + * @param[in] resolution 栅格地图的分辨率 + * @param[in] frame_id 栅格地图的坐标系 + * @param[in] time 地图对应的时间 + * @return std::unique_ptr ros格式的栅格地图 + */ +std::unique_ptr CreateOccupancyGridMsg( + //,,,const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const ros::Time& time) { + auto occupancy_grid = absl::make_unique(); + //std::cout <<"value start"<header.stamp = time; + occupancy_grid->header.frame_id = frame_id; + occupancy_grid->info.map_load_time = time; + occupancy_grid->info.resolution = resolution; + occupancy_grid->info.width = width; + occupancy_grid->info.height = height; + occupancy_grid->info.origin.position.x = + -painted_slices.origin.x() * resolution; + occupancy_grid->info.origin.position.y = + (-height + painted_slices.origin.y()) * resolution; + occupancy_grid->info.origin.position.z = 0.; + occupancy_grid->info.origin.orientation.w = 1.; + occupancy_grid->info.origin.orientation.x = 0.; + occupancy_grid->info.origin.orientation.y = 0.; + occupancy_grid->info.origin.orientation.z = 0.; + + // 获取 uint32_t* 格式的地图数据 + //reinterpret_cast + const uint32_t* pixel_data = reinterpret_cast( + cairo_image_surface_get_data(painted_slices.surface.get())); + + occupancy_grid->data.reserve(width * height); + for (int y = height - 1; y >= 0; --y) { + for (int x = 0; x < width; ++x) { + const uint32_t packed = pixel_data[y * width + x]; + //if (packed!=(0xffffa500)&& packed!=(0xffffff00)) + //std::cout <> 24; + //if(co!=255) + //std::cout <> 16; + // 根据packed获取这个栅格是否被更新过 + const unsigned char observed = packed >> 8; + // 。。。const unsigned char color = packed; + const unsigned char sem =(uint8_t)(packed & 0xFFu); + // source code + // 。。。根据像素值确定栅格占用值 + int value = + observed == 0 + ? -1 + : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); + + /* note: 生成ROS兼容的栅格地图 + * 像素值65-100的设置占用值为100,表示占用,代表障碍物 + * 像素值0-19.6的设置占用值为0,表示空闲,代表可通过区域 + * 像素值在中间的值保持不变,灰色 + */ + //if (color!=255) + //std::cout << int (color) << " "; + // int value = -1; + // if (observed != 0) + // { + // //if (color!=255) + // //std::cout << int (color) << "w "; + // int value_temp = ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); + // //std::cout << value_temp << ""; + // if (250) + // { + // value_temp =sem; + // if (sem!=120) + // std::cout << int (sem) << ""; + // } + // else + // value_temp=100; + // } + // else if (0<=value_temp<25) + // value_temp = 0; + // value = value_temp; + // } + + //如果value>65,将color赋给删去const的value + CHECK_LE(-1,value); + CHECK_GE(100,value); + occupancy_grid->data.push_back(value); + + + } + } + //std::cout <<"value end"< ros格式的栅格地图 + */ +std::unique_ptr CreateOccupancyGridMsg( + //,,,const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const int* pixel_data_color, + const double resolution, const std::string& frame_id, + const ros::Time& time) { + auto occupancy_grid = absl::make_unique(); + //std::cout <<"!!!!!!!!!!!!!"<header.stamp = time; + occupancy_grid->header.frame_id = frame_id; + occupancy_grid->info.map_load_time = time; + occupancy_grid->info.resolution = resolution; + occupancy_grid->info.width = width; + occupancy_grid->info.height = height; + occupancy_grid->info.origin.position.x = + -1000 * resolution; + occupancy_grid->info.origin.position.y = + (-1000) * resolution; + //std::cout<<(occupancy_grid->info.origin.position.x) <<"x1 "<< occupancy_grid->info.origin.position.y<<"y1 "<info.origin.position.z = 0.; + occupancy_grid->info.origin.orientation.w = 1.; + occupancy_grid->info.origin.orientation.x = 0.; + occupancy_grid->info.origin.orientation.y = 0.; + occupancy_grid->info.origin.orientation.z = 0.; + + // 获取 uint32_t* 格式的地图数据 + //reinterpret_cast + const uint32_t* pixel_data = reinterpret_cast( + cairo_image_surface_get_data(painted_slices.surface.get())); + + occupancy_grid->data.reserve(width * height); + for (int y = height - 1; y >= 0; --y) { + for (int x = 0; x < width; ++x) { + // 根据packed获取这个栅格是否被更新过 + const unsigned char sem = pixel_data_color[y * width + x]; + //如果value>65,将color赋给删去const的value + + occupancy_grid->data.push_back(sem); + + } + } + //std::cout <<"value end"< +ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg); + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg); + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg); + +::cartographer::sensor::LandmarkData ToLandmarkData( + const cartographer_ros_msgs::LandmarkList& landmark_list); + +::cartographer::transform::Rigid3d ToRigid3d( + const geometry_msgs::TransformStamped& transform); + +::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose); + +Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3); + +Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion); + +// Converts from WGS84 (latitude, longitude, altitude) to ECEF. +Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, + double altitude); + +// Returns a transform that takes ECEF coordinates from nearby points to a local +// frame that has z pointing upwards. +cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, + double longitude); + +// Points to an occupancy grid message at a specific resolution from painted +// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...). +std::unique_ptr CreateOccupancyGridMsg( + //,,,const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const ros::Time& time); +//。。。重载CreateOccupancyGridMsg +std::unique_ptr CreateOccupancyGridMsg( + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const ros::Time& time,std::unique_ptr& occupancy_grid_old); +//... +std::unique_ptr CreateOccupancyGridMsg( + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const int* pixel_data_color, + const double resolution, const std::string& frame_id, + const ros::Time& time); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/msg_conversion_test.cc new file mode 100644 index 0000000..85039dd --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -0,0 +1,213 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/msg_conversion.h" + +#include +#include + +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer_ros/time_conversion.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "sensor_msgs/LaserScan.h" + +namespace cartographer_ros { +namespace { + +using ::cartographer::sensor::LandmarkData; +using ::cartographer::sensor::LandmarkObservation; +using ::testing::AllOf; +using ::testing::DoubleNear; +using ::testing::ElementsAre; +using ::testing::Field; + +constexpr double kEps = 1e-6; + +TEST(MsgConversion, LaserScanToPointCloud) { + sensor_msgs::LaserScan laser_scan; + for (int i = 0; i < 8; ++i) { + laser_scan.ranges.push_back(1.f); + } + laser_scan.angle_min = 0.f; + laser_scan.angle_max = 8.f * static_cast(M_PI_4); + laser_scan.angle_increment = static_cast(M_PI_4); + laser_scan.range_min = 0.f; + laser_scan.range_max = 10.f; + + const auto point_cloud = + std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; + EXPECT_TRUE( + point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[1].position.isApprox( + Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); + EXPECT_TRUE( + point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[3].position.isApprox( + Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); + EXPECT_TRUE( + point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[5].position.isApprox( + Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), + kEps)); + EXPECT_TRUE( + point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[7].position.isApprox( + Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); + for (int i = 0; i < 8; ++i) { + EXPECT_NEAR(point_cloud[i].time, 0.f, kEps); + } +} + +TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { + sensor_msgs::LaserScan laser_scan; + laser_scan.ranges.push_back(1.f); + laser_scan.ranges.push_back(std::numeric_limits::infinity()); + laser_scan.ranges.push_back(2.f); + laser_scan.ranges.push_back(std::numeric_limits::quiet_NaN()); + laser_scan.ranges.push_back(3.f); + laser_scan.angle_min = 0.f; + laser_scan.angle_max = 3.f * static_cast(M_PI_4); + laser_scan.angle_increment = static_cast(M_PI_4); + laser_scan.range_min = 2.f; + laser_scan.range_max = 10.f; + + const auto point_cloud = + std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; + ASSERT_EQ(2, point_cloud.size()); + EXPECT_TRUE( + point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps)); + EXPECT_TRUE( + point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps)); + EXPECT_NEAR(point_cloud[0].time, 0.f, kEps); + EXPECT_NEAR(point_cloud[1].time, 0.f, kEps); +} + +::testing::Matcher EqualsLandmark( + const LandmarkObservation& expected) { + return ::testing::AllOf( + Field(&LandmarkObservation::id, expected.id), + Field(&LandmarkObservation::landmark_to_tracking_transform, + ::cartographer::transform::IsNearly( + expected.landmark_to_tracking_transform, kEps)), + Field(&LandmarkObservation::translation_weight, + DoubleNear(expected.translation_weight, kEps)), + Field(&LandmarkObservation::rotation_weight, + DoubleNear(expected.rotation_weight, kEps))); +} + +TEST(MsgConversion, LandmarkListToLandmarkData) { + cartographer_ros_msgs::LandmarkList message; + message.header.stamp.fromSec(10); + + cartographer_ros_msgs::LandmarkEntry landmark_0; + landmark_0.id = "landmark_0"; + landmark_0.tracking_from_landmark_transform.position.x = 1.0; + landmark_0.tracking_from_landmark_transform.position.y = 2.0; + landmark_0.tracking_from_landmark_transform.position.z = 3.0; + landmark_0.tracking_from_landmark_transform.orientation.w = 1.0; + landmark_0.tracking_from_landmark_transform.orientation.x = 0.0; + landmark_0.tracking_from_landmark_transform.orientation.y = 0.0; + landmark_0.tracking_from_landmark_transform.orientation.z = 0.0; + landmark_0.translation_weight = 1.0; + landmark_0.rotation_weight = 2.0; + message.landmarks.push_back(landmark_0); + + LandmarkData actual_landmark_data = ToLandmarkData(message); + EXPECT_THAT(actual_landmark_data, + AllOf(Field(&LandmarkData::time, FromRos(message.header.stamp)), + Field(&LandmarkData::landmark_observations, + ElementsAre(EqualsLandmark(LandmarkObservation{ + "landmark_0", + ::cartographer::transform::Rigid3d( + Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(1., 0., 0., 0.)), + 1.f, + 2.f, + }))))); +} + +TEST(MsgConversion, LatLongAltToEcef) { + Eigen::Vector3d equator_prime_meridian = LatLongAltToEcef(0, 0, 0); + EXPECT_TRUE(equator_prime_meridian.isApprox(Eigen::Vector3d(6378137, 0, 0))) + << equator_prime_meridian; + Eigen::Vector3d plus_ten_meters = LatLongAltToEcef(0, 0, 10); + EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0))) + << plus_ten_meters; + Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0); + EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps)) + << north_pole; + Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0); + EXPECT_TRUE( + also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps)) + << also_north_pole; + Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0); + EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), kEps)) + << south_pole; + Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20); + EXPECT_TRUE( + above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), kEps)) + << above_south_pole; + Eigen::Vector3d somewhere_on_earth = + LatLongAltToEcef(48.1372149, 11.5748024, 517.1); + EXPECT_TRUE(somewhere_on_earth.isApprox( + Eigen::Vector3d(4177983, 855702, 4727457), kEps)) + << somewhere_on_earth; +} + +TEST(MsgConversion, ComputeLocalFrameFromLatLong) { + cartographer::transform::Rigid3d north_pole = + ComputeLocalFrameFromLatLong(90., 0.); + EXPECT_TRUE((north_pole * LatLongAltToEcef(90., 0., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())); + cartographer::transform::Rigid3d south_pole = + ComputeLocalFrameFromLatLong(-90., 0.); + EXPECT_TRUE((south_pole * LatLongAltToEcef(-90., 0., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())); + cartographer::transform::Rigid3d prime_meridian_equator = + ComputeLocalFrameFromLatLong(0., 0.); + EXPECT_TRUE((prime_meridian_equator * LatLongAltToEcef(0., 0., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())) + << prime_meridian_equator * LatLongAltToEcef(0., 0., 1.); + cartographer::transform::Rigid3d meridian_90_equator = + ComputeLocalFrameFromLatLong(0., 90.); + EXPECT_TRUE((meridian_90_equator * LatLongAltToEcef(0., 90., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())) + << meridian_90_equator * LatLongAltToEcef(0., 90., 1.); + + std::mt19937 rng(42); + std::uniform_real_distribution lat_distribution(-90., 90.); + std::uniform_real_distribution long_distribution(-180., 180.); + std::uniform_real_distribution alt_distribution(-519., 519.); + + for (int i = 0; i < 1000; ++i) { + const double latitude = lat_distribution(rng); + const double longitude = long_distribution(rng); + const double altitude = alt_distribution(rng); + cartographer::transform::Rigid3d transform_to_local_frame = + ComputeLocalFrameFromLatLong(latitude, longitude); + EXPECT_TRUE((transform_to_local_frame * + LatLongAltToEcef(latitude, longitude, altitude)) + .isApprox(altitude * Eigen::Vector3d::UnitZ(), kEps)) + << transform_to_local_frame * + LatLongAltToEcef(latitude, longitude, altitude) + << "\n" + << altitude; + } +} + +} // namespace +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node.cc new file mode 100644 index 0000000..65325bd --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node.cc @@ -0,0 +1,981 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/node.h" + +#include +#include +#include +#include +#include +#include +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "absl/strings/str_cat.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/metrics/register.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/metrics/family_factory.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/sensor_bridge.h" +#include "cartographer_ros/tf_bridge.h" +#include "cartographer_ros/time_conversion.h" +#include "cartographer_ros_msgs/StatusCode.h" +#include "cartographer_ros_msgs/StatusResponse.h" +#include "geometry_msgs/PoseStamped.h" +#include "glog/logging.h" +#include "nav_msgs/Odometry.h" +#include "ros/serialization.h" +#include "sensor_msgs/PointCloud2.h" +#include "tf2_eigen/tf2_eigen.h" +#include "visualization_msgs/MarkerArray.h" +extern int cnew[4000000]; + +namespace cartographer_ros { + +namespace carto = ::cartographer; + +using carto::transform::Rigid3d; +using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + +namespace { +// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and +// calls 'handler' on the 'node' to handle messages. Returns the subscriber. +template +::ros::Subscriber SubscribeWithHandler( + void (Node::*handler)(int, const std::string&, + const typename MessageType::ConstPtr&), + const int trajectory_id, const std::string& topic, + ::ros::NodeHandle* const node_handle, Node* const node) { + return node_handle->subscribe( + topic, kInfiniteSubscriberQueueSize, + boost::function( + [node, handler, trajectory_id, + topic](const typename MessageType::ConstPtr& msg) { + (node->*handler)(trajectory_id, topic, msg); + })); +} + +std::string TrajectoryStateToString(const TrajectoryState trajectory_state) { + switch (trajectory_state) { + case TrajectoryState::ACTIVE: + return "ACTIVE"; + case TrajectoryState::FINISHED: + return "FINISHED"; + case TrajectoryState::FROZEN: + return "FROZEN"; + case TrajectoryState::DELETED: + return "DELETED"; + } + return ""; +} + +} // namespace + +Node::Node( + const NodeOptions& node_options, + std::unique_ptr map_builder, + tf2_ros::Buffer* const tf_buffer, const bool collect_metrics) + : node_options_(node_options), + map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { + absl::MutexLock lock(&mutex_); + if (collect_metrics) { + metrics_registry_ = absl::make_unique(); + carto::metrics::RegisterAllMetrics(metrics_registry_.get()); + } + + submap_list_publisher_ = + node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( + kSubmapListTopic, kLatestOnlyPublisherQueueSize); + + markerPub= + node_handle_.advertise( + "TEXT_VIEW_FACING", 10); + + trajectory_node_list_publisher_ = + node_handle_.advertise<::visualization_msgs::MarkerArray>( + kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize); + landmark_poses_list_publisher_ = + node_handle_.advertise<::visualization_msgs::MarkerArray>( + kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize); + constraint_list_publisher_ = + node_handle_.advertise<::visualization_msgs::MarkerArray>( + kConstraintListTopic, kLatestOnlyPublisherQueueSize); + if (node_options_.publish_tracked_pose) { + tracked_pose_publisher_ = + node_handle_.advertise<::geometry_msgs::PoseStamped>( + kTrackedPoseTopic, kLatestOnlyPublisherQueueSize); + } + service_servers_.push_back(node_handle_.advertiseService( + kSubmapQueryServiceName, &Node::HandleSubmapQuery, this)); + service_servers_.push_back(node_handle_.advertiseService( + kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this)); + service_servers_.push_back(node_handle_.advertiseService( + kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); + service_servers_.push_back(node_handle_.advertiseService( + kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); + service_servers_.push_back(node_handle_.advertiseService( + kWriteStateServiceName, &Node::HandleWriteState, this)); + service_servers_.push_back(node_handle_.advertiseService( + kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this)); + service_servers_.push_back(node_handle_.advertiseService( + kReadMetricsServiceName, &Node::HandleReadMetrics, this)); + + scan_matched_point_cloud_publisher_ = + node_handle_.advertise( + kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); + + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.submap_publish_period_sec), + &Node::PublishSubmapList, this)); + if (node_options_.pose_publish_period_sec > 0) { + publish_local_trajectory_data_timer_ = node_handle_.createTimer( + ::ros::Duration(node_options_.pose_publish_period_sec), + &Node::PublishLocalTrajectoryData, this); + } + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.trajectory_publish_period_sec), + &Node::PublishTrajectoryNodeList, this)); + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.trajectory_publish_period_sec), + &Node::PublishLandmarkPosesList, this)); + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(kConstraintPublishPeriodSec), + &Node::PublishConstraintList, this)); +} + +Node::~Node() { FinishAllTrajectories(); } + +::ros::NodeHandle* Node::node_handle() { return &node_handle_; } + +bool Node::HandleSubmapQuery( + ::cartographer_ros_msgs::SubmapQuery::Request& request, + ::cartographer_ros_msgs::SubmapQuery::Response& response) { + absl::MutexLock lock(&mutex_); + map_builder_bridge_.HandleSubmapQuery(request, response); + return true; +} + +bool Node::HandleTrajectoryQuery( + ::cartographer_ros_msgs::TrajectoryQuery::Request& request, + ::cartographer_ros_msgs::TrajectoryQuery::Response& response) { + absl::MutexLock lock(&mutex_); + response.status = TrajectoryStateToStatus( + request.trajectory_id, + {TrajectoryState::ACTIVE, TrajectoryState::FINISHED, + TrajectoryState::FROZEN} /* valid states */); + if (response.status.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't query trajectory from pose graph: " + << response.status.message; + return true; + } + map_builder_bridge_.HandleTrajectoryQuery(request, response); + return true; +} + + +void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { + absl::MutexLock lock(&mutex_); + submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); +} +int Node::PublishText() { + + using namespace std; + visualization_msgs::Marker marker; + marker.header.frame_id="/odom"; + marker.header.stamp = ros::Time::now(); + marker.ns = "basic_shapes"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.id =0; + marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + + marker.scale.z = 0.8; + marker.color.b = 0; + marker.color.g = 255; + marker.color.r = 0; + marker.color.a = 1; + + ros::Rate r(1); + int k=0; + while(1) + { + geometry_msgs::Pose pose; + pose.position.x = (float)(k++)/10; + pose.position.y = 0; + pose.position.z =0; + ostringstream str; + str<=0 ;--y) { + // for (int x = 0; x <1999 ; ++x) { + // if(cnew[y*2000+x]==200){ + // LOG(ERROR)<<"tuo xie :"<info.width<<"!!!!!!"<( + &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, + this), + topic}); + } + for (const std::string& topic : ComputeRepeatedTopicNames( + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, + &node_handle_, this), + topic}); + } + for (const std::string& topic : + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandlePointCloud2Message, trajectory_id, topic, + &node_handle_, this), + topic}); + } + + // ::ros::Subscriber sub_map = node_handle_.subscribe<::nav_msgs::OccupancyGrid>("map", 1, + // boost::function( + // [this](const nav_msgs::OccupancyGrid::ConstPtr& msg) { + // (this->HandleMap)(msg); + // })); + + +subscribers_[trajectory_id].push_back({node_handle_.subscribe<::nav_msgs::OccupancyGrid>("map", 1, + boost::function( + [this](const nav_msgs::OccupancyGrid::ConstPtr& msg) { + (this->HandleMap)(msg); + })), "map_wyk"}); + + + // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is + // required. + if (node_options_.map_builder_options.use_trajectory_builder_3d() || + (node_options_.map_builder_options.use_trajectory_builder_2d() && + options.trajectory_builder_options.trajectory_builder_2d_options() + .use_imu_data())) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler(&Node::HandleImuMessage, + trajectory_id, kImuTopic, + &node_handle_, this), + kImuTopic}); + } + + if (options.use_odometry) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler(&Node::HandleOdometryMessage, + trajectory_id, kOdometryTopic, + &node_handle_, this), + kOdometryTopic}); + } + if (options.use_nav_sat) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic, + &node_handle_, this), + kNavSatFixTopic}); + } + if (options.use_landmarks) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic, + &node_handle_, this), + kLandmarkTopic}); + } +} + +bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { + if (node_options_.map_builder_options.use_trajectory_builder_2d()) { + return options.trajectory_builder_options + .has_trajectory_builder_2d_options(); + } + if (node_options_.map_builder_options.use_trajectory_builder_3d()) { + return options.trajectory_builder_options + .has_trajectory_builder_3d_options(); + } + return false; +} + +bool Node::ValidateTopicNames(const TrajectoryOptions& options) { + for (const auto& sensor_id : ComputeExpectedSensorIds(options)) { + const std::string& topic = sensor_id.id; + if (subscribed_topics_.count(topic) > 0) { + LOG(ERROR) << "Topic name [" << topic << "] is already used."; + return false; + } + } + return true; +} + +cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus( + const int trajectory_id, const std::set& valid_states) { + const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); + cartographer_ros_msgs::StatusResponse status_response; + + const auto it = trajectory_states.find(trajectory_id); + if (it == trajectory_states.end()) { + status_response.message = + absl::StrCat("Trajectory ", trajectory_id, " doesn't exist."); + status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; + return status_response; + } + + status_response.message = + absl::StrCat("Trajectory ", trajectory_id, " is in '", + TrajectoryStateToString(it->second), "' state."); + status_response.code = + valid_states.count(it->second) + ? cartographer_ros_msgs::StatusCode::OK + : cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + return status_response; +} + +cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( + const int trajectory_id) { + cartographer_ros_msgs::StatusResponse status_response; + if (trajectories_scheduled_for_finish_.count(trajectory_id)) { + status_response.message = absl::StrCat("Trajectory ", trajectory_id, + " already pending to finish."); + status_response.code = cartographer_ros_msgs::StatusCode::OK; + LOG(INFO) << status_response.message; + return status_response; + } + + // First, check if we can actually finish the trajectory. + status_response = TrajectoryStateToStatus( + trajectory_id, {TrajectoryState::ACTIVE} /* valid states */); + if (status_response.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't finish trajectory: " << status_response.message; + return status_response; + } + + // Shutdown the subscribers of this trajectory. + // A valid case with no subscribers is e.g. if we just visualize states. + if (subscribers_.count(trajectory_id)) { + for (auto& entry : subscribers_[trajectory_id]) { + entry.subscriber.shutdown(); + subscribed_topics_.erase(entry.topic); + LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; + } + CHECK_EQ(subscribers_.erase(trajectory_id), 1); + } + map_builder_bridge_.FinishTrajectory(trajectory_id); + trajectories_scheduled_for_finish_.emplace(trajectory_id); + status_response.message = + absl::StrCat("Finished trajectory ", trajectory_id, "."); + status_response.code = cartographer_ros_msgs::StatusCode::OK; + return status_response; +} + +bool Node::HandleStartTrajectory( + ::cartographer_ros_msgs::StartTrajectory::Request& request, + ::cartographer_ros_msgs::StartTrajectory::Response& response) { + TrajectoryOptions trajectory_options; + std::tie(std::ignore, trajectory_options) = LoadOptions( + request.configuration_directory, request.configuration_basename); + + if (request.use_initial_pose) { + const auto pose = ToRigid3d(request.initial_pose); + if (!pose.IsValid()) { + response.status.message = + "Invalid pose argument. Orientation quaternion must be normalized."; + LOG(ERROR) << response.status.message; + response.status.code = + cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + return true; + } + + // Check if the requested trajectory for the relative initial pose exists. + response.status = TrajectoryStateToStatus( + request.relative_to_trajectory_id, + {TrajectoryState::ACTIVE, TrajectoryState::FROZEN, + TrajectoryState::FINISHED} /* valid states */); + if (response.status.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't start a trajectory with initial pose: " + << response.status.message; + return true; + } + + ::cartographer::mapping::proto::InitialTrajectoryPose + initial_trajectory_pose; + initial_trajectory_pose.set_to_trajectory_id( + request.relative_to_trajectory_id); + *initial_trajectory_pose.mutable_relative_pose() = + cartographer::transform::ToProto(pose); + initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal( + ::cartographer_ros::FromRos(ros::Time(0)))); + *trajectory_options.trajectory_builder_options + .mutable_initial_trajectory_pose() = initial_trajectory_pose; + } + + if (!ValidateTrajectoryOptions(trajectory_options)) { + response.status.message = "Invalid trajectory options."; + LOG(ERROR) << response.status.message; + response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + } else if (!ValidateTopicNames(trajectory_options)) { + response.status.message = "Topics are already used by another trajectory."; + LOG(ERROR) << response.status.message; + response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + } else { + response.status.message = "Success."; + response.trajectory_id = AddTrajectory(trajectory_options); + response.status.code = cartographer_ros_msgs::StatusCode::OK; + } + return true; +} + +void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { + absl::MutexLock lock(&mutex_); + CHECK(ValidateTrajectoryOptions(options)); + AddTrajectory(options); +} + +std::vector< + std::set> +Node::ComputeDefaultSensorIdsForMultipleBags( + const std::vector& bags_options) const { + using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; + std::vector> bags_sensor_ids; + for (size_t i = 0; i < bags_options.size(); ++i) { + std::string prefix; + if (bags_options.size() > 1) { + prefix = "bag_" + std::to_string(i + 1) + "_"; + } + std::set unique_sensor_ids; + for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) { + unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); + } + bags_sensor_ids.push_back(unique_sensor_ids); + } + return bags_sensor_ids; +} + +int Node::AddOfflineTrajectory( + const std::set& + expected_sensor_ids, + const TrajectoryOptions& options) { + absl::MutexLock lock(&mutex_); + const int trajectory_id = + map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + AddExtrapolator(trajectory_id, options); + AddSensorSamplers(trajectory_id, options); + return trajectory_id; +} + +bool Node::HandleGetTrajectoryStates( + ::cartographer_ros_msgs::GetTrajectoryStates::Request& request, + ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) { + using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + absl::MutexLock lock(&mutex_); + response.status.code = ::cartographer_ros_msgs::StatusCode::OK; + response.trajectory_states.header.stamp = ros::Time::now(); + for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + response.trajectory_states.trajectory_id.push_back(entry.first); + switch (entry.second) { + case TrajectoryState::ACTIVE: + response.trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::TrajectoryStates::ACTIVE); + break; + case TrajectoryState::FINISHED: + response.trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::TrajectoryStates::FINISHED); + break; + case TrajectoryState::FROZEN: + response.trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::TrajectoryStates::FROZEN); + break; + case TrajectoryState::DELETED: + response.trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::TrajectoryStates::DELETED); + break; + } + } + return true; +} + +bool Node::HandleFinishTrajectory( + ::cartographer_ros_msgs::FinishTrajectory::Request& request, + ::cartographer_ros_msgs::FinishTrajectory::Response& response) { + absl::MutexLock lock(&mutex_); + response.status = FinishTrajectoryUnderLock(request.trajectory_id); + return true; +} + +bool Node::HandleWriteState( + ::cartographer_ros_msgs::WriteState::Request& request, + ::cartographer_ros_msgs::WriteState::Response& response) { + absl::MutexLock lock(&mutex_); + if (map_builder_bridge_.SerializeState(request.filename, + request.include_unfinished_submaps)) { + response.status.code = cartographer_ros_msgs::StatusCode::OK; + response.status.message = + absl::StrCat("State written to '", request.filename, "'."); + } else { + response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + response.status.message = + absl::StrCat("Failed to write '", request.filename, "'."); + } + return true; +} + +bool Node::HandleReadMetrics( + ::cartographer_ros_msgs::ReadMetrics::Request& request, + ::cartographer_ros_msgs::ReadMetrics::Response& response) { + absl::MutexLock lock(&mutex_); + response.timestamp = ros::Time::now(); + if (!metrics_registry_) { + response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE; + response.status.message = "Collection of runtime metrics is not activated."; + return true; + } + metrics_registry_->ReadMetrics(&response); + response.status.code = cartographer_ros_msgs::StatusCode::OK; + response.status.message = "Successfully read metrics."; + return true; +} + +void Node::FinishAllTrajectories() { + absl::MutexLock lock(&mutex_); + for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + if (entry.second == TrajectoryState::ACTIVE) { + const int trajectory_id = entry.first; + CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, + cartographer_ros_msgs::StatusCode::OK); + } + } +} + +bool Node::FinishTrajectory(const int trajectory_id) { + absl::MutexLock lock(&mutex_); + return FinishTrajectoryUnderLock(trajectory_id).code == + cartographer_ros_msgs::StatusCode::OK; +} + +void Node::RunFinalOptimization() { + { + for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + const int trajectory_id = entry.first; + if (entry.second == TrajectoryState::ACTIVE) { + LOG(WARNING) + << "Can't run final optimization if there are one or more active " + "trajectories. Trying to finish trajectory with ID " + << std::to_string(trajectory_id) << " now."; + CHECK(FinishTrajectory(trajectory_id)) + << "Failed to finish trajectory with ID " + << std::to_string(trajectory_id) << "."; + } + } + } + // Assuming we are not adding new data anymore, the final optimization + // can be performed without holding the mutex. + map_builder_bridge_.RunFinalOptimization(); +} + +void Node::HandleOdometryMessage(const int trajectory_id, + const std::string& sensor_id, + const nav_msgs::Odometry::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { + return; + } + auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); + auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg); + if (odometry_data_ptr != nullptr) { + extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr); + } + sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg); +} + +void Node::HandleNavSatFixMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::NavSatFix::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleNavSatFixMessage(sensor_id, msg); +} + +void Node::HandleLandmarkMessage( + const int trajectory_id, const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleLandmarkMessage(sensor_id, msg); +} + +void Node::HandleImuMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::Imu::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { + return; + } + auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); + auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); + if (imu_data_ptr != nullptr) { + extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); + } + sensor_bridge_ptr->HandleImuMessage(sensor_id, msg); +} + +void Node::HandleLaserScanMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::LaserScan::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleLaserScanMessage(sensor_id, msg); +} + +void Node::HandleMultiEchoLaserScanMessage( + const int trajectory_id, const std::string& sensor_id, + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage(sensor_id, msg); +} + +void Node::HandlePointCloud2Message( + const int trajectory_id, const std::string& sensor_id, + const sensor_msgs::PointCloud2::ConstPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandlePointCloud2Message(sensor_id, msg); +} + +void Node::SerializeState(const std::string& filename, + const bool include_unfinished_submaps) { + absl::MutexLock lock(&mutex_); + CHECK( + map_builder_bridge_.SerializeState(filename, include_unfinished_submaps)) + << "Could not write state."; +} + +void Node::LoadState(const std::string& state_filename, + const bool load_frozen_state) { + absl::MutexLock lock(&mutex_); + map_builder_bridge_.LoadState(state_filename, load_frozen_state); +} + +void Node::MaybeWarnAboutTopicMismatch( + const ::ros::WallTimerEvent& unused_timer_event) { + ::ros::master::V_TopicInfo ros_topics; + ::ros::master::getTopics(ros_topics); + std::set published_topics; + std::stringstream published_topics_string; + for (const auto& it : ros_topics) { + std::string resolved_topic = node_handle_.resolveName(it.name, false); + published_topics.insert(resolved_topic); + published_topics_string << resolved_topic << ","; + } + bool print_topics = false; + for (const auto& entry : subscribers_) { + int trajectory_id = entry.first; + for (const auto& subscriber : entry.second) { + std::string resolved_topic = node_handle_.resolveName(subscriber.topic); + if (published_topics.count(resolved_topic) == 0) { + LOG(WARNING) << "Expected topic \"" << subscriber.topic + << "\" (trajectory " << trajectory_id << ")" + << " (resolved topic \"" << resolved_topic << "\")" + << " but no publisher is currently active."; + print_topics = true; + } + } + } + if (print_topics) { + LOG(WARNING) << "Currently available topics are: " + << published_topics_string.str(); + } +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node.h new file mode 100644 index 0000000..fa94105 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node.h @@ -0,0 +1,240 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H + +#include +#include +#include +#include +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_extrapolator.h" +#include "cartographer_ros/map_builder_bridge.h" +#include "cartographer_ros/metrics/family_factory.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/node_options.h" +#include "cartographer_ros/trajectory_options.h" +#include "cartographer_ros_msgs/FinishTrajectory.h" +#include "cartographer_ros_msgs/GetTrajectoryStates.h" +#include "cartographer_ros_msgs/ReadMetrics.h" +#include "cartographer_ros_msgs/StartTrajectory.h" +#include "cartographer_ros_msgs/StatusResponse.h" +#include "cartographer_ros_msgs/SubmapEntry.h" +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/WriteState.h" +#include "nav_msgs/Odometry.h" +#include "ros/ros.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/NavSatFix.h" +#include "sensor_msgs/PointCloud2.h" +#include "tf2_ros/transform_broadcaster.h" + +namespace cartographer_ros { + +// Wires up ROS topics to SLAM. +class Node { + public: + Node(const NodeOptions& node_options, + std::unique_ptr map_builder, + tf2_ros::Buffer* tf_buffer, bool collect_metrics); + ~Node(); + + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + int PublishText(); + // Finishes all yet active trajectories. + void FinishAllTrajectories(); + // Finishes a single given trajectory. Returns false if the trajectory did not + // exist or was already finished. + bool FinishTrajectory(int trajectory_id); + + // Runs final optimization. All trajectories have to be finished when calling. + void RunFinalOptimization(); + + // Starts the first trajectory with the default topics. + void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options); + + // Returns unique SensorIds for multiple input bag files based on + // their TrajectoryOptions. + // 'SensorId::id' is the expected ROS topic name. + std::vector< + std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>> + ComputeDefaultSensorIdsForMultipleBags( + const std::vector& bags_options) const; + + // Adds a trajectory for offline processing, i.e. not listening to topics. + int AddOfflineTrajectory( + const std::set< + cartographer::mapping::TrajectoryBuilderInterface::SensorId>& + expected_sensor_ids, + const TrajectoryOptions& options); + + // The following functions handle adding sensor data to a trajectory. + void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, + const nav_msgs::Odometry::ConstPtr& msg); + void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, + const sensor_msgs::NavSatFix::ConstPtr& msg); + void HandleLandmarkMessage( + int trajectory_id, const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + void HandleImuMessage(int trajectory_id, const std::string& sensor_id, + const sensor_msgs::Imu::ConstPtr& msg); + void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, + const sensor_msgs::LaserScan::ConstPtr& msg); + void HandleMultiEchoLaserScanMessage( + int trajectory_id, const std::string& sensor_id, + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, + const sensor_msgs::PointCloud2::ConstPtr& msg); + + void HandleMap(const nav_msgs::OccupancyGrid::ConstPtr& mapmsg); + + // Serializes the complete Node state. + void SerializeState(const std::string& filename, + const bool include_unfinished_submaps); + + // Loads a serialized SLAM state from a .pbstream file. + void LoadState(const std::string& state_filename, bool load_frozen_state); + + ::ros::NodeHandle* node_handle(); + + private: + struct Subscriber { + ::ros::Subscriber subscriber; + + // ::ros::Subscriber::getTopic() does not necessarily return the same + // std::string + // it was given in its constructor. Since we rely on the topic name as the + // unique identifier of a subscriber, we remember it ourselves. + std::string topic; + }; + + bool HandleSubmapQuery( + cartographer_ros_msgs::SubmapQuery::Request& request, + cartographer_ros_msgs::SubmapQuery::Response& response); + bool HandleTrajectoryQuery( + ::cartographer_ros_msgs::TrajectoryQuery::Request& request, + ::cartographer_ros_msgs::TrajectoryQuery::Response& response); + bool HandleStartTrajectory( + cartographer_ros_msgs::StartTrajectory::Request& request, + cartographer_ros_msgs::StartTrajectory::Response& response); + bool HandleFinishTrajectory( + cartographer_ros_msgs::FinishTrajectory::Request& request, + cartographer_ros_msgs::FinishTrajectory::Response& response); + bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, + cartographer_ros_msgs::WriteState::Response& response); + bool HandleGetTrajectoryStates( + ::cartographer_ros_msgs::GetTrajectoryStates::Request& request, + ::cartographer_ros_msgs::GetTrajectoryStates::Response& response); + bool HandleReadMetrics( + cartographer_ros_msgs::ReadMetrics::Request& request, + cartographer_ros_msgs::ReadMetrics::Response& response); + + // Returns the set of SensorIds expected for a trajectory. + // 'SensorId::id' is the expected ROS topic name. + std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> + ComputeExpectedSensorIds(const TrajectoryOptions& options) const; + int AddTrajectory(const TrajectoryOptions& options); + void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id); + void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); + void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); + void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); + void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event); + void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); + void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); + void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); + bool ValidateTrajectoryOptions(const TrajectoryOptions& options); + bool ValidateTopicNames(const TrajectoryOptions& options); + cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( + int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); + + // Helper function for service handlers that need to check trajectory states. + cartographer_ros_msgs::StatusResponse TrajectoryStateToStatus( + int trajectory_id, + const std::set< + cartographer::mapping::PoseGraphInterface::TrajectoryState>& + valid_states); + const NodeOptions node_options_; + + tf2_ros::TransformBroadcaster tf_broadcaster_; + + absl::Mutex mutex_; + std::unique_ptr metrics_registry_; + MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); + + ::ros::Publisher markerPub; + + ::ros::NodeHandle node_handle_; + ::ros::Publisher submap_list_publisher_; + ::ros::Publisher trajectory_node_list_publisher_; + ::ros::Publisher landmark_poses_list_publisher_; + ::ros::Publisher constraint_list_publisher_; + ::ros::Publisher tracked_pose_publisher_; + // These ros::ServiceServers need to live for the lifetime of the node. + std::vector<::ros::ServiceServer> service_servers_; + ::ros::Publisher scan_matched_point_cloud_publisher_; + + struct TrajectorySensorSamplers { + TrajectorySensorSamplers(const double rangefinder_sampling_ratio, + const double odometry_sampling_ratio, + const double fixed_frame_pose_sampling_ratio, + const double imu_sampling_ratio, + const double landmark_sampling_ratio) + : rangefinder_sampler(rangefinder_sampling_ratio), + odometry_sampler(odometry_sampling_ratio), + fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio), + imu_sampler(imu_sampling_ratio), + landmark_sampler(landmark_sampling_ratio) {} + + ::cartographer::common::FixedRatioSampler rangefinder_sampler; + ::cartographer::common::FixedRatioSampler odometry_sampler; + ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler; + ::cartographer::common::FixedRatioSampler imu_sampler; + ::cartographer::common::FixedRatioSampler landmark_sampler; + }; + + // These are keyed with 'trajectory_id'. + std::map extrapolators_; + std::map last_published_tf_stamps_; + std::unordered_map sensor_samplers_; + std::unordered_map> subscribers_; + std::unordered_set subscribed_topics_; + std::unordered_set trajectories_scheduled_for_finish_; + + // We have to keep the timer handles of ::ros::WallTimers around, otherwise + // they do not fire. + std::vector<::ros::WallTimer> wall_timers_; + + // The timer for publishing local trajectory data (i.e. pose transforms and + // range data point clouds) is a regular timer which is not triggered when + // simulation time is standing still. This prevents overflowing the transform + // listener buffer by publishing the same transforms over and over again. + ::ros::Timer publish_local_trajectory_data_timer_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_constants.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_constants.cc new file mode 100644 index 0000000..415ad37 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_constants.cc @@ -0,0 +1,37 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/node_constants.h" + +#include "glog/logging.h" + +namespace cartographer_ros { + +std::vector ComputeRepeatedTopicNames(const std::string& topic, + const int num_topics) { + CHECK_GE(num_topics, 0); + if (num_topics == 1) { + return {topic}; + } + std::vector topics; + topics.reserve(num_topics); + for (int i = 0; i < num_topics; ++i) { + topics.emplace_back(topic + "_" + std::to_string(i + 1)); + } + return topics; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_constants.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_constants.h new file mode 100644 index 0000000..07a85f3 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_constants.h @@ -0,0 +1,59 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H + +#include +#include + +namespace cartographer_ros { + +// Default topic names; expected to be remapped as needed. +constexpr char kLaserScanTopic[] = "scan"; +constexpr char kMultiEchoLaserScanTopic[] = "echoes"; +constexpr char kPointCloud2Topic[] = "points2"; +constexpr char kImuTopic[] = "imu"; +constexpr char kOdometryTopic[] = "odom"; +constexpr char kNavSatFixTopic[] = "fix"; +constexpr char kLandmarkTopic[] = "landmark"; +constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; +constexpr char kOccupancyGridTopic[] = "map"; +constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; +constexpr char kSubmapListTopic[] = "submap_list"; +constexpr char kTrackedPoseTopic[] = "tracked_pose"; +constexpr char kSubmapQueryServiceName[] = "submap_query"; +constexpr char kTrajectoryQueryServiceName[] = "trajectory_query"; +constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; +constexpr char kWriteStateServiceName[] = "write_state"; +constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states"; +constexpr char kReadMetricsServiceName[] = "read_metrics"; +constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; +constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list"; +constexpr char kConstraintListTopic[] = "constraint_list"; +constexpr double kConstraintPublishPeriodSec = 0.5; +constexpr double kTopicMismatchCheckDelaySec = 3.0; + +constexpr int kInfiniteSubscriberQueueSize = 0; +constexpr int kLatestOnlyPublisherQueueSize = 1; + +// For multiple topics adds numbers to the topic name and returns the list. +std::vector ComputeRepeatedTopicNames(const std::string& topic, + int num_topics); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_main.cc new file mode 100644 index 0000000..f0b4ecf --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_main.cc @@ -0,0 +1,102 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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 "absl/memory/memory.h" +#include "cartographer/mapping/map_builder.h" +#include "cartographer_ros/node.h" +#include "cartographer_ros/node_options.h" +#include "cartographer_ros/ros_log_sink.h" +#include "gflags/gflags.h" +#include "tf2_ros/transform_listener.h" + +DEFINE_bool(collect_metrics, false, + "Activates the collection of runtime metrics. If activated, the " + "metrics can be accessed via a ROS service."); +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); +DEFINE_string(load_state_filename, "", + "If non-empty, filename of a .pbstream file to load, containing " + "a saved SLAM state."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); +DEFINE_bool( + start_trajectory_with_default_topics, true, + "Enable to immediately start the first trajectory with default topics."); +DEFINE_string( + save_state_filename, "", + "If non-empty, serialize state and write it to disk before shutting down."); + +namespace cartographer_ros { +namespace { + +void Run() { + constexpr double kTfBufferCacheTimeInSeconds = 10.; + tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; + tf2_ros::TransformListener tf(tf_buffer); + NodeOptions node_options; + TrajectoryOptions trajectory_options; + std::tie(node_options, trajectory_options) = + LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); + + auto map_builder = + cartographer::mapping::CreateMapBuilder(node_options.map_builder_options); + Node node(node_options, std::move(map_builder), &tf_buffer, + FLAGS_collect_metrics); + //node.PublishText(); + if (!FLAGS_load_state_filename.empty()) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); + } + + if (FLAGS_start_trajectory_with_default_topics) { + node.StartTrajectoryWithDefaultTopics(trajectory_options); + } + + ::ros::spin(); + + node.FinishAllTrajectories(); + node.RunFinalOptimization(); + + if (!FLAGS_save_state_filename.empty()) { + node.SerializeState(FLAGS_save_state_filename, + true /* include_unfinished_submaps */); + } +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + FLAGS_log_dir = "/home/wzw/slam_new/log"; + + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basename.empty()) + << "-configuration_basename is missing."; + + ::ros::init(argc, argv, "cartographer_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + cartographer_ros::Run(); + ::ros::shutdown(); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_options.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_options.cc new file mode 100644 index 0000000..681754e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_options.cc @@ -0,0 +1,73 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/node_options.h" + +#include + +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "glog/logging.h" + +namespace cartographer_ros { + +NodeOptions CreateNodeOptions( + ::cartographer::common::LuaParameterDictionary* const + lua_parameter_dictionary) { + NodeOptions options; + options.map_builder_options = + ::cartographer::mapping::CreateMapBuilderOptions( + lua_parameter_dictionary->GetDictionary("map_builder").get()); + options.map_frame = lua_parameter_dictionary->GetString("map_frame"); + options.lookup_transform_timeout_sec = + lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec"); + options.submap_publish_period_sec = + lua_parameter_dictionary->GetDouble("submap_publish_period_sec"); + options.pose_publish_period_sec = + lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); + options.trajectory_publish_period_sec = + lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec"); + if (lua_parameter_dictionary->HasKey("publish_to_tf")) { + options.publish_to_tf = + lua_parameter_dictionary->GetBool("publish_to_tf"); + } + if (lua_parameter_dictionary->HasKey("publish_tracked_pose")) { + options.publish_tracked_pose = + lua_parameter_dictionary->GetBool("publish_tracked_pose"); + } + if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) { + options.use_pose_extrapolator = + lua_parameter_dictionary->GetBool("use_pose_extrapolator"); + } + return options; +} + +std::tuple LoadOptions( + const std::string& configuration_directory, + const std::string& configuration_basename) { + auto file_resolver = + absl::make_unique( + std::vector{configuration_directory}); + const std::string code = + file_resolver->GetFileContentOrDie(configuration_basename); + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver)); + + return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary), + CreateTrajectoryOptions(&lua_parameter_dictionary)); +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_options.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_options.h new file mode 100644 index 0000000..95bb159 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/node_options.h @@ -0,0 +1,51 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H + +#include +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" +#include "cartographer_ros/trajectory_options.h" + +namespace cartographer_ros { + +// Top-level options of Cartographer's ROS integration. +struct NodeOptions { + ::cartographer::mapping::proto::MapBuilderOptions map_builder_options; + std::string map_frame; + double lookup_transform_timeout_sec; + double submap_publish_period_sec; + double pose_publish_period_sec; + double trajectory_publish_period_sec; + bool publish_to_tf = true; + bool publish_tracked_pose = false; + bool use_pose_extrapolator = true; +}; + +NodeOptions CreateNodeOptions( + ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); + +std::tuple LoadOptions( + const std::string& configuration_directory, + const std::string& configuration_basename); +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc new file mode 100644 index 0000000..e96358e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -0,0 +1,264 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" + +// cairo是一个非常流行的开源2D图形渲染引擎库 +#include "cairo/cairo.h" + +#include "cartographer/common/port.h" +#include "cartographer/io/image.h" + +#include "cartographer/io/submap_painter.h" + +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/submap.h" + +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "gflags/gflags.h" +#include "nav_msgs/OccupancyGrid.h" +#include "ros/ros.h" + + +// 这里的分辨率只是生成地图的分辨率, 与实际cartographer使用的地图的分辨率无关 +DEFINE_double(resolution, 0.05, + "Resolution of a grid cell in the published occupancy grid."); +DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period."); +DEFINE_bool(include_frozen_submaps, true, + "Include frozen submaps in the occupancy grid."); +DEFINE_bool(include_unfrozen_submaps, true, + "Include unfrozen submaps in the occupancy grid."); +DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic, + "Name of the topic on which the occupancy grid is published."); + +namespace cartographer_ros { +namespace { + +using ::cartographer::io::PaintSubmapSlicesResult; +using ::cartographer::io::SubmapSlice; +using ::cartographer::mapping::SubmapId; + +class Node { + public: + explicit Node(double resolution, double publish_period_sec); + ~Node() {} + + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + + private: + void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); + void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); + + ::ros::NodeHandle node_handle_; + std::unique_ptr msg_ptr; + const double resolution_; + + absl::Mutex mutex_; + ::ros::ServiceClient client_ GUARDED_BY(mutex_); + ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); + ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); + std::map submap_slices_ GUARDED_BY(mutex_); + ::ros::WallTimer occupancy_grid_publisher_timer_; + std::string last_frame_id_; + ros::Time last_timestamp_; + //...新增私有成员变量 + +}; +//构造函数后面的冒号起分割作用，是类给成员变量赋值的方法，初始化列表，更适用于成员变量的常量const型 +Node::Node(const double resolution, const double publish_period_sec) + : msg_ptr(nullptr), + resolution_(resolution), + // SubmapQuery服务的客户端 + client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + kSubmapQueryServiceName)), + // 订阅 submap_list topic, 注册回调函数 + //ros::Subscriber sub = nh.subscribe("chatter",10,doMsg);第二个是数据类型，第三个是回调函数 + submap_list_subscriber_(node_handle_.subscribe( + kSubmapListTopic, kLatestOnlyPublisherQueueSize, + boost::function( + [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { + HandleSubmapList(msg); + }))), + // 声明 map 发布器 + occupancy_grid_publisher_( + node_handle_.advertise<::nav_msgs::OccupancyGrid>( + FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize, + true /* latched */)), + // 定时发布map + occupancy_grid_publisher_timer_( + node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec), + &Node::DrawAndPublish, this)) {} + +// 订阅SubmapList话题的回调函数 处理收到的消息 +void Node::HandleSubmapList( + const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { + absl::MutexLock locker(&mutex_); + + // We do not do any work if nobody listens. + // 如果没人订阅就不处理 + if (occupancy_grid_publisher_.getNumSubscribers() == 0) { + return; + } + + // Keep track of submap IDs that don't appear in the message anymore. + // 先将所有的submap_slices_保存到submap_ids_to_delete中 + std::set submap_ids_to_delete; + for (const auto& pair : submap_slices_) { + submap_ids_to_delete.insert(pair.first); + } + + // 遍历所有的submap + for (const auto& submap_msg : msg->submap) { + // 生成SubmapId + const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; + // 如果msg中存在这个生成SubmapId就在 待删除列表 中移除 + submap_ids_to_delete.erase(id); + // 根据参数决定 是否 对 frozen 与 unfrozen 的submap进行跳过 + if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) || + (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) { + continue; + } + + // 获取key为id的值的引用进行赋值, 如果key中没有这个id就新建一个键值对 + SubmapSlice& submap_slice = submap_slices_[id]; + submap_slice.pose = ToRigid3d(submap_msg.pose); + submap_slice.metadata_version = submap_msg.submap_version; + + // 如果已经填充过地图信息了就跳过 + if (submap_slice.surface != nullptr && + submap_slice.version == submap_msg.submap_version) { + continue; + } + + // Step: 1 获取格式为io::SubmapTextures的地图栅格数据 + auto fetched_textures = + ::cartographer_ros::FetchSubmapTextures(id, &client_); + + if (fetched_textures == nullptr) { + continue; + } + CHECK(!fetched_textures->textures.empty()); + submap_slice.version = fetched_textures->version; + + // We use the first texture only. By convention this is the highest + // resolution texture and that is the one we want to use to construct the + // map for ROS. + // 将 格式为io::SubmapTextures的地图栅格数据 放入 submap_slice 中 + const auto fetched_texture = fetched_textures->textures.begin(); + submap_slice.width = fetched_texture->width; + submap_slice.height = fetched_texture->height; + submap_slice.slice_pose = fetched_texture->slice_pose; + submap_slice.resolution = fetched_texture->resolution; + submap_slice.cairo_data.clear(); + + // Step: 2 生成surface, surface是指向Cairo画布的指针 + submap_slice.surface = ::cartographer::io::DrawTexture( + fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, + //。。。增加color传参 + fetched_texture->pixels.color, + fetched_texture->width, fetched_texture->height, + &submap_slice.cairo_data); + } // end for + + // Delete all submaps that didn't appear in the message. + // msg处理完之后, 还在 待删除 列表中的信息 就是后来不出现的, 可以删掉了 + for (const auto& id : submap_ids_to_delete) { + submap_slices_.erase(id); + } + + last_timestamp_ = msg->header.stamp; + last_frame_id_ = msg->header.frame_id; +} + +void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { + absl::MutexLock locker(&mutex_); + if (submap_slices_.empty() || last_frame_id_.empty()) { + return; + } + //if(submap_slices_.size()==1){ + + + auto painted_slices_color = Slices(submap_slices_, resolution_); + auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_); + const int width = cairo_image_surface_get_width(painted_slices.surface.get()); + const int height = + cairo_image_surface_get_height(painted_slices.surface.get()); +/* + for (int y = height - 1; y >= 0; --y) { + for (int x = 0; x < width; ++x) { + const int packed = painted_slices_color[y * width + x]; + int sem =packed; + + if (sem>0){ + std::cout< msg_ptr = CreateOccupancyGridMsg( + // painted_slices, resolution_, last_frame_id_, last_timestamp_); + int *painted_slices_color_sum=c; + // for (int y = 1999; y >=0 ;--y) { + // for (int x = 0; x <1999 ; ++x) { + // if(c[y*2000+x]==200){ + // LOG(ERROR)<<"tuo xie :"< msg_ptr = CreateOccupancyGridMsg( + painted_slices, painted_slices_color_sum,resolution_, last_frame_id_, last_timestamp_); + // std::unique_ptr msg_ptr = CreateOccupancyGridMsg( + // painted_slices, resolution_, last_frame_id_, last_timestamp_); + occupancy_grid_publisher_.publish(*msg_ptr); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps) + << "Ignoring both frozen and unfrozen submaps makes no sense."; + + ::ros::init(argc, argv, "cartographer_occupancy_grid_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + // 这个Node类指的是上边的类,与node.cc无关 + ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec); + + ::ros::spin(); + ::ros::shutdown(); +} + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node.cc new file mode 100644 index 0000000..e3a7e7a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node.cc @@ -0,0 +1,396 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/offline_node.h" + +#include +#include +#ifndef WIN32 +#include +#endif +#include + +#include + +#include "absl/strings/str_split.h" +#include "cartographer_ros/node.h" +#include "cartographer_ros/playable_bag.h" +#include "cartographer_ros/urdf_reader.h" +#include "gflags/gflags.h" +#include "ros/callback_queue.h" +#include "rosgraph_msgs/Clock.h" +#include "tf2_ros/static_transform_broadcaster.h" +#include "urdf/model.h" + +DEFINE_bool(collect_metrics, false, + "Activates the collection of runtime metrics. If activated, the " + "metrics can be accessed via a ROS service."); +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string( + configuration_basenames, "", + "Comma-separated list of basenames, i.e. not containing any " + "directory prefix, of the configuration files for each trajectory. " + "The first configuration file will be used for node options. " + "If less configuration files are specified than trajectories, the " + "first file will be used the remaining trajectories."); +DEFINE_string( + bag_filenames, "", + "Comma-separated list of bags to process. One bag per trajectory. " + "Any combination of simultaneous and sequential bags is supported."); +DEFINE_string(urdf_filenames, "", + "Comma-separated list of one or more URDF files that contain " + "static links for the sensor configuration(s)."); +DEFINE_bool(use_bag_transforms, true, + "Whether to read, use and republish transforms from bags."); +DEFINE_string(load_state_filename, "", + "If non-empty, filename of a .pbstream file to load, containing " + "a saved SLAM state."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); +DEFINE_string(save_state_filename, "", + "Explicit name of the file to which the serialized state will be " + "written before shutdown. If left empty, the filename will be " + "inferred from the first bagfile's name as: " + ".pbstream"); +DEFINE_bool(keep_running, false, + "Keep running the offline node after all messages from the bag " + "have been processed."); +DEFINE_double(skip_seconds, 0, + "Optional amount of seconds to skip from the beginning " + "(i.e. when the earliest bag starts.). "); + +namespace cartographer_ros { + +constexpr char kClockTopic[] = "clock"; +constexpr char kTfStaticTopic[] = "/tf_static"; +constexpr char kTfTopic[] = "tf"; +constexpr double kClockPublishFrequencySec = 1. / 30.; +constexpr int kSingleThreaded = 1; +// We publish tf messages one second earlier than other messages. Under +// the assumption of higher frequency tf this should ensure that tf can +// always interpolate. +const ::ros::Duration kDelay = ::ros::Duration(1.0); + +void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basenames.empty()) + << "-configuration_basenames is missing."; + CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) + << "-bag_filenames and -load_state_filename cannot both be unspecified."; + const std::vector bag_filenames = + absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()); + cartographer_ros::NodeOptions node_options; + const std::vector configuration_basenames = + absl::StrSplit(FLAGS_configuration_basenames, ',', absl::SkipEmpty()); + std::vector bag_trajectory_options(1); + std::tie(node_options, bag_trajectory_options.at(0)) = + LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0)); + + for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) { + TrajectoryOptions current_trajectory_options; + if (bag_index < configuration_basenames.size()) { + std::tie(std::ignore, current_trajectory_options) = LoadOptions( + FLAGS_configuration_directory, configuration_basenames.at(bag_index)); + } else { + current_trajectory_options = bag_trajectory_options.at(0); + } + bag_trajectory_options.push_back(current_trajectory_options); + } + if (bag_filenames.size() > 0) { + CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size()); + } + + // Since we preload the transform buffer, we should never have to wait for a + // transform. When we finish processing the bag, we will simply drop any + // remaining sensor data that cannot be transformed due to missing transforms. + node_options.lookup_transform_timeout_sec = 0.; + + auto map_builder = map_builder_factory(node_options.map_builder_options); + + const std::chrono::time_point start_time = + std::chrono::steady_clock::now(); + + tf2_ros::Buffer tf_buffer; + + std::vector urdf_transforms; + const std::vector urdf_filenames = + absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty()); + for (const auto& urdf_filename : urdf_filenames) { + const auto current_urdf_transforms = + ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); + urdf_transforms.insert(urdf_transforms.end(), + current_urdf_transforms.begin(), + current_urdf_transforms.end()); + } + + tf_buffer.setUsingDedicatedThread(true); + + Node node(node_options, std::move(map_builder), &tf_buffer, + FLAGS_collect_metrics); + if (!FLAGS_load_state_filename.empty()) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); + } + + ::ros::Publisher tf_publisher = + node.node_handle()->advertise( + kTfTopic, kLatestOnlyPublisherQueueSize); + + ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster; + + ::ros::Publisher clock_publisher = + node.node_handle()->advertise( + kClockTopic, kLatestOnlyPublisherQueueSize); + + if (urdf_transforms.size() > 0) { + static_tf_broadcaster.sendTransform(urdf_transforms); + } + + ros::AsyncSpinner async_spinner(kSingleThreaded); + async_spinner.start(); + rosgraph_msgs::Clock clock; + auto clock_republish_timer = node.node_handle()->createWallTimer( + ::ros::WallDuration(kClockPublishFrequencySec), + [&clock_publisher, &clock](const ::ros::WallTimerEvent&) { + clock_publisher.publish(clock); + }, + false /* oneshot */, false /* autostart */); + + std::vector< + std::set> + bag_expected_sensor_ids; + if (configuration_basenames.size() == 1) { + const auto current_bag_expected_sensor_ids = + node.ComputeDefaultSensorIdsForMultipleBags( + {bag_trajectory_options.front()}); + bag_expected_sensor_ids = {bag_filenames.size(), + current_bag_expected_sensor_ids.front()}; + } else { + bag_expected_sensor_ids = + node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options); + } + CHECK_EQ(bag_expected_sensor_ids.size(), bag_filenames.size()); + + std::map, + cartographer::mapping::TrajectoryBuilderInterface::SensorId> + bag_topic_to_sensor_id; + PlayableBagMultiplexer playable_bag_multiplexer; + for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size(); + ++current_bag_index) { + const std::string& bag_filename = bag_filenames.at(current_bag_index); + if (!::ros::ok()) { + return; + } + for (const auto& expected_sensor_id : + bag_expected_sensor_ids.at(current_bag_index)) { + const auto bag_resolved_topic = std::make_pair( + static_cast(current_bag_index), + node.node_handle()->resolveName(expected_sensor_id.id)); + if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) { + LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag " + << current_bag_index << " resolves to topic " + << bag_resolved_topic.second << " which is already used by " + << " sensor " + << bag_topic_to_sensor_id.at(bag_resolved_topic).id; + } + bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id; + } + + playable_bag_multiplexer.AddPlayableBag(PlayableBag( + bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay, + // PlayableBag::FilteringEarlyMessageHandler is used to get an early + // peek at the tf messages in the bag and insert them into 'tf_buffer'. + // When a message is retrieved by GetNextMessage() further below, + // we will have already inserted further 'kDelay' seconds worth of + // transforms into 'tf_buffer' via this lambda. + [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) { + if (msg.isType()) { + if (FLAGS_use_bag_transforms) { + const auto tf_message = msg.instantiate(); + tf_publisher.publish(tf_message); + + for (const auto& transform : tf_message->transforms) { + try { + // We need to keep 'tf_buffer' small because it becomes very + // inefficient otherwise. We make sure that tf_messages are + // published before any data messages, so that tf lookups + // always work. + tf_buffer.setTransform(transform, "unused_authority", + msg.getTopic() == kTfStaticTopic); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + } + } + // Tell 'PlayableBag' to filter the tf message since there is no + // further use for it. + return false; + } else { + return true; + } + })); + } + + std::set bag_topics; + std::stringstream bag_topics_string; + for (const auto& topic : playable_bag_multiplexer.topics()) { + std::string resolved_topic = node.node_handle()->resolveName(topic, false); + bag_topics.insert(resolved_topic); + bag_topics_string << resolved_topic << ","; + } + bool print_topics = false; + for (const auto& entry : bag_topic_to_sensor_id) { + const std::string& resolved_topic = entry.first.second; + if (bag_topics.count(resolved_topic) == 0) { + LOG(WARNING) << "Expected resolved topic \"" << resolved_topic + << "\" not found in bag file(s)."; + print_topics = true; + } + } + if (print_topics) { + LOG(WARNING) << "Available topics in bag file(s) are " + << bag_topics_string.str(); + } + + std::unordered_map bag_index_to_trajectory_id; + const ros::Time begin_time = + // If no bags were loaded, we cannot peek the time of first message. + playable_bag_multiplexer.IsMessageAvailable() + ? playable_bag_multiplexer.PeekMessageTime() + : ros::Time(); + while (playable_bag_multiplexer.IsMessageAvailable()) { + if (!::ros::ok()) { + return; + } + + const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); + const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple); + const int bag_index = std::get<1>(next_msg_tuple); + const bool is_last_message_in_bag = std::get<2>(next_msg_tuple); + + if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) { + continue; + } + + int trajectory_id; + // Lazily add trajectories only when the first message arrives in order + // to avoid blocking the sensor queue. + if (bag_index_to_trajectory_id.count(bag_index) == 0) { + trajectory_id = + node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index), + bag_trajectory_options.at(bag_index)); + CHECK(bag_index_to_trajectory_id + .emplace(std::piecewise_construct, + std::forward_as_tuple(bag_index), + std::forward_as_tuple(trajectory_id)) + .second); + LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag " + << bag_filenames.at(bag_index); + } else { + trajectory_id = bag_index_to_trajectory_id.at(bag_index); + } + + const auto bag_topic = std::make_pair( + bag_index, + node.node_handle()->resolveName(msg.getTopic(), false /* resolve */)); + auto it = bag_topic_to_sensor_id.find(bag_topic); + if (it != bag_topic_to_sensor_id.end()) { + const std::string& sensor_id = it->second.id; + if (msg.isType()) { + node.HandleLaserScanMessage(trajectory_id, sensor_id, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleMultiEchoLaserScanMessage( + trajectory_id, sensor_id, + msg.instantiate()); + } + if (msg.isType()) { + node.HandlePointCloud2Message( + trajectory_id, sensor_id, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleImuMessage(trajectory_id, sensor_id, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleOdometryMessage(trajectory_id, sensor_id, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleNavSatFixMessage(trajectory_id, sensor_id, + msg.instantiate()); + } + if (msg.isType()) { + node.HandleLandmarkMessage( + trajectory_id, sensor_id, + msg.instantiate()); + } + } + clock.clock = msg.getTime(); + clock_publisher.publish(clock); + + if (is_last_message_in_bag) { + node.FinishTrajectory(trajectory_id); + } + } + + // Ensure the clock is republished after the bag has been finished, during the + // final optimization, serialization, and optional indefinite spinning at the + // end. + clock_republish_timer.start(); + node.RunFinalOptimization(); + + const std::chrono::time_point end_time = + std::chrono::steady_clock::now(); + const double wall_clock_seconds = + std::chrono::duration_cast>(end_time - + start_time) + .count(); + + LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s"; +#ifdef __linux__ + timespec cpu_timespec = {}; + clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec); + LOG(INFO) << "Elapsed CPU time: " + << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s"; + rusage usage; + CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno); + LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB"; +#endif + + // Serialize unless we have neither a bagfile nor an explicit state filename. + if (::ros::ok() && + !(bag_filenames.empty() && FLAGS_save_state_filename.empty())) { + const std::string state_output_filename = + FLAGS_save_state_filename.empty() + ? absl::StrCat(bag_filenames.front(), ".pbstream") + : FLAGS_save_state_filename; + LOG(INFO) << "Writing state to '" << state_output_filename << "'..."; + node.SerializeState(state_output_filename, + true /* include_unfinished_submaps */); + } + if (FLAGS_keep_running) { + LOG(INFO) << "Finished processing and waiting for shutdown."; + ::ros::waitForShutdown(); + } +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node.h new file mode 100644 index 0000000..25f4f54 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node.h @@ -0,0 +1,38 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H + +#include +#include +#include +#include + +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer_ros/node_options.h" + +namespace cartographer_ros { + +using MapBuilderFactory = + std::function( + const ::cartographer::mapping::proto::MapBuilderOptions&)>; + +void RunOfflineNode(const MapBuilderFactory& map_builder_factory); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node_main.cc new file mode 100644 index 0000000..7e6c3da --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -0,0 +1,41 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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/map_builder.h" +#include "cartographer_ros/offline_node.h" +#include "cartographer_ros/ros_log_sink.h" +#include "gflags/gflags.h" +#include "ros/ros.h" + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + ::ros::init(argc, argv, "cartographer_offline_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + + const cartographer_ros::MapBuilderFactory map_builder_factory = []( + const ::cartographer::mapping::proto::MapBuilderOptions& + map_builder_options) { + return ::cartographer::mapping::CreateMapBuilder(map_builder_options); + }; + + cartographer_ros::RunOfflineNode(map_builder_factory); + + ::ros::shutdown(); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc new file mode 100644 index 0000000..3ca601f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -0,0 +1,99 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// Publishes a frozen nav_msgs/OccupancyGrid map from serialized submaps. + +#include +#include + +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/ros_map.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "nav_msgs/OccupancyGrid.h" +#include "ros/ros.h" + +DEFINE_string(pbstream_filename, "", + "Filename of a pbstream to draw a map from."); +DEFINE_string(map_topic, "map", "Name of the published map topic."); +DEFINE_string(map_frame_id, "map", "Frame ID of the published map."); +DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); + +namespace cartographer_ros { +namespace { + +std::unique_ptr LoadOccupancyGridMsg( + const std::string& pbstream_filename, const double resolution) { + ::cartographer::io::ProtoStreamReader reader(pbstream_filename); + ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); + + LOG(INFO) << "Loading submap slices from serialized data."; + std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> + submap_slices; + ::cartographer::mapping::ValueConversionTables conversion_tables; + ::cartographer::io::DeserializeAndFillSubmapSlices( + &deserializer, &submap_slices, &conversion_tables); + CHECK(reader.eof()); + + LOG(INFO) << "Generating combined map image from submap slices."; + const auto painted_slices = + ::cartographer::io::PaintSubmapSlices(submap_slices, resolution); + return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id, + ros::Time::now()); +} + +void Run(const std::string& pbstream_filename, const std::string& map_topic, + const std::string& map_frame_id, const double resolution) { + std::unique_ptr msg_ptr = + LoadOccupancyGridMsg(pbstream_filename, resolution); + + ::ros::NodeHandle node_handle(""); + ::ros::Publisher pub = node_handle.advertise( + map_topic, kLatestOnlyPublisherQueueSize, true /*latched */); + + LOG(INFO) << "Publishing occupancy grid topic " << map_topic + << " (frame_id: " << map_frame_id + << ", resolution:" << std::to_string(resolution) << ")."; + pub.publish(*msg_ptr); + ::ros::spin(); + ::ros::shutdown(); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; + + ::ros::init(argc, argv, "cartographer_pbstream_map_publisher"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + + ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic, + FLAGS_map_frame_id, FLAGS_resolution); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc new file mode 100644 index 0000000..e92f0a0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc @@ -0,0 +1,81 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer_ros/ros_map.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string(pbstream_filename, "", + "Filename of a pbstream to draw a map from."); +DEFINE_string(map_filestem, "map", "Stem of the output files."); +DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); + +namespace cartographer_ros { +namespace { + +void Run(const std::string& pbstream_filename, const std::string& map_filestem, + const double resolution) { + ::cartographer::io::ProtoStreamReader reader(pbstream_filename); + ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); + + LOG(INFO) << "Loading submap slices from serialized data."; + std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> + submap_slices; + ::cartographer::mapping::ValueConversionTables conversion_tables; + ::cartographer::io::DeserializeAndFillSubmapSlices( + &deserializer, &submap_slices, &conversion_tables); + CHECK(reader.eof()); + + LOG(INFO) << "Generating combined map image from submap slices."; + auto result = + ::cartographer::io::PaintSubmapSlices(submap_slices, resolution); + + ::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm"); + + ::cartographer::io::Image image(std::move(result.surface)); + WritePgm(image, resolution, &pgm_writer); + + const Eigen::Vector2d origin( + -result.origin.x() * resolution, + (result.origin.y() - image.height()) * resolution); + + ::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml"); + WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; + CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing."; + + ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem, + FLAGS_resolution); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/playable_bag.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/playable_bag.cc new file mode 100644 index 0000000..f305ad8 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/playable_bag.cc @@ -0,0 +1,170 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/playable_bag.h" + +#include "absl/memory/memory.h" +#include "cartographer_ros/node_constants.h" +#include "glog/logging.h" +#include "tf2_msgs/TFMessage.h" + +namespace cartographer_ros { + +PlayableBag::PlayableBag( + const std::string& bag_filename, const int bag_id, + const ros::Time start_time, const ros::Time end_time, + const ros::Duration buffer_delay, + FilteringEarlyMessageHandler filtering_early_message_handler) + : bag_(absl::make_unique(bag_filename, rosbag::bagmode::Read)), + view_(absl::make_unique(*bag_, start_time, end_time)), + view_iterator_(view_->begin()), + finished_(false), + bag_id_(bag_id), + bag_filename_(bag_filename), + duration_in_seconds_( + (view_->getEndTime() - view_->getBeginTime()).toSec()), + message_counter_(0), + buffer_delay_(buffer_delay), + filtering_early_message_handler_( + std::move(filtering_early_message_handler)) { + AdvanceUntilMessageAvailable(); + for (const auto* connection_info : view_->getConnections()) { + topics_.insert(connection_info->topic); + } +} + +ros::Time PlayableBag::PeekMessageTime() const { + CHECK(IsMessageAvailable()); + return buffered_messages_.front().getTime(); +} + +std::tuple PlayableBag::GetBeginEndTime() const { + return std::make_tuple(view_->getBeginTime(), view_->getEndTime()); +} + +rosbag::MessageInstance PlayableBag::GetNextMessage( + cartographer_ros_msgs::BagfileProgress* progress) { + CHECK(IsMessageAvailable()); + const rosbag::MessageInstance msg = buffered_messages_.front(); + buffered_messages_.pop_front(); + AdvanceUntilMessageAvailable(); + double processed_seconds = (msg.getTime() - view_->getBeginTime()).toSec(); + if ((message_counter_ % 10000) == 0) { + LOG(INFO) << "Processed " << processed_seconds << " of " + << duration_in_seconds_ << " seconds of bag " << bag_filename_; + } + + if (progress) { + progress->current_bagfile_name = bag_filename_; + progress->current_bagfile_id = bag_id_; + progress->total_messages = view_->size(); + progress->processed_messages = message_counter_; + progress->total_seconds = duration_in_seconds_; + progress->processed_seconds = processed_seconds; + } + + return msg; +} + +bool PlayableBag::IsMessageAvailable() const { + return !buffered_messages_.empty() && + (buffered_messages_.front().getTime() < + buffered_messages_.back().getTime() - buffer_delay_); +} + +int PlayableBag::bag_id() const { return bag_id_; } + +void PlayableBag::AdvanceOneMessage() { + CHECK(!finished_); + if (view_iterator_ == view_->end()) { + finished_ = true; + return; + } + rosbag::MessageInstance& msg = *view_iterator_; + if (!filtering_early_message_handler_ || + filtering_early_message_handler_(msg)) { + buffered_messages_.push_back(msg); + } + ++view_iterator_; + ++message_counter_; +} + +void PlayableBag::AdvanceUntilMessageAvailable() { + if (finished_) { + return; + } + do { + AdvanceOneMessage(); + } while (!finished_ && !IsMessageAvailable()); +} + +PlayableBagMultiplexer::PlayableBagMultiplexer() : pnh_("~") { + bag_progress_pub_ = pnh_.advertise( + "bagfile_progress", 10); + progress_pub_interval_ = pnh_.param("bagfile_progress_pub_interval", 10.0); +} + +void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { + for (const auto& topic : playable_bag.topics()) { + topics_.insert(topic); + } + playable_bags_.push_back(std::move(playable_bag)); + CHECK(playable_bags_.back().IsMessageAvailable()); + next_message_queue_.emplace( + BagMessageItem{playable_bags_.back().PeekMessageTime(), + static_cast(playable_bags_.size() - 1)}); + bag_progress_time_map_[playable_bag.bag_id()] = ros::Time::now(); +} + +bool PlayableBagMultiplexer::IsMessageAvailable() const { + return !next_message_queue_.empty(); +} + +std::tuple +PlayableBagMultiplexer::GetNextMessage() { + CHECK(IsMessageAvailable()); + const int current_bag_index = next_message_queue_.top().bag_index; + PlayableBag& current_bag = playable_bags_.at(current_bag_index); + cartographer_ros_msgs::BagfileProgress progress; + rosbag::MessageInstance msg = current_bag.GetNextMessage(&progress); + const bool publish_progress = + current_bag.finished() || + ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >= + ros::Duration(progress_pub_interval_); + if (bag_progress_pub_.getNumSubscribers() > 0 && publish_progress) { + progress.total_bagfiles = playable_bags_.size(); + if (current_bag.finished()) { + progress.processed_seconds = current_bag.duration_in_seconds(); + } + bag_progress_pub_.publish(progress); + bag_progress_time_map_[current_bag.bag_id()] = ros::Time::now(); + } + CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp); + next_message_queue_.pop(); + if (current_bag.IsMessageAvailable()) { + next_message_queue_.emplace( + BagMessageItem{current_bag.PeekMessageTime(), current_bag_index}); + } + return std::make_tuple(std::move(msg), current_bag.bag_id(), + !current_bag.IsMessageAvailable()); +} + +ros::Time PlayableBagMultiplexer::PeekMessageTime() const { + CHECK(IsMessageAvailable()); + return next_message_queue_.top().message_timestamp; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/playable_bag.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/playable_bag.h new file mode 100644 index 0000000..1ceed06 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/playable_bag.h @@ -0,0 +1,117 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H + +#include +#include + +#include "cartographer_ros_msgs/BagfileProgress.h" +#include "ros/node_handle.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "tf2_ros/buffer.h" + +namespace cartographer_ros { + +class PlayableBag { + public: + // Handles messages early, i.e. when they are about to enter the buffer. + // Returns a boolean indicating whether the message should enter the buffer. + using FilteringEarlyMessageHandler = + std::function; + + PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time, + ros::Time end_time, ros::Duration buffer_delay, + FilteringEarlyMessageHandler filtering_early_message_handler); + + ros::Time PeekMessageTime() const; + rosbag::MessageInstance GetNextMessage( + cartographer_ros_msgs::BagfileProgress* progress); + bool IsMessageAvailable() const; + std::tuple GetBeginEndTime() const; + + int bag_id() const; + std::set topics() const { return topics_; } + double duration_in_seconds() const { return duration_in_seconds_; } + bool finished() const { return finished_; } + + private: + void AdvanceOneMessage(); + void AdvanceUntilMessageAvailable(); + + std::unique_ptr bag_; + std::unique_ptr view_; + rosbag::View::const_iterator view_iterator_; + bool finished_; + const int bag_id_; + const std::string bag_filename_; + const double duration_in_seconds_; + int message_counter_; + std::deque buffered_messages_; + const ::ros::Duration buffer_delay_; + FilteringEarlyMessageHandler filtering_early_message_handler_; + std::set topics_; +}; + +class PlayableBagMultiplexer { + public: + PlayableBagMultiplexer(); + void AddPlayableBag(PlayableBag playable_bag); + + // Returns the next message from the multiplexed (merge-sorted) message + // stream, along with the bag id corresponding to the message, and whether + // this was the last message in that bag. + std::tuple + GetNextMessage(); + + bool IsMessageAvailable() const; + ros::Time PeekMessageTime() const; + + std::set topics() const { return topics_; } + + private: + struct BagMessageItem { + ros::Time message_timestamp; + int bag_index; + struct TimestampIsGreater { + bool operator()(const BagMessageItem& l, const BagMessageItem& r) { + return l.message_timestamp > r.message_timestamp; + } + }; + }; + + ros::NodeHandle pnh_; + // Publishes information about the bag-file(s) processing and its progress + ros::Publisher bag_progress_pub_; + // Map between bagfile id and the last time when its progress was published + std::map bag_progress_time_map_; + // The time interval of publishing bag-file(s) processing in seconds + double progress_pub_interval_; + + std::vector playable_bags_; + std::priority_queue, + BagMessageItem::TimestampIsGreater> + next_message_queue_; + std::set topics_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_log_sink.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_log_sink.cc new file mode 100644 index 0000000..f5c7bbf --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_log_sink.cc @@ -0,0 +1,76 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/ros_log_sink.h" + +#include +#include +#include +#include + +#include "glog/log_severity.h" +#include "ros/console.h" + +namespace cartographer_ros { + +namespace { + +const char* GetBasename(const char* filepath) { + const char* base = std::strrchr(filepath, '/'); + return base ? (base + 1) : filepath; +} + +} // namespace + +ScopedRosLogSink::ScopedRosLogSink() : will_die_(false) { AddLogSink(this); } +ScopedRosLogSink::~ScopedRosLogSink() { RemoveLogSink(this); } + +void ScopedRosLogSink::send(const ::google::LogSeverity severity, + const char* const filename, + const char* const base_filename, const int line, + const struct std::tm* const tm_time, + const char* const message, + const size_t message_len) { + const std::string message_string = ::google::LogSink::ToString( + severity, GetBasename(filename), line, tm_time, message, message_len); + switch (severity) { + case ::google::GLOG_INFO: + ROS_INFO_STREAM(message_string); + break; + + case ::google::GLOG_WARNING: + ROS_WARN_STREAM(message_string); + break; + + case ::google::GLOG_ERROR: + ROS_ERROR_STREAM(message_string); + break; + + case ::google::GLOG_FATAL: + ROS_FATAL_STREAM(message_string); + will_die_ = true; + break; + } +} + +void ScopedRosLogSink::WaitTillSent() { + if (will_die_) { + // Give ROS some time to actually publish our message. + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_log_sink.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_log_sink.h new file mode 100644 index 0000000..28538bf --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_log_sink.h @@ -0,0 +1,45 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H + +#include + +#include "glog/logging.h" + +namespace cartographer_ros { + +// Makes Google logging use ROS logging for output while an instance of this +// class exists. +class ScopedRosLogSink : public ::google::LogSink { + public: + ScopedRosLogSink(); + ~ScopedRosLogSink() override; + + void send(::google::LogSeverity severity, const char* filename, + const char* base_filename, int line, const struct std::tm* tm_time, + const char* message, size_t message_len) override; + + void WaitTillSent() override; + + private: + bool will_die_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map.cc new file mode 100644 index 0000000..c5c0a1f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map.cc @@ -0,0 +1,49 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/ros_map.h" + +#include "absl/strings/str_cat.h" + +namespace cartographer_ros { + +void WritePgm(const ::cartographer::io::Image& image, const double resolution, + ::cartographer::io::FileWriter* file_writer) { + const std::string header = + absl::StrCat("P5\n# Cartographer map; ", resolution, " m/pixel\n", + image.width(), " ", image.height(), "\n255\n"); + file_writer->Write(header.data(), header.size()); + for (int y = 0; y < image.height(); ++y) { + for (int x = 0; x < image.width(); ++x) { + const char color = image.GetPixel(x, y)[0]; + file_writer->Write(&color, 1); + } + } +} + +void WriteYaml(const double resolution, const Eigen::Vector2d& origin, + const std::string& pgm_filename, + ::cartographer::io::FileWriter* file_writer) { + // Magic constants taken directly from ros map_saver code: + // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 + const std::string output = absl::StrCat( + "image: ", pgm_filename, "\n", "resolution: ", resolution, "\n", + "origin: [", origin.x(), ", ", origin.y(), + ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"); + file_writer->Write(output.data(), output.size()); +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map.h new file mode 100644 index 0000000..4336a16 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map.h @@ -0,0 +1,41 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H + +#include + +#include "Eigen/Core" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/image.h" +#include "cartographer/mapping/2d/map_limits.h" + +namespace cartographer_ros { + +// Write 'image' as a pgm into 'file_writer'. The resolution is used in the +// comment only' +void WritePgm(const ::cartographer::io::Image& image, const double resolution, + ::cartographer::io::FileWriter* file_writer); + +// Write the corresponding yaml into 'file_writer'. +void WriteYaml(const double resolution, const Eigen::Vector2d& origin, + const std::string& pgm_filename, + ::cartographer::io::FileWriter* file_writer); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc new file mode 100644 index 0000000..25c0e83 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc @@ -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. + */ + +#include "cartographer_ros/ros_map_writing_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/io/image.h" +#include "cartographer/io/probability_grid_points_processor.h" +#include "cartographer_ros/ros_map.h" + +namespace cartographer_ros { + +RosMapWritingPointsProcessor::RosMapWritingPointsProcessor( + const double resolution, + const ::cartographer::mapping::proto:: + ProbabilityGridRangeDataInserterOptions2D& range_data_inserter_options, + ::cartographer::io::FileWriterFactory file_writer_factory, + const std::string& filestem, + ::cartographer::io::PointsProcessor* const next) + : filestem_(filestem), + next_(next), + file_writer_factory_(file_writer_factory), + range_data_inserter_(range_data_inserter_options), + probability_grid_(::cartographer::io::CreateProbabilityGrid( + resolution, &conversion_tables_)) {} + +std::unique_ptr +RosMapWritingPointsProcessor::FromDictionary( + ::cartographer::io::FileWriterFactory file_writer_factory, + ::cartographer::common::LuaParameterDictionary* const dictionary, + ::cartographer::io::PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("resolution"), + ::cartographer::mapping::CreateProbabilityGridRangeDataInserterOptions2D( + dictionary->GetDictionary("range_data_inserter").get()), + file_writer_factory, dictionary->GetString("filestem"), next); +} + +void RosMapWritingPointsProcessor::Process( + std::unique_ptr<::cartographer::io::PointsBatch> batch) { + range_data_inserter_.Insert( + {batch->origin, ::cartographer::sensor::PointCloud(batch->points), {}}, + &probability_grid_); + next_->Process(std::move(batch)); +} + +::cartographer::io::PointsProcessor::FlushResult +RosMapWritingPointsProcessor::Flush() { + Eigen::Array2i offset; + std::unique_ptr<::cartographer::io::Image> image = + ::cartographer::io::DrawProbabilityGrid(probability_grid_, &offset); + if (image != nullptr) { + auto pgm_writer = file_writer_factory_(filestem_ + ".pgm"); + const std::string pgm_filename = pgm_writer->GetFilename(); + const auto& limits = probability_grid_.limits(); + image->Rotate90DegreesClockwise(); + + WritePgm(*image, limits.resolution(), pgm_writer.get()); + CHECK(pgm_writer->Close()); + + const Eigen::Vector2d origin( + limits.max().x() - (offset.y() + image->width()) * limits.resolution(), + limits.max().y() - + (offset.x() + image->height()) * limits.resolution()); + auto yaml_writer = file_writer_factory_(filestem_ + ".yaml"); + WriteYaml(limits.resolution(), origin, pgm_filename, yaml_writer.get()); + CHECK(yaml_writer->Close()); + } + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "ROS map writing must be configured to occur after any " + "stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL); + // The following unreachable return statement is needed to avoid a GCC bug + // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508 + return FlushResult::kFinished; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h new file mode 100644 index 0000000..f2f4f5b --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h @@ -0,0 +1,69 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" + +namespace cartographer_ros { + +// Very similar to Cartographer's ProbabilityGridPointsProcessor, but writes +// out a PGM and YAML suitable for ROS map server to consume. +class RosMapWritingPointsProcessor + : public ::cartographer::io::PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "write_ros_map"; + RosMapWritingPointsProcessor( + double resolution, + const ::cartographer::mapping::proto:: + ProbabilityGridRangeDataInserterOptions2D& + range_data_inserter_options, + ::cartographer::io::FileWriterFactory file_writer_factory, + const std::string& filestem, PointsProcessor* next); + RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete; + RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) = + delete; + + static std::unique_ptr FromDictionary( + ::cartographer::io::FileWriterFactory file_writer_factory, + ::cartographer::common::LuaParameterDictionary* dictionary, + PointsProcessor* next); + + ~RosMapWritingPointsProcessor() override {} + + void Process(std::unique_ptr<::cartographer::io::PointsBatch> batch) override; + FlushResult Flush() override; + + private: + const std::string filestem_; + PointsProcessor* const next_; + ::cartographer::io::FileWriterFactory file_writer_factory_; + ::cartographer::mapping::ProbabilityGridRangeDataInserter2D + range_data_inserter_; + ::cartographer::mapping::ValueConversionTables conversion_tables_; + ::cartographer::mapping::ProbabilityGrid probability_grid_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/rosbag_validate_main.cc new file mode 100644 index 0000000..c52c700 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -0,0 +1,425 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include + +#include "absl/memory/memory.h" +#include "cartographer/common/histogram.h" +#include "cartographer_ros/msg_conversion.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "nav_msgs/Odometry.h" +#include "ros/ros.h" +#include "ros/time.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_msgs/TFMessage.h" +#include "tf2_ros/buffer.h" +#include "urdf/model.h" + +DEFINE_string(bag_filename, "", "Bag to process."); +DEFINE_bool(dump_timing, false, + "Dump per-sensor timing information in files called " + "timing_.csv in the current directory."); + +namespace cartographer_ros { +namespace { + +struct FrameProperties { + ros::Time last_timestamp; + std::string topic; + std::vector time_deltas; + std::unique_ptr timing_file; + std::string data_type; +}; + +const double kMinLinearAcceleration = 3.; +const double kMaxLinearAcceleration = 30.; +const double kTimeDeltaSerializationSensorWarning = 0.1; +const double kTimeDeltaSerializationSensorError = 0.5; +const double kMinAverageAcceleration = 9.5; +const double kMaxAverageAcceleration = 10.5; +const double kMaxGapPointsData = 0.1; +const double kMaxGapImuData = 0.05; +const std::set kPointDataTypes = { + std::string( + ros::message_traits::DataType::value()), + std::string(ros::message_traits::DataType< + sensor_msgs::MultiEchoLaserScan>::value()), + std::string( + ros::message_traits::DataType::value())}; + +std::unique_ptr CreateTimingFile(const std::string& frame_id) { + auto timing_file = absl::make_unique( + std::string("timing_") + frame_id + ".csv", std::ios_base::out); + + (*timing_file) + << "# Timing information for sensor with frame id: " << frame_id + << std::endl + << "# Columns are in order" << std::endl + << "# - packet index of the packet in the bag, first packet is 1" + << std::endl + << "# - timestamp when rosbag wrote the packet, i.e. " + "rosbag::MessageInstance::getTime().toNSec()" + << std::endl + << "# - timestamp when data was acquired, i.e. " + "message.header.stamp.toNSec()" + << std::endl + << "#" << std::endl + << "# The data can be read in python using" << std::endl + << "# import numpy" << std::endl + << "# np.loadtxt(, dtype='uint64')" << std::endl; + + return timing_file; +} + +void CheckImuMessage(const sensor_msgs::Imu& imu_message) { + auto linear_acceleration = ToEigen(imu_message.linear_acceleration); + if (std::isnan(linear_acceleration.norm()) || + linear_acceleration.norm() < kMinLinearAcceleration || + linear_acceleration.norm() > kMaxLinearAcceleration) { + LOG_FIRST_N(WARNING, 3) + << "frame_id " << imu_message.header.frame_id << " time " + << imu_message.header.stamp.toNSec() << ": IMU linear acceleration is " + << linear_acceleration.norm() << " m/s^2," + << " expected is [" << kMinLinearAcceleration << ", " + << kMaxLinearAcceleration << "] m/s^2." + << " (It should include gravity and be given in m/s^2.)" + << " linear_acceleration " << linear_acceleration.transpose(); + } +} + +bool IsValidPose(const geometry_msgs::Pose& pose) { + return ToRigid3d(pose).IsValid(); +} + +void CheckOdometryMessage(const nav_msgs::Odometry& message) { + const auto& pose = message.pose.pose; + if (!IsValidPose(pose)) { + LOG_FIRST_N(ERROR, 3) << "frame_id " << message.header.frame_id << " time " + << message.header.stamp.toNSec() + << ": Odometry pose is invalid." + << " pose " << pose; + } +} + +void CheckTfMessage(const tf2_msgs::TFMessage& message) { + for (const auto& transform : message.transforms) { + if (transform.header.frame_id == "map") { + LOG_FIRST_N(ERROR, 1) + << "Input contains transform message from frame_id \"" + << transform.header.frame_id << "\" to child_frame_id \"" + << transform.child_frame_id + << "\". This is almost always output published by cartographer and " + "should not appear as input. (Unless you have some complex " + "remove_frames configuration, this is will not work. Simplest " + "solution is to record input without cartographer running.)"; + } + } +} + +bool IsPointDataType(const std::string& data_type) { + return (kPointDataTypes.count(data_type) != 0); +} + +class RangeDataChecker { + public: + template + void CheckMessage(const MessageType& message) { + const std::string& frame_id = message.header.frame_id; + ros::Time current_time_stamp = message.header.stamp; + RangeChecksum current_checksum; + cartographer::common::Time time_from, time_to; + ReadRangeMessage(message, ¤t_checksum, &time_from, &time_to); + auto previous_time_to_it = frame_id_to_previous_time_to_.find(frame_id); + if (previous_time_to_it != frame_id_to_previous_time_to_.end() && + previous_time_to_it->second >= time_from) { + if (previous_time_to_it->second >= time_to) { + LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id + << "\" is not sequential in time." + << "Previous range message ends at time " + << previous_time_to_it->second + << ", current one at time " << time_to; + } else { + LOG_FIRST_N(WARNING, 3) + << "Sensor with frame_id \"" << frame_id + << "\" measurements overlap in time. " + << "Previous range message, ending at time stamp " + << previous_time_to_it->second + << ", must finish before current range message, " + << "which ranges from " << time_from << " to " << time_to; + } + double overlap = cartographer::common::ToSeconds( + previous_time_to_it->second - time_from); + auto it = frame_id_to_max_overlap_duration_.find(frame_id); + if (it == frame_id_to_max_overlap_duration_.end() || + overlap > frame_id_to_max_overlap_duration_.at(frame_id)) { + frame_id_to_max_overlap_duration_[frame_id] = overlap; + } + } + frame_id_to_previous_time_to_[frame_id] = time_to; + if (current_checksum.first == 0) { + return; + } + auto it = frame_id_to_range_checksum_.find(frame_id); + if (it != frame_id_to_range_checksum_.end()) { + RangeChecksum previous_checksum = it->second; + if (previous_checksum == current_checksum) { + LOG_FIRST_N(ERROR, 3) + << "Sensor with frame_id \"" << frame_id + << "\" sends exactly the same range measurements multiple times. " + << "Range data at time " << current_time_stamp + << " equals preceding data with " << current_checksum.first + << " points."; + } + } + frame_id_to_range_checksum_[frame_id] = current_checksum; + } + + void PrintReport() { + for (auto& it : frame_id_to_max_overlap_duration_) { + LOG(WARNING) << "Sensor with frame_id \"" << it.first + << "\" range measurements have longest overlap of " + << it.second << " s"; + } + } + + private: + typedef std::pair + RangeChecksum; + + template + static void ReadRangeMessage(const MessageType& message, + RangeChecksum* range_checksum, + cartographer::common::Time* from, + cartographer::common::Time* to) { + auto point_cloud_time = ToPointCloudWithIntensities(message); + const cartographer::sensor::TimedPointCloud& point_cloud = + std::get<0>(point_cloud_time).points; + *to = std::get<1>(point_cloud_time); + *from = *to + cartographer::common::FromSeconds(point_cloud[0].time); + Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); + for (const auto& point : point_cloud) { + points_sum.head<3>() += point.position; + points_sum[3] += point.time; + } + if (point_cloud.size() > 0) { + double first_point_relative_time = point_cloud[0].time; + double last_point_relative_time = (*point_cloud.rbegin()).time; + if (first_point_relative_time > 0) { + LOG_FIRST_N(ERROR, 1) + << "Sensor with frame_id \"" << message.header.frame_id + << "\" has range data that has positive relative time " + << first_point_relative_time << " s, must negative or zero."; + } + if (last_point_relative_time != 0) { + LOG_FIRST_N(INFO, 1) + << "Sensor with frame_id \"" << message.header.frame_id + << "\" has range data whose last point has relative time " + << last_point_relative_time << " s, should be zero."; + } + } + *range_checksum = {point_cloud.size(), points_sum}; + } + + std::map frame_id_to_range_checksum_; + std::map + frame_id_to_previous_time_to_; + std::map frame_id_to_max_overlap_duration_; +}; + +void Run(const std::string& bag_filename, const bool dump_timing) { + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag); + + std::map frame_id_to_properties; + size_t message_index = 0; + int num_imu_messages = 0; + double sum_imu_acceleration = 0.; + RangeDataChecker range_data_checker; + for (const rosbag::MessageInstance& message : view) { + ++message_index; + std::string frame_id; + ros::Time time; + if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + CheckImuMessage(*msg); + num_imu_messages++; + sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm(); + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + CheckOdometryMessage(*msg); + } else if (message.isType()) { + auto msg = message.instantiate(); + CheckTfMessage(*msg); + continue; + } else { + continue; + } + + bool first_packet = false; + if (!frame_id_to_properties.count(frame_id)) { + frame_id_to_properties.emplace( + frame_id, + FrameProperties{time, message.getTopic(), std::vector(), + dump_timing ? CreateTimingFile(frame_id) : nullptr, + message.getDataType()}); + first_packet = true; + } + + auto& entry = frame_id_to_properties.at(frame_id); + if (!first_packet) { + const double delta_t_sec = (time - entry.last_timestamp).toSec(); + if (delta_t_sec <= 0) { + LOG_FIRST_N(ERROR, 3) + << "Sensor with frame_id \"" << frame_id + << "\" jumps backwards in time, i.e. timestamps are not strictly " + "increasing. Make sure that the bag contains the data for each " + "frame_id sorted by header.stamp, i.e. the order in which they " + "were acquired from the sensor."; + } + entry.time_deltas.push_back(delta_t_sec); + } + + if (entry.topic != message.getTopic()) { + LOG_FIRST_N(ERROR, 3) + << "frame_id \"" << frame_id + << "\" is send on multiple topics. It was seen at least on " + << entry.topic << " and " << message.getTopic(); + } + entry.last_timestamp = time; + + if (dump_timing) { + CHECK(entry.timing_file != nullptr); + (*entry.timing_file) << message_index << "\t" + << message.getTime().toNSec() << "\t" + << time.toNSec() << std::endl; + } + + double duration_serialization_sensor = (time - message.getTime()).toSec(); + if (std::abs(duration_serialization_sensor) > + kTimeDeltaSerializationSensorWarning) { + std::stringstream stream; + stream << "frame_id \"" << frame_id << "\" on topic " + << message.getTopic() << " has serialization time " + << message.getTime() << " but sensor time " << time + << " differing by " << duration_serialization_sensor << " s."; + if (std::abs(duration_serialization_sensor) > + kTimeDeltaSerializationSensorError) { + LOG_FIRST_N(ERROR, 3) << stream.str(); + } else { + LOG_FIRST_N(WARNING, 1) << stream.str(); + } + } + } + bag.close(); + + range_data_checker.PrintReport(); + + if (num_imu_messages > 0) { + double average_imu_acceleration = sum_imu_acceleration / num_imu_messages; + if (std::isnan(average_imu_acceleration) || + average_imu_acceleration < kMinAverageAcceleration || + average_imu_acceleration > kMaxAverageAcceleration) { + LOG(ERROR) << "Average IMU linear acceleration is " + << average_imu_acceleration << " m/s^2," + << " expected is [" << kMinAverageAcceleration << ", " + << kMaxAverageAcceleration + << "] m/s^2. Linear acceleration data " + "should include gravity and be given in m/s^2."; + } + } + + constexpr int kNumBucketsForHistogram = 10; + for (const auto& entry_pair : frame_id_to_properties) { + const FrameProperties& frame_properties = entry_pair.second; + float max_time_delta = + *std::max_element(frame_properties.time_deltas.begin(), + frame_properties.time_deltas.end()); + if (IsPointDataType(frame_properties.data_type) && + max_time_delta > kMaxGapPointsData) { + LOG(ERROR) << "Point data (frame_id: \"" << entry_pair.first + << "\") has a large gap, largest is " << max_time_delta + << " s, recommended is [0.0005, 0.05] s with no jitter."; + } + if (frame_properties.data_type == + ros::message_traits::DataType::value() && + max_time_delta > kMaxGapImuData) { + LOG(ERROR) << "IMU data (frame_id: \"" << entry_pair.first + << "\") has a large gap, largest is " << max_time_delta + << " s, recommended is [0.0005, 0.005] s with no jitter."; + } + + cartographer::common::Histogram histogram; + for (float time_delta : frame_properties.time_deltas) { + histogram.Add(time_delta); + } + LOG(INFO) << "Time delta histogram for consecutive messages on topic \"" + << frame_properties.topic << "\" (frame_id: \"" + << entry_pair.first << "\"):\n" + << histogram.ToString(kNumBucketsForHistogram); + } + + if (dump_timing) { + for (const auto& entry_pair : frame_id_to_properties) { + entry_pair.second.timing_file->close(); + CHECK(*entry_pair.second.timing_file) + << "Could not write timing information for \"" << entry_pair.first + << "\""; + } + } +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; + ::cartographer_ros::Run(FLAGS_bag_filename, FLAGS_dump_timing); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/sensor_bridge.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/sensor_bridge.cc new file mode 100644 index 0000000..5f61d36 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -0,0 +1,352 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/sensor_bridge.h" + +#include "absl/memory/memory.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" + +namespace cartographer_ros { + +namespace carto = ::cartographer; + +using carto::transform::Rigid3d; + +namespace { + +// 检查frame_id是否带有'/',如果带则报错 +const std::string& CheckNoLeadingSlash(const std::string& frame_id) { + if (frame_id.size() > 0) { + CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id + << " should not start with a /. See 1.7 in " + "http://wiki.ros.org/tf2/Migration."; + } + return frame_id; +} + +} // namespace + +/** + * @brief 构造函数, 并且初始化TfBridge + * + * @param[in] num_subdivisions_per_laser_scan 一帧数据分成几次发送 + * @param[in] tracking_frame 数据都转换到tracking_frame + * @param[in] lookup_transform_timeout_sec 查找tf的超时时间 + * @param[in] tf_buffer tf_buffer + * @param[in] trajectory_builder 轨迹构建器 + */ +SensorBridge::SensorBridge( + const int num_subdivisions_per_laser_scan, + const std::string& tracking_frame, + const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer, + carto::mapping::TrajectoryBuilderInterface* const trajectory_builder) + : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), + tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), + trajectory_builder_(trajectory_builder) {} + +// 将ros格式的里程计数据 转成tracking frame的pose, 再转成carto的里程计数据类型 +std::unique_ptr SensorBridge::ToOdometryData( + const nav_msgs::Odometry::ConstPtr& msg) { + const carto::common::Time time = FromRos(msg->header.stamp); + // 找到 tracking坐标系 到 里程计的child_frame_id 的坐标变换, 所以下方要对sensor_to_tracking取逆 + const auto sensor_to_tracking = tf_bridge_.LookupToTracking( + time, CheckNoLeadingSlash(msg->child_frame_id)); + if (sensor_to_tracking == nullptr) { + return nullptr; + } + + // 将里程计的footprint的pose转成tracking_frame的pose, 再转成carto的里程计数据类型 + return absl::make_unique( + carto::sensor::OdometryData{ + time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()}); +} + +// 调用trajectory_builder_的AddSensorData进行数据的处理 +void SensorBridge::HandleOdometryMessage( + const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { + // 数据类型与数据坐标系的转换 + std::unique_ptr odometry_data = + ToOdometryData(msg); + if (odometry_data != nullptr) { + // tag: 这个trajectory_builder_是谁, 讲map_builder时候再揭晓 + trajectory_builder_->AddSensorData( + sensor_id, + carto::sensor::OdometryData{odometry_data->time, odometry_data->pose}); + } +} + +// 将ros格式的gps数据转换成相对坐标系下的坐标,再调用trajectory_builder_的AddSensorData进行数据的处理 +void SensorBridge::HandleNavSatFixMessage( + const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) { + const carto::common::Time time = FromRos(msg->header.stamp); + // 如果不是固定解,就加入一个固定的空位姿 + if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) { + trajectory_builder_->AddSensorData( + sensor_id, + carto::sensor::FixedFramePoseData{time, absl::optional()}); + return; + } + + // 确定ecef原点到局部坐标系的坐标变换 + if (!ecef_to_local_frame_.has_value()) { + ecef_to_local_frame_ = + ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude); + LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = " + << msg->latitude << ", long = " << msg->longitude << "."; + } + + // 通过这个坐标变换 乘以 之后的gps数据,就相当于减去了一个固定的坐标,从而得到了gps数据间的相对坐标变换 + trajectory_builder_->AddSensorData( + sensor_id, carto::sensor::FixedFramePoseData{ + time, absl::optional(Rigid3d::Translation( + ecef_to_local_frame_.value() * + LatLongAltToEcef(msg->latitude, msg->longitude, + msg->altitude)))}); +} + +// 处理Landmark数据, 先转成carto的格式,再传入trajectory_builder_ +void SensorBridge::HandleLandmarkMessage( + const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + // 将在ros中自定义的LandmarkList类型的数据, 转成LandmarkData + auto landmark_data = ToLandmarkData(*msg); + + // 获取 tracking_frame到landmark的frame 的坐标变换 + auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking( + landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id)); + + // 将数据转到tracking_frame下 + if (tracking_from_landmark_sensor != nullptr) { + for (auto& observation : landmark_data.landmark_observations) { + observation.landmark_to_tracking_transform = + *tracking_from_landmark_sensor * + observation.landmark_to_tracking_transform; + } + } + // 调用trajectory_builder_的AddSensorData进行数据的处理 + trajectory_builder_->AddSensorData(sensor_id, landmark_data); +} + +// 进行数据类型转换与坐标系的转换 +std::unique_ptr SensorBridge::ToImuData( + const sensor_msgs::Imu::ConstPtr& msg) { + // 检查是否存在线性加速度与角速度 + CHECK_NE(msg->linear_acceleration_covariance[0], -1) + << "Your IMU data claims to not contain linear acceleration measurements " + "by setting linear_acceleration_covariance[0] to -1. Cartographer " + "requires this data to work. See " + "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html."; + CHECK_NE(msg->angular_velocity_covariance[0], -1) + << "Your IMU data claims to not contain angular velocity measurements " + "by setting angular_velocity_covariance[0] to -1. Cartographer " + "requires this data to work. See " + "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html."; + + const carto::common::Time time = FromRos(msg->header.stamp); + const auto sensor_to_tracking = tf_bridge_.LookupToTracking( + time, CheckNoLeadingSlash(msg->header.frame_id)); + if (sensor_to_tracking == nullptr) { + return nullptr; + } + // 推荐将imu的坐标系当做tracking frame + CHECK(sensor_to_tracking->translation().norm() < 1e-5) + << "The IMU frame must be colocated with the tracking frame. " + "Transforming linear acceleration into the tracking frame will " + "otherwise be imprecise."; + // 进行坐标系的转换 + return absl::make_unique(carto::sensor::ImuData{ + time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), + sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); +} + +// 调用trajectory_builder_的AddSensorData进行数据的处理 +void SensorBridge::HandleImuMessage(const std::string& sensor_id, + const sensor_msgs::Imu::ConstPtr& msg) { + std::unique_ptr imu_data = ToImuData(msg); + if (imu_data != nullptr) { + trajectory_builder_->AddSensorData( + sensor_id, + carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration, + imu_data->angular_velocity}); + } +} + +// 处理LaserScan数据, 先转成点云,再传入trajectory_builder_ +void SensorBridge::HandleLaserScanMessage( + //。。。消息格式要改 + const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { + //const std::string& sensor_id, const sensor::SemLaserScan::ConstPtr& msg) { +//。。。PointCloudWithIntensities结构要加颜色 + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; + //。。。tie加一个color ToPointCloudWithIntensities（）加 + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); +} +// 处理语义LaserScan数据, 先转成点云,再传入trajectory_builder_ +// void SensorBridge::HandleSemLaserScanMessage( +// const std::string& sensor_id, const sensor_msgs::SemLaserScan::ConstPtr& msg) { +// carto::sensor::PointCloudWithIntensities point_cloud; +// carto::common::Time time; +// std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); +// HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); +// } + +// 处理MultiEchoLaserScan数据, 先转成点云,再传入trajectory_builder_ +void SensorBridge::HandleMultiEchoLaserScanMessage( + const std::string& sensor_id, + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); +} + +// 处理ros格式的PointCloud2, 先转成点云,再传入trajectory_builder_ +void SensorBridge::HandlePointCloud2Message( + const std::string& sensor_id, + const sensor_msgs::PointCloud2::ConstPtr& msg) { + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points); +} + +const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } + +// 根据参数配置,将一帧雷达数据分成几段, 再传入trajectory_builder_ +void SensorBridge::HandleLaserScan( + const std::string& sensor_id, const carto::common::Time time, + const std::string& frame_id, + const carto::sensor::PointCloudWithIntensities& points) { + if (points.points.empty()) { + return; + } + // CHECK_LE: 小于等于 + CHECK_LE(points.points.back().time, 0.f); + // TODO(gaschler): Use per-point time instead of subdivisions. + + // 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1 + for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { + const size_t start_index = + points.points.size() * i / num_subdivisions_per_laser_scan_; + const size_t end_index = + points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_; + + // 生成分段的点云 + //。。。改TimedPointCloud结构为PointCloudWithIntensities + //carto::sensor::TimedPointCloud subdivision( + carto::sensor::PointCloudWithIntensities subdivision; + subdivision.points.assign( + points.points.begin() + start_index, points.points.begin() + end_index); + //。。。增加color + subdivision.intensities.assign( + points.intensities.begin() + start_index, points.intensities.begin() + end_index); + if (start_index == end_index) { + continue; + } + //。。。 + //const double time_to_subdivision_end = subdivision.back().time; + const double time_to_subdivision_end = subdivision.points.back().time; + // `subdivision_time` is the end of the measurement so sensor::Collator will + // send all other sensor data first. + const carto::common::Time subdivision_time = + time + carto::common::FromSeconds(time_to_subdivision_end); + + auto it = sensor_to_previous_subdivision_time_.find(sensor_id); + if (it != sensor_to_previous_subdivision_time_.end() && + // 上一段点云的时间不应该大于等于这一段点云的时间 + it->second >= subdivision_time) { + LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor " + << sensor_id << " because previous subdivision time " + << it->second << " is not before current subdivision time " + << subdivision_time; + continue; + } + // 更新对应sensor_id的时间戳 + sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time; + + // 检查点云的时间 + for (auto& point : subdivision.points) { + point.time -= time_to_subdivision_end; + } + //。。。 + //CHECK_EQ(subdivision.back().time, 0.f); + CHECK_EQ(subdivision.points.back().time, 0.f); + // 将分段后的点云 subdivision 传入 trajectory_builder_ + HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); + } // for +} + +// 雷达相关的数据最终的处理函数 +// 先将数据转到tracking坐标系下,再调用trajectory_builder_的AddSensorData进行数据的处理 + +/** + * @brief + * + * @param[in] sensor_id 数据的话题 + * @param[in] time 点云的时间戳(最后一个点的时间) + * @param[in] frame_id 点云的frame + * @param[in] ranges 雷达坐标系下的TimedPointCloud格式的点云 + */ +//... +void SensorBridge::HandleRangefinder( + const std::string& sensor_id, const carto::common::Time time, + //。。。const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { + const std::string& frame_id, const ::cartographer::sensor::PointCloudWithIntensities& ranges) { + //。。。if (!ranges.empty()) { + if (!ranges.points.empty()) { + //。。。CHECK_LE(ranges.back().time, 0.f); + CHECK_LE(ranges.points.back().time, 0.f); + } + const auto sensor_to_tracking = + tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); + + // 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin + // 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_ + if (sensor_to_tracking != nullptr) { + trajectory_builder_->AddSensorData( + sensor_id, carto::sensor::TimedPointCloudData{ + time, + sensor_to_tracking->translation().cast(), + // 将点云从雷达坐标系下转到tracking_frame坐标系系下 + carto::sensor::TransformTimedPointCloud( + ranges, sensor_to_tracking->cast()).points, + carto::sensor::TransformTimedPointCloud( + ranges, sensor_to_tracking->cast()).intensities} ); // 强度始终为空 + } +} + +void SensorBridge::HandleRangefinder( + const std::string& sensor_id, const carto::common::Time time, + const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { + if (!ranges.empty()) { + CHECK_LE(ranges.back().time, 0.f); + } + const auto sensor_to_tracking = + tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); + if (sensor_to_tracking != nullptr) { + trajectory_builder_->AddSensorData( + sensor_id, carto::sensor::TimedPointCloudData{ + time, sensor_to_tracking->translation().cast(), + carto::sensor::TransformTimedPointCloud( + ranges, sensor_to_tracking->cast())}); + } +} + +} // namespace cartographer_ros + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/sensor_bridge.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/sensor_bridge.h new file mode 100644 index 0000000..17fbd1a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -0,0 +1,107 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H + +#include + +#include "absl/types/optional.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/tf_bridge.h" +#include "cartographer_ros_msgs/LandmarkList.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/TransformStamped.h" +#include "nav_msgs/OccupancyGrid.h" +#include "nav_msgs/Odometry.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/NavSatFix.h" +#include "sensor_msgs/PointCloud2.h" + +namespace cartographer_ros { + +// Converts ROS messages into SensorData in tracking frame for the MapBuilder. +class SensorBridge { + public: + explicit SensorBridge( + int num_subdivisions_per_laser_scan, const std::string& tracking_frame, + double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer, + ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder); + + SensorBridge(const SensorBridge&) = delete; + SensorBridge& operator=(const SensorBridge&) = delete; + + std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData( + const nav_msgs::Odometry::ConstPtr& msg); + void HandleOdometryMessage(const std::string& sensor_id, + const nav_msgs::Odometry::ConstPtr& msg); + void HandleNavSatFixMessage(const std::string& sensor_id, + const sensor_msgs::NavSatFix::ConstPtr& msg); + void HandleLandmarkMessage( + const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + + std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( + const sensor_msgs::Imu::ConstPtr& msg); + void HandleImuMessage(const std::string& sensor_id, + const sensor_msgs::Imu::ConstPtr& msg); + void HandleLaserScanMessage(const std::string& sensor_id, + const sensor_msgs::LaserScan::ConstPtr& msg); + void HandleMultiEchoLaserScanMessage( + const std::string& sensor_id, + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + void HandlePointCloud2Message(const std::string& sensor_id, + const sensor_msgs::PointCloud2::ConstPtr& msg); + + const TfBridge& tf_bridge() const; + + private: + void HandleLaserScan( + const std::string& sensor_id, ::cartographer::common::Time start_time, + const std::string& frame_id, + const ::cartographer::sensor::PointCloudWithIntensities& points); + void HandleRangefinder(const std::string& sensor_id, + ::cartographer::common::Time time, + const std::string& frame_id, + //... + const ::cartographer::sensor::TimedPointCloud& ranges); + //... + void HandleRangefinder(const std::string& sensor_id, + ::cartographer::common::Time time, + const std::string& frame_id, + //... + const ::cartographer::sensor::PointCloudWithIntensities& ranges); + + const int num_subdivisions_per_laser_scan_; + std::map + sensor_to_previous_subdivision_time_; + const TfBridge tf_bridge_; + ::cartographer::mapping::TrajectoryBuilderInterface* const + trajectory_builder_; + + absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/submap.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/submap.cc new file mode 100644 index 0000000..111eb16 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/submap.cc @@ -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. + */ + +#include "cartographer_ros/submap.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/port.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros_msgs/StatusCode.h" +#include "cartographer_ros_msgs/SubmapQuery.h" + +namespace cartographer_ros { + +std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( + const ::cartographer::mapping::SubmapId& submap_id, + ros::ServiceClient* client) { + ::cartographer_ros_msgs::SubmapQuery srv; + srv.request.trajectory_id = submap_id.trajectory_id; + srv.request.submap_index = submap_id.submap_index; + if (!client->call(srv) || + srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) { + return nullptr; + } + if (srv.response.textures.empty()) { + return nullptr; + } + auto response = absl::make_unique<::cartographer::io::SubmapTextures>(); + response->version = srv.response.submap_version; + for (const auto& texture : srv.response.textures) { + const std::string compressed_cells(texture.cells.begin(), + texture.cells.end()); + response->textures.emplace_back(::cartographer::io::SubmapTexture{ + ::cartographer::io::UnpackTextureData(compressed_cells, texture.width, + texture.height), + texture.width, texture.height, texture.resolution, + ToRigid3d(texture.slice_pose)}); + } + return response; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/submap.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/submap.h new file mode 100644 index 0000000..0df2215 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/submap.h @@ -0,0 +1,40 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H + +#include +#include +#include + +#include "cartographer/io/image.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' +// on error. +std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( + const ::cartographer::mapping::SubmapId& submap_id, + ros::ServiceClient* client); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/tf_bridge.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/tf_bridge.cc new file mode 100644 index 0000000..29f80b1 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/tf_bridge.cc @@ -0,0 +1,57 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/tf_bridge.h" + +#include "absl/memory/memory.h" +#include "cartographer_ros/msg_conversion.h" + +namespace cartographer_ros { + +TfBridge::TfBridge(const std::string& tracking_frame, + const double lookup_transform_timeout_sec, + const tf2_ros::Buffer* buffer) + : tracking_frame_(tracking_frame), + lookup_transform_timeout_sec_(lookup_transform_timeout_sec), + buffer_(buffer) {} + +std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( + const ::cartographer::common::Time time, + const std::string& frame_id) const { + ::ros::Duration timeout(lookup_transform_timeout_sec_); + std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; + try { + const ::ros::Time latest_tf_time = + buffer_ + ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.), + timeout) + .header.stamp; + const ::ros::Time requested_time = ToRos(time); + if (latest_tf_time >= requested_time) { + // We already have newer data, so we do not wait. Otherwise, we would wait + // for the full 'timeout' even if we ask for data that is too old. + timeout = ::ros::Duration(0.); + } + return absl::make_unique<::cartographer::transform::Rigid3d>( + ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id, + requested_time, timeout))); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + return nullptr; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/tf_bridge.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/tf_bridge.h new file mode 100644 index 0000000..a74f85a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/tf_bridge.h @@ -0,0 +1,50 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros/time_conversion.h" +#include "tf2_ros/buffer.h" + +namespace cartographer_ros { + +class TfBridge { + public: + TfBridge(const std::string& tracking_frame, + double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer); + ~TfBridge() {} + + TfBridge(const TfBridge&) = delete; + TfBridge& operator=(const TfBridge&) = delete; + + // Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at + // 'time'. + std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking( + ::cartographer::common::Time time, const std::string& frame_id) const; + + private: + const std::string tracking_frame_; + const double lookup_transform_timeout_sec_; + const tf2_ros::Buffer* const buffer_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion.cc new file mode 100644 index 0000000..1075c40 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion.cc @@ -0,0 +1,47 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/time_conversion.h" + +#include "cartographer/common/time.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +::ros::Time ToRos(::cartographer::common::Time time) { + int64_t uts_timestamp = ::cartographer::common::ToUniversal(time); + int64_t ns_since_unix_epoch = + (uts_timestamp - + ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * + 10000000ll) * + 100ll; + ::ros::Time ros_time; + ros_time.fromNSec(ns_since_unix_epoch); + return ros_time; +} + +// TODO(pedrofernandez): Write test. +::cartographer::common::Time FromRos(const ::ros::Time& time) { + // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000", + // exactly 719162 days before the Unix epoch. + return ::cartographer::common::FromUniversal( + (time.sec + + ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) * + 10000000ll + + (time.nsec + 50) / 100); // + 50 to get the rounding correct. +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion.h new file mode 100644 index 0000000..dda5bb5 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion.h @@ -0,0 +1,31 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H + +#include "cartographer/common/time.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +::ros::Time ToRos(::cartographer::common::Time time); + +::cartographer::common::Time FromRos(const ::ros::Time& time); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion_test.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion_test.cc new file mode 100644 index 0000000..3b2d572 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/time_conversion_test.cc @@ -0,0 +1,44 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/time_conversion.h" + +#include + +#include "cartographer/common/time.h" +#include "gtest/gtest.h" +#include "ros/ros.h" + +namespace cartographer_ros { +namespace { + +TEST(TimeConversion, testToRos) { + std::vector values = {0, 1469091375, 1466481821, 1462101382, + 1468238899}; + for (int64_t seconds_since_epoch : values) { + ::ros::Time ros_now; + ros_now.fromSec(seconds_since_epoch); + ::cartographer::common::Time cartographer_now( + ::cartographer::common::FromSeconds( + seconds_since_epoch + + ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds)); + EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now)); + EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now)); + } +} + +} // namespace +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/trajectory_options.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/trajectory_options.cc new file mode 100644 index 0000000..143f5b2 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/trajectory_options.h" + +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/time_conversion.h" +#include "glog/logging.h" + +namespace cartographer_ros { + +namespace { + +void CheckTrajectoryOptions(const TrajectoryOptions& options) { + CHECK_GE(options.num_subdivisions_per_laser_scan, 1); + CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans + + options.num_point_clouds, + 1) + << "Configuration error: 'num_laser_scans', " + "'num_multi_echo_laser_scans' and 'num_point_clouds' are " + "all zero, but at least one is required."; +} + +} // namespace + +TrajectoryOptions CreateTrajectoryOptions( + ::cartographer::common::LuaParameterDictionary* const + lua_parameter_dictionary) { + TrajectoryOptions options; + options.trajectory_builder_options = + ::cartographer::mapping::CreateTrajectoryBuilderOptions( + lua_parameter_dictionary->GetDictionary("trajectory_builder").get()); + options.tracking_frame = + lua_parameter_dictionary->GetString("tracking_frame"); + options.published_frame = + lua_parameter_dictionary->GetString("published_frame"); + options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); + options.provide_odom_frame = + lua_parameter_dictionary->GetBool("provide_odom_frame"); + options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry"); + options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat"); + options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks"); + options.publish_frame_projected_to_2d = + lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d"); + options.num_laser_scans = + lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans"); + options.num_multi_echo_laser_scans = + lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans"); + options.num_subdivisions_per_laser_scan = + lua_parameter_dictionary->GetNonNegativeInt( + "num_subdivisions_per_laser_scan"); + options.num_point_clouds = + lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds"); + options.rangefinder_sampling_ratio = + lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio"); + options.odometry_sampling_ratio = + lua_parameter_dictionary->GetDouble("odometry_sampling_ratio"); + options.fixed_frame_pose_sampling_ratio = + lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio"); + options.imu_sampling_ratio = + lua_parameter_dictionary->GetDouble("imu_sampling_ratio"); + options.landmarks_sampling_ratio = + lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio"); + CheckTrajectoryOptions(options); + return options; +} +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/trajectory_options.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/trajectory_options.h new file mode 100644 index 0000000..93817b7 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/trajectory_options.h @@ -0,0 +1,55 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" + +namespace cartographer_ros { + +struct TrajectoryOptions { + ::cartographer::mapping::proto::TrajectoryBuilderOptions + trajectory_builder_options; + std::string tracking_frame; + std::string published_frame; + std::string odom_frame; + bool provide_odom_frame; + bool use_odometry; + bool use_nav_sat; + bool use_landmarks; + bool publish_frame_projected_to_2d; + int num_laser_scans; + int num_multi_echo_laser_scans; + int num_subdivisions_per_laser_scan; + int num_point_clouds; + double rangefinder_sampling_ratio; + double odometry_sampling_ratio; + double fixed_frame_pose_sampling_ratio; + double imu_sampling_ratio; + double landmarks_sampling_ratio; +}; + +TrajectoryOptions CreateTrajectoryOptions( + ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/urdf_reader.cc b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/urdf_reader.cc new file mode 100644 index 0000000..e389e18 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/urdf_reader.cc @@ -0,0 +1,59 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_ros/urdf_reader.h" + +#include +#include + +#include "cartographer_ros/msg_conversion.h" +#include "urdf/model.h" + +namespace cartographer_ros { + +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) { + urdf::Model model; + CHECK(model.initFile(urdf_filename)); +#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS + std::vector links; +#else + std::vector > links; +#endif + model.getLinks(links); + std::vector transforms; + for (const auto& link : links) { + if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { + continue; + } + + const urdf::Pose& pose = + link->parent_joint->parent_to_joint_origin_transform; + geometry_msgs::TransformStamped transform; + transform.transform = + ToGeometryMsgTransform(cartographer::transform::Rigid3d( + Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), + Eigen::Quaterniond(pose.rotation.w, pose.rotation.x, + pose.rotation.y, pose.rotation.z))); + transform.child_frame_id = link->name; + transform.header.frame_id = link->getParent()->name; + tf_buffer->setTransform(transform, "urdf", true /* is_static */); + transforms.push_back(transform); + } + return transforms; +} + +} // namespace cartographer_ros diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/urdf_reader.h b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/urdf_reader.h new file mode 100644 index 0000000..93d8d11 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/cartographer_ros/urdf_reader.h @@ -0,0 +1,32 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H + +#include + +#include "cartographer/common/port.h" +#include "tf2_ros/buffer.h" + +namespace cartographer_ros { + +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua new file mode 100644 index 0000000..9a2d3bf --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua @@ -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. + +-- WARNING: we create a lot of X-Rays of a potentially large space in this +-- pipeline. For example, running over the +-- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can +-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size +-- of a pixel in a X-Ray. +VOXEL_SIZE = 5e-2 + +include "transform.lua" + +options = { + tracking_frame = "base_link", + pipeline = { + { + action = "min_max_range_filter", + min_range = 1., + max_range = 60., + }, + { + action = "dump_num_points", + }, + + -- Gray X-Rays. These only use geometry to color pixels. + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz_all", + transform = YZ_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy_all", + transform = XY_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz_all", + transform = XZ_TRANSFORM, + }, + + -- We now use the intensities to color our points. We apply a linear + -- transform to clamp our intensity values into [0, 255] and then use this + -- value for RGB of our points. Every stage in the pipeline after this now + -- receives colored points. + -- + -- We write xrays again. These now use geometry and the intensities to + -- color pixels - they look quite similar, just a little lighter. + { + action = "intensity_to_color", + min_intensity = 0., + max_intensity = 4095., + }, + + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz_all_intensity", + transform = YZ_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy_all_intensity", + transform = XY_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz_all_intensity", + transform = XZ_TRANSFORM, + }, + + -- We also write a PLY file at this stage, because gray points look good. + -- The points in the PLY can be visualized using + -- https://github.com/cartographer-project/point_cloud_viewer. + { + action = "write_ply", + filename = "points.ply", + }, + + -- Now we recolor our points by frame and write another batch of X-Rays. It + -- is visible in them what was seen by the horizontal and the vertical + -- laser. + { + action = "color_points", + frame_id = "horizontal_laser_link", + color = { 255., 0., 0. }, + }, + { + action = "color_points", + frame_id = "vertical_laser_link", + color = { 0., 255., 0. }, + }, + + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz_all_color", + transform = YZ_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy_all_color", + transform = XY_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz_all_color", + transform = XZ_TRANSFORM, + }, + } +} + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_2d_ci.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_2d_ci.lua new file mode 100644 index 0000000..baf09bc --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_2d_ci.lua @@ -0,0 +1,56 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +-- WARNING: we create a lot of X-Rays of a potentially large space in this +-- pipeline. For example, running over the +-- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can +-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size +-- of a pixel in a X-Ray. +VOXEL_SIZE = 5e-2 + +include "transform.lua" + +options = { + tracking_frame = "base_link", + pipeline = { + { + action = "min_max_range_filter", + min_range = 1., + max_range = 60., + }, + { + action = "dump_num_points", + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz_all", + transform = YZ_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy_all", + transform = XY_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz_all", + transform = XZ_TRANSFORM, + }, + } +} + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua new file mode 100644 index 0000000..43c2bd9 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua @@ -0,0 +1,86 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +VOXEL_SIZE = 5e-2 + +include "transform.lua" + +options = { + tracking_frame = "base_link", + pipeline = { + { + action = "min_max_range_filter", + min_range = 1., + max_range = 60., + }, + { + action = "dump_num_points", + }, + + -- Gray X-Rays. These only use geometry to color pixels. + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz_all", + transform = YZ_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy_all", + transform = XY_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz_all", + transform = XZ_TRANSFORM, + }, + + -- Now we recolor our points by frame and write another batch of X-Rays. It + -- is visible in them what was seen by the horizontal and the vertical + -- laser. + { + action = "color_points", + frame_id = "horizontal_vlp16_link", + color = { 255., 0., 0. }, + }, + { + action = "color_points", + frame_id = "vertical_vlp16_link", + color = { 0., 255., 0. }, + }, + + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz_all_color", + transform = YZ_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy_all_color", + transform = XY_TRANSFORM, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz_all_color", + transform = XZ_TRANSFORM, + }, + } +} + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_ros_map.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_ros_map.lua new file mode 100644 index 0000000..faa92e6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/assets_writer_ros_map.lua @@ -0,0 +1,36 @@ +-- Copyright 2018 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +options = { + tracking_frame = "base_link", + pipeline = { + { + action = "min_max_range_filter", + min_range = 1., + max_range = 60., + }, + { + action = "write_ros_map", + range_data_inserter = { + insert_free_space = true, + hit_probability = 0.55, + miss_probability = 0.49, + }, + filestem = "map", + resolution = 0.05, + } + } +} + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d.lua new file mode 100644 index 0000000..8d29069 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d.lua @@ -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. + +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "base_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, + use_odometry = false, + use_nav_sat = false, + use_landmarks = false, + num_laser_scans = 0, + num_multi_echo_laser_scans = 1, + num_subdivisions_per_laser_scan = 10, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_localization.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_localization.lua new file mode 100644 index 0000000..c5da92c --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_localization.lua @@ -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. + +include "backpack_2d.lua" + +TRAJECTORY_BUILDER.pure_localization_trimmer = { + max_submaps_to_keep = 3, +} +POSE_GRAPH.optimize_every_n_nodes = 20 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua new file mode 100644 index 0000000..09afbe6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua @@ -0,0 +1,27 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +include "backpack_2d_localization.lua" + +-- output map to base_link for evaluation +options.provide_odom_frame = false +POSE_GRAPH.optimization_problem.log_solver_summary = true + +-- fast localization +MAP_BUILDER.num_background_threads = 12 +POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio +POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio +POSE_GRAPH.max_num_final_iterations = 1 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_server.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_server.lua new file mode 100644 index 0000000..342f1b4 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_2d_server.lua @@ -0,0 +1,19 @@ +-- Copyright 2018 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +include "map_builder_server.lua" + +MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true + +return MAP_BUILDER_SERVER diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_3d.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_3d.lua new file mode 100644 index 0000000..b5c05f3 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_3d.lua @@ -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 "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "base_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, + use_odometry = false, + use_nav_sat = false, + use_landmarks = false, + num_laser_scans = 0, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 2, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160 + +MAP_BUILDER.use_trajectory_builder_3d = true +MAP_BUILDER.num_background_threads = 7 +POSE_GRAPH.optimization_problem.huber_scale = 5e2 +POSE_GRAPH.optimize_every_n_nodes = 320 +POSE_GRAPH.constraint_builder.sampling_ratio = 0.03 +POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10 +POSE_GRAPH.constraint_builder.min_score = 0.62 +POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_3d_localization.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_3d_localization.lua new file mode 100644 index 0000000..3e677f5 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/backpack_3d_localization.lua @@ -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. + +include "backpack_3d.lua" + +TRAJECTORY_BUILDER.pure_localization_trimmer = { + max_submaps_to_keep = 3, +} +POSE_GRAPH.optimize_every_n_nodes = 100 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/demo_2d.rviz b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/demo_2d.rviz new file mode 100644 index 0000000..2620eb2 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/demo_2d.rviz @@ -0,0 +1,251 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Submaps1 + - /PointCloud21 + Splitter Ratio: 0.600671 + Tree Height: 821 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160;165 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + horizontal_laser_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + vertical_laser_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + horizontal_laser_link: + {} + imu_link: + {} + vertical_laser_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + horizontal_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + vertical_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: Submaps + Enabled: true + Name: Submaps + Submap query service: /submap_query + Topic: /submap_list + Tracking frame: base_link + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05 + Style: Flat Squares + Topic: /scan_matched_points2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /trajectory_node_list + Name: Trajectories + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /landmark_poses_list + Name: Landmark Poses + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /constraint_list + Name: Constraints + Namespaces: + "": true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 100; 100; 100 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: 0 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Name: Current View + Near Clip Distance: 0.01 + Scale: 40 + Target Frame: + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1123 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 24 diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/demo_3d.rviz b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/demo_3d.rviz new file mode 100644 index 0000000..e061b6a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/demo_3d.rviz @@ -0,0 +1,313 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Submaps1 + - /PointCloud23 + - /PointCloud23/Autocompute Value Bounds1 + Splitter Ratio: 0.600671 + Tree Height: 817 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + horizontal_vlp16_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + vertical_vlp16_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + horizontal_vlp16_link: + {} + imu_link: + {} + vertical_vlp16_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + horizontal_vlp16_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + vertical_vlp16_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 7.6265 + Min Value: 5.66667 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 20; 220; 20 + Color Transformer: FlatColor + Decay Time: 0.1 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 200 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /horizontal_laser_3d + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5.6087 + Min Value: 3.77875 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 240; 220; 20 + Color Transformer: FlatColor + Decay Time: 0.1 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 200 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /vertical_laser_3d + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: Submaps + Enabled: true + Name: Submaps + Submap query service: /submap_query + Topic: /submap_list + Tracking frame: base_link + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 20 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05 + Style: Flat Squares + Topic: /scan_matched_points2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /trajectory_node_list + Name: Trajectories + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /landmark_poses_list + Name: Landmark Poses + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /constraint_list + Name: Constraints + Namespaces: + "": true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 100; 100; 100 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: 0 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Name: Current View + Near Clip Distance: 0.01 + Scale: 10 + Target Frame: + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1123 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 24 diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/downLaser.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/downLaser.lua new file mode 100644 index 0000000..4b91758 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/downLaser.lua @@ -0,0 +1,52 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "down_laser_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = false, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 4 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 10 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = false +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 --可能要改 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 100 + +return options + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/mycloud.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/mycloud.lua new file mode 100644 index 0000000..228a054 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/mycloud.lua @@ -0,0 +1,52 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "cloud_link", + published_frame = "cloud_link", + odom_frame = "odom", + provide_odom_frame = false, + publish_frame_projected_to_2d = false, + use_odometry = false, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 0, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 1, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 8 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = false +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 1 + +return options + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/mylua.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/mylua.lua new file mode 100644 index 0000000..6f247cd --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/mylua.lua @@ -0,0 +1,52 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 2, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 8 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 100 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 100 + +return options + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/myluanew.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/myluanew.lua new file mode 100644 index 0000000..1e46c29 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/myluanew.lua @@ -0,0 +1,52 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 4 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 1 + +return options + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/pointcloud.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/pointcloud.lua new file mode 100644 index 0000000..a4a3285 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/pointcloud.lua @@ -0,0 +1,54 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 0, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 1, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 4 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. + + +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 8.9 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 1 + +return options + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/pr2.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/pr2.lua new file mode 100644 index 0000000..ed6d473 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/pr2.lua @@ -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. + +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "base_footprint", + published_frame = "base_footprint", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, + use_odometry = false, + use_nav_sat = false, + use_landmarks = false, + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true + +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.use_imu_data = false +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.) + +POSE_GRAPH.optimization_problem.huber_scale = 1e2 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/revo_lds.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/revo_lds.lua new file mode 100644 index 0000000..0951fb0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/revo_lds.lua @@ -0,0 +1,48 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "up_laser_link", + published_frame = "up_laser_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = false, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 5 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = false +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 35 +POSE_GRAPH.constraint_builder.min_score = 0.65 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/taurob_tracker.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/taurob_tracker.lua new file mode 100644 index 0000000..1097018 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/taurob_tracker.lua @@ -0,0 +1,60 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "base_link", + published_frame = "odom", + odom_frame = "odom", + provide_odom_frame = false, + publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 180 +TRAJECTORY_BUILDER_3D.min_range = 0.5 +TRAJECTORY_BUILDER_3D.max_range = 20. +TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40. + +MAP_BUILDER.use_trajectory_builder_3d = true +MAP_BUILDER.num_background_threads = 7 +POSE_GRAPH.optimization_problem.huber_scale = 5e2 +POSE_GRAPH.optimize_every_n_nodes = 40 +POSE_GRAPH.constraint_builder.sampling_ratio = 0.03 +POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10 +POSE_GRAPH.constraint_builder.min_score = 0.62 +POSE_GRAPH.constraint_builder.log_matches = true + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/transform.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/transform.lua new file mode 100644 index 0000000..205f232 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/transform.lua @@ -0,0 +1,28 @@ +-- Copyright 2017 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +XY_TRANSFORM = { + translation = { 0., 0., 0. }, + rotation = { 0., -math.pi / 2., 0., }, +} + +XZ_TRANSFORM = { + translation = { 0., 0., 0. }, + rotation = { 0. , 0., -math.pi / 2, }, +} + +YZ_TRANSFORM = { + translation = { 0., 0., 0. }, + rotation = { 0. , 0., math.pi, }, +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/twoLaser.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/twoLaser.lua new file mode 100644 index 0000000..ce5ec48 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/twoLaser.lua @@ -0,0 +1,52 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_link", + published_frame = "odom", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + + num_laser_scans = 2, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 4 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 8. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 --可能要改 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 0 + +return options + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/visualize_pbstream.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/visualize_pbstream.lua new file mode 100644 index 0000000..e941ab1 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/visualize_pbstream.lua @@ -0,0 +1,21 @@ +-- Copyright 2018 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +include "backpack_2d.lua" + +POSE_GRAPH.constraint_builder.sampling_ratio = 0 +POSE_GRAPH.global_sampling_ratio = 0 +POSE_GRAPH.optimize_every_n_nodes = 0 + +return options diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/wyk.lua b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/wyk.lua new file mode 100644 index 0000000..ad86bba --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/configuration_files/wyk.lua @@ -0,0 +1,53 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_link", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + use_pose_extrapolator = false, + + + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +MAP_BUILDER.num_background_threads = 8 + +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 10000 +TRAJECTORY_BUILDER_2D.min_range = 0.15 +TRAJECTORY_BUILDER_2D.max_range = 6. +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. +TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 +POSE_GRAPH.optimization_problem.huber_scale = 1e2 +POSE_GRAPH.optimize_every_n_nodes = 0 +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.global_constraint_search_after_n_seconds = 20 +POSE_GRAPH.max_num_final_iterations = 1 + +return options \ No newline at end of file diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_backpack_2d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_backpack_2d.launch new file mode 100644 index 0000000..939b6d7 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_backpack_2d.launch @@ -0,0 +1,28 @@ + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_backpack_3d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_backpack_3d.launch new file mode 100644 index 0000000..2b3d074 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_backpack_3d.launch @@ -0,0 +1,27 @@ + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_ros_map.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_ros_map.launch new file mode 100644 index 0000000..a5c4dca --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/assets_writer_ros_map.launch @@ -0,0 +1,33 @@ + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/backpack_2d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/backpack_2d.launch new file mode 100644 index 0000000..0697378 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/backpack_2d.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/backpack_3d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/backpack_3d.launch new file mode 100644 index 0000000..52dea59 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/backpack_3d.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_2d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_2d.launch new file mode 100644 index 0000000..248e14b --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_2d.launch @@ -0,0 +1,26 @@ + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_2d_localization.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_2d_localization.launch new file mode 100644 index 0000000..e91e99f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_2d_localization.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_3d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_3d.launch new file mode 100644 index 0000000..f5bfc3a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_3d.launch @@ -0,0 +1,26 @@ + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_3d_localization.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_3d_localization.launch new file mode 100644 index 0000000..94304a1 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_backpack_3d_localization.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_pr2.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_pr2.launch new file mode 100644 index 0000000..991a12b --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_pr2.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + - map + - odom_combined + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_revo_lds.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_revo_lds.launch new file mode 100644 index 0000000..b06855f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_revo_lds.launch @@ -0,0 +1,16 @@ + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_taurob_tracker.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_taurob_tracker.launch new file mode 100644 index 0000000..1af7f65 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/demo_taurob_tracker.launch @@ -0,0 +1,26 @@ + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/downLaser.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/downLaser.launch new file mode 100644 index 0000000..3d2213e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/downLaser.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + " + \ No newline at end of file diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/grpc_demo_backpack_2d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/grpc_demo_backpack_2d.launch new file mode 100644 index 0000000..9f942a7 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/grpc_demo_backpack_2d.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch new file mode 100644 index 0000000..64bf878 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mycloud.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mycloud.launch new file mode 100644 index 0000000..4e7d6d7 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mycloud.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunch.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunch.launch new file mode 100644 index 0000000..bd130c8 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunch.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunchnew.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunchnew.launch new file mode 100644 index 0000000..1ddb2b9 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunchnew.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunchnew1.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunchnew1.launch new file mode 100644 index 0000000..480d73d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/mylaunchnew1.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_backpack_2d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_backpack_2d.launch new file mode 100644 index 0000000..bb8ae0a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_backpack_2d.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_backpack_3d.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_backpack_3d.launch new file mode 100644 index 0000000..8dfa0f8 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_backpack_3d.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_node.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_node.launch new file mode 100644 index 0000000..cf404a0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/offline_node.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/pointcloud.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/pointcloud.launch new file mode 100644 index 0000000..4bb2a4f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/pointcloud.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/taurob_tracker.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/taurob_tracker.launch new file mode 100644 index 0000000..4111fa0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/taurob_tracker.launch @@ -0,0 +1,29 @@ + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/twoLaser.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/twoLaser.launch new file mode 100644 index 0000000..777fc61 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/twoLaser.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/visualize_pbstream.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/visualize_pbstream.launch new file mode 100644 index 0000000..cf5b7e0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/visualize_pbstream.launch @@ -0,0 +1,29 @@ + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/wyk.launch b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/wyk.launch new file mode 100644 index 0000000..8179fcf --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/launch/wyk.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + " + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/package.xml b/carto_ws/src/cartographer_ros-master/cartographer_ros/package.xml new file mode 100644 index 0000000..cd64648 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/package.xml @@ -0,0 +1,71 @@ + + + + + cartographer_ros + 1.0.0 + + Cartographer is a system that provides real-time simultaneous localization + and mapping (SLAM) in 2D and 3D across multiple platforms and sensor + configurations. This package provides Cartographer's ROS integration. + + + The Cartographer Authors + + Apache 2.0 + + https://github.com/cartographer-project/cartographer_ros + + + The Cartographer Authors + + + catkin + + git + google-mock + protobuf-dev + python3-sphinx + + cartographer + cartographer_ros_msgs + geometry_msgs + libgflags-dev + libgoogle-glog-dev + libpcl-all-dev + message_runtime + nav_msgs + pcl_conversions + robot_state_publisher + rosbag + roscpp + roslaunch + roslib + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_ros + urdf + visualization_msgs + + rosunit + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/cartographer_grpc_server.sh b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/cartographer_grpc_server.sh new file mode 100644 index 0000000..fd767b9 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/cartographer_grpc_server.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -xe +cartographer_grpc_server $@ diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh new file mode 100644 index 0000000..53822c5 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh @@ -0,0 +1,74 @@ +#!/bin/bash + +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +MAPFILE="$1" +BAGFILE="$2" +if [ "$#" -ne 2 ]; then + echo "\n\nUsage: $0 \n" + exit 1; +fi + +LAUNCHSCRIPT=' + + + + +' +echo $LAUNCHSCRIPT | roslaunch - + +LAUNCHSCRIPT=' + + + + + + + + + + +' +echo $LAUNCHSCRIPT | roslaunch - + +rosrun cartographer_ros cartographer_dev_trajectory_comparison \ + -bag_filename $BAGFILE.tf-result.bag \ + -pbstream_filename $BAGFILE.pbstream diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py new file mode 100644 index 0000000..919a748 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2018 Magazino GmbH +# The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import itertools +import random + +import rospy +from tf import transformations +from cartographer_ros_msgs.msg import LandmarkEntry, LandmarkList + +DESC = ''' +Samples a number of random landmarks at a specified rate. +Can be used to test the timing of landmark input or the effect of erroneous +landmarks with duplicate IDs (if --allow_duplicate_ids is set). + +For example: + +./publish_fake_random_landmarks.py \\ + --publish_period 0.1 \\ + --id_vocabulary A B C \\ + --id_length 5 \\ + --sample_period 1.0 + +will publish empty landmark lists at 10Hz and random landmarks every second. +IDs are also sampled, using the cartesian product of the provided "vocabulary". +In the above example, a sampled ID could be e.g. "AACBC" (length=5). +''' + +TOPIC = "landmark" + + +class LandmarkSamplerOptions(object): + + def __init__(self, *args, **kwargs): + self.allow_duplicate_ids = False + self.id_vocabulary = {} + self.id_length = 0 + self.max_distance = 0. + self.num_landmarks = 0 + self.rotation_weight = 0. + self.translation_weight = 0. + for name, arg in kwargs.items(): + setattr(self, name, arg) + + +class LandmarkIdSampler(object): + + def __init__(self, id_vocabulary, id_length): + # Precompute all combinations of the symbols in the vocabulary. + # WARNING: can be huge, check before potentially blocking the system. + if len(id_vocabulary)**id_length > 1e6: + raise ValueError("ID sampling space is too large") + self.sampling_space = list( + itertools.product(*(id_vocabulary for i in range(id_length)))) + + def sample_id(self): + # Draw a random combination of symbols and stringify it. + random_index = random.randint(0, len(self.sampling_space) - 1) + sampled_id = "".join(self.sampling_space[random_index]) + return sampled_id + + +class LandmarkSampler(object): + + def __init__(self, options): + if not isinstance(options, LandmarkSamplerOptions): + raise TypeError("expected LandmarkSamplerOptions") + self.options = options + rospy.loginfo("Initializing landmark ID sampler...") + self.landmark_id_sampler = LandmarkIdSampler(options.id_vocabulary, + options.id_length) + self._sampled_ids = [] + + def random_landmark(self): + landmark = LandmarkEntry() + landmark.translation_weight = self.options.translation_weight + landmark.rotation_weight = self.options.rotation_weight + landmark.id = self.landmark_id_sampler.sample_id() + if landmark.id in self._sampled_ids: + if not self.options.allow_duplicate_ids: + rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id)) + return None + else: + rospy.logwarn("Duplicate ID: {}".format(landmark.id)) + self._sampled_ids.append(landmark.id) + + vector = transformations.random_vector(3) * self.options.max_distance + landmark.tracking_from_landmark_transform.position.x = vector[0] + landmark.tracking_from_landmark_transform.position.y = vector[1] + landmark.tracking_from_landmark_transform.position.z = vector[2] + + quaternion = transformations.random_quaternion() + landmark.tracking_from_landmark_transform.orientation.x = quaternion[0] + landmark.tracking_from_landmark_transform.orientation.y = quaternion[1] + landmark.tracking_from_landmark_transform.orientation.z = quaternion[2] + landmark.tracking_from_landmark_transform.orientation.w = quaternion[3] + return landmark + + def random_landmark_list(self): + landmark_list = LandmarkList() + landmark_list.header.stamp = rospy.Time.now() + for _ in range(self.options.num_landmarks): + random_landmark = self.random_landmark() + if random_landmark is not None: + landmark_list.landmarks.append(random_landmark) + return landmark_list + + +class SampledLandmarkPublisher(object): + + def __init__(self, publish_period, sample_period, landmark_sampler_options): + self.landmark_sampler = LandmarkSampler(landmark_sampler_options) + rospy.loginfo("Publishing landmarks to topic: {}".format(TOPIC)) + self.publisher = rospy.Publisher(TOPIC, LandmarkList, queue_size=1) + self.publish_timer = rospy.Timer( + rospy.Duration(publish_period), self.publish_empty_landmark_list) + self.sample_timer = rospy.Timer( + rospy.Duration(sample_period), self.publish_random_landmark_list) + + def publish_random_landmark_list(self, timer_event): + self.publisher.publish(self.landmark_sampler.random_landmark_list()) + + def publish_empty_landmark_list(self, timer_event): + landmark_list = LandmarkList(rospy.Header(stamp=rospy.Time.now()), []) + self.publisher.publish(landmark_list) + + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser( + description=DESC, formatter_class=argparse.RawTextHelpFormatter) + parser.add_argument("--translation_weight", type=float, default=1e5) + parser.add_argument("--rotation_weight", type=float, default=1e5) + parser.add_argument( + "--publish_period", + type=float, + default=0.1, + help="Baseline period for publishing empty landmark lists.") + parser.add_argument( + "--sample_period", + type=float, + default=5., + help="Period at which randomly sampled landmarks are published.") + parser.add_argument( + "--num_landmarks", + type=int, + default=5, + help="The number of random landmarks published simultaneously.") + parser.add_argument( + "--max_distance", + type=float, + default=1.0, + help="Maximum distance of a random landmark to the tracking frame.") + parser.add_argument( + "--id_vocabulary", + nargs='+', + default={"a", "b", "c", "1", "2", "3"}, + help="Set of symbols that can appear in random landmark IDs.") + parser.add_argument( + "--id_length", + type=int, + default=5, + help="The length of the random landmark IDs (number of symbols).") + parser.add_argument( + "--allow_duplicate_ids", + action="store_true", + help="Publish landmarks with IDs that have already been published.") + + args, unknown = parser.parse_known_args(rospy.myargv()[1:]) + rospy.init_node("landmark_sampler") + + landmark_sampler_options = LandmarkSamplerOptions(**args.__dict__) + sampler = SampledLandmarkPublisher(args.publish_period, args.sample_period, + landmark_sampler_options) + + rospy.spin() diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/remove_leading_slashes.py b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/remove_leading_slashes.py new file mode 100644 index 0000000..388f3a9 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/remove_leading_slashes.py @@ -0,0 +1,53 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""A simple tool to remove leading slashes from frame names.""" + +import argparse +import os +import rosbag + + +def ParseArgs(): + argument_parser = argparse.ArgumentParser( + description="Removes leading slashes from frame names.") + argument_parser.add_argument("input", type=str, help="Input bag") + return argument_parser.parse_args() + + +def RewriteMsg(msg): + if hasattr(msg, "header"): + if msg.header.frame_id.startswith("/"): + msg.header.frame_id = msg.header.frame_id[1:] + if hasattr(msg, "child_frame_id"): + if msg.child_frame_id.startswith("/"): + msg.child_frame_id = msg.child_frame_id[1:] + if hasattr(msg, "transforms"): + for transform_msg in msg.transforms: + RewriteMsg(transform_msg) + + +def Main(): + options = ParseArgs() + with rosbag.Bag(os.path.splitext(options.input)[0] + ".filtered.bag", + "w") as outbag: + for topic, msg, t in rosbag.Bag(options.input).read_messages(): + RewriteMsg(msg) + outbag.write(topic, msg, msg.header.stamp if msg._has_header else t) + + +if __name__ == "__main__": + Main() diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/tf_remove_frames.py b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/tf_remove_frames.py new file mode 100644 index 0000000..04e2789 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/scripts/tf_remove_frames.py @@ -0,0 +1,40 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rospy +from tf.msg import tfMessage + + +def main(): + rospy.init_node('tf_remove_frames') + publisher = rospy.Publisher('/tf_out', tfMessage, queue_size=1) + remove_frames = rospy.get_param('~remove_frames', []) + + def callback(msg): + msg.transforms = [ + t for t in msg.transforms + if t.header.frame_id.lstrip('/') not in remove_frames and + t.child_frame_id.lstrip('/') not in remove_frames + ] + publisher.publish(msg) + + rospy.Subscriber('/tf_in', tfMessage, callback) + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/backpack_2d.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/backpack_2d.urdf new file mode 100644 index 0000000..c9c8f97 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/backpack_2d.urdf @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/backpack_3d.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/backpack_3d.urdf new file mode 100644 index 0000000..0d71046 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/backpack_3d.urdf @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/downLaser.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/downLaser.urdf new file mode 100644 index 0000000..66d30f0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/downLaser.urdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/myurdf.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/myurdf.urdf new file mode 100644 index 0000000..968a1f5 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/myurdf.urdf @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/myurdfnew.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/myurdfnew.urdf new file mode 100644 index 0000000..30a0227 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/myurdfnew.urdf @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/pointcloud.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/pointcloud.urdf new file mode 100644 index 0000000..f07a2fe --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/pointcloud.urdf @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/twoLaser.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/twoLaser.urdf new file mode 100644 index 0000000..ec034bb --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/twoLaser.urdf @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/wyk.urdf b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/wyk.urdf new file mode 100644 index 0000000..d123515 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros/urdf/wyk.urdf @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/CHANGELOG.rst b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/CHANGELOG.rst new file mode 100644 index 0000000..9f73069 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cartographer_ros_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.0 (2017-11-23) +------------------ +* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0 + +0.2.0 (2017-06-19) +------------------ +* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0 + +0.1.0 (2017-05-18) +------------------ +* First unstable development release diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/CMakeLists.txt b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/CMakeLists.txt new file mode 100644 index 0000000..d828955 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/CMakeLists.txt @@ -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. + +cmake_minimum_required(VERSION 2.8.3) + +project(cartographer_ros_msgs) +SET(CMAKE_BUILD_TYPE Debug) + +set(PACKAGE_DEPENDENCIES + geometry_msgs + std_msgs +) + +find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES}) + +add_message_files( + DIRECTORY msg + FILES + BagfileProgress.msg + HistogramBucket.msg + LandmarkEntry.msg + LandmarkList.msg + MetricFamily.msg + MetricLabel.msg + Metric.msg + StatusCode.msg + StatusResponse.msg + SubmapEntry.msg + SubmapList.msg + SubmapTexture.msg + TrajectoryStates.msg +) + +add_service_files( + DIRECTORY srv + FILES + FinishTrajectory.srv + GetTrajectoryStates.srv + ReadMetrics.srv + StartTrajectory.srv + SubmapQuery.srv + TrajectoryQuery.srv + WriteState.srv +) + +generate_messages( + DEPENDENCIES + ${PACKAGE_DEPENDENCIES} +) + +catkin_package( + CATKIN_DEPENDS + ${PACKAGE_DEPENDENCIES} + message_runtime +) diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/BagfileProgress.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/BagfileProgress.msg new file mode 100644 index 0000000..88a810e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/BagfileProgress.msg @@ -0,0 +1,23 @@ +# +# Licensed under the Apache License, Version 2.0 (the 'License'); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an 'AS IS' BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +# Contains general information about the bagfiles processing progress + +string current_bagfile_name +uint32 current_bagfile_id +uint32 total_bagfiles +uint32 total_messages +uint32 processed_messages +float32 total_seconds +float32 processed_seconds diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/HistogramBucket.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/HistogramBucket.msg new file mode 100644 index 0000000..09cd5f7 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/HistogramBucket.msg @@ -0,0 +1,19 @@ +# 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# A histogram bucket counts values x for which: +# previous_boundary < x <= bucket_boundary +# holds. +float64 bucket_boundary +float64 count diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/LandmarkEntry.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/LandmarkEntry.msg new file mode 100644 index 0000000..a7ded6e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/LandmarkEntry.msg @@ -0,0 +1,18 @@ +# 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +string id +geometry_msgs/Pose tracking_from_landmark_transform +float64 translation_weight +float64 rotation_weight diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/LandmarkList.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/LandmarkList.msg new file mode 100644 index 0000000..a8e25ef --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/LandmarkList.msg @@ -0,0 +1,16 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +std_msgs/Header header +cartographer_ros_msgs/LandmarkEntry[] landmarks diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/Metric.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/Metric.msg new file mode 100644 index 0000000..6d886d6 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/Metric.msg @@ -0,0 +1,26 @@ +# 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +uint8 TYPE_COUNTER=0 +uint8 TYPE_GAUGE=1 +uint8 TYPE_HISTOGRAM=2 + +uint8 type +cartographer_ros_msgs/MetricLabel[] labels + +# TYPE_COUNTER or TYPE_GAUGE +float64 value + +# TYPE_HISTOGRAM +cartographer_ros_msgs/HistogramBucket[] counts_by_bucket diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/MetricFamily.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/MetricFamily.msg new file mode 100644 index 0000000..3996abe --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/MetricFamily.msg @@ -0,0 +1,17 @@ +# 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +string name +string description +cartographer_ros_msgs/Metric[] metrics diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/MetricLabel.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/MetricLabel.msg new file mode 100644 index 0000000..12eb1f0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/MetricLabel.msg @@ -0,0 +1,16 @@ +# 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +string key +string value diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/StatusCode.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/StatusCode.msg new file mode 100644 index 0000000..bd0005d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/StatusCode.msg @@ -0,0 +1,32 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Definition of status code constants equivalent to the gRPC API. +# https://developers.google.com/maps-booking/reference/grpc-api/status_codes +uint8 OK=0 +uint8 CANCELLED=1 +uint8 UNKNOWN=2 +uint8 INVALID_ARGUMENT=3 +uint8 DEADLINE_EXCEEDED=4 +uint8 NOT_FOUND=5 +uint8 ALREADY_EXISTS=6 +uint8 PERMISSION_DENIED=7 +uint8 RESOURCE_EXHAUSTED=8 +uint8 FAILED_PRECONDITION=9 +uint8 ABORTED=10 +uint8 OUT_OF_RANGE=11 +uint8 UNIMPLEMENTED=12 +uint8 INTERNAL=13 +uint8 UNAVAILABLE=14 +uint8 DATA_LOSS=15 diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/StatusResponse.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/StatusResponse.msg new file mode 100644 index 0000000..5b79139 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/StatusResponse.msg @@ -0,0 +1,17 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# A common message type to indicate the outcome of a service call. +uint8 code +string message diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapEntry.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapEntry.msg new file mode 100644 index 0000000..2a38f7c --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapEntry.msg @@ -0,0 +1,19 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +int32 trajectory_id +int32 submap_index +int32 submap_version +geometry_msgs/Pose pose +bool is_frozen diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapList.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapList.msg new file mode 100644 index 0000000..79a5e39 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapList.msg @@ -0,0 +1,16 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +std_msgs/Header header +cartographer_ros_msgs/SubmapEntry[] submap diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapTexture.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapTexture.msg new file mode 100644 index 0000000..31a2e91 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/SubmapTexture.msg @@ -0,0 +1,19 @@ +# Copyright 2017 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +uint8[] cells +int32 width +int32 height +float64 resolution +geometry_msgs/Pose slice_pose diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/TrajectoryStates.msg b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/TrajectoryStates.msg new file mode 100644 index 0000000..8f3a43b --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/msg/TrajectoryStates.msg @@ -0,0 +1,22 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the 'License'); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an 'AS IS' BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +uint8 ACTIVE = 0 +uint8 FINISHED = 1 +uint8 FROZEN = 2 +uint8 DELETED = 3 + +std_msgs/Header header +int32[] trajectory_id +uint8[] trajectory_state diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/package.xml b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/package.xml new file mode 100644 index 0000000..e267133 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/package.xml @@ -0,0 +1,43 @@ + + + + + cartographer_ros_msgs + 1.0.0 + + ROS messages for the cartographer_ros package. + + + The Cartographer Authors + + Apache 2.0 + + https://github.com/cartographer-project/cartographer_ros + + + The Cartographer Authors + + + catkin + + geometry_msgs + std_msgs + + message_generation + + message_runtime + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/FinishTrajectory.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/FinishTrajectory.srv new file mode 100644 index 0000000..6e7d0c2 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/FinishTrajectory.srv @@ -0,0 +1,17 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +int32 trajectory_id +--- +cartographer_ros_msgs/StatusResponse status diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv new file mode 100644 index 0000000..5ae7a90 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv @@ -0,0 +1,18 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +--- +cartographer_ros_msgs/StatusResponse status +cartographer_ros_msgs/TrajectoryStates trajectory_states diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/ReadMetrics.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/ReadMetrics.srv new file mode 100644 index 0000000..5070e38 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/ReadMetrics.srv @@ -0,0 +1,18 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +--- +cartographer_ros_msgs/StatusResponse status +cartographer_ros_msgs/MetricFamily[] metric_families +time timestamp diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/StartTrajectory.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/StartTrajectory.srv new file mode 100644 index 0000000..5696635 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/StartTrajectory.srv @@ -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. + +string configuration_directory +string configuration_basename +bool use_initial_pose +geometry_msgs/Pose initial_pose +int32 relative_to_trajectory_id +--- +cartographer_ros_msgs/StatusResponse status +int32 trajectory_id diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/SubmapQuery.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/SubmapQuery.srv new file mode 100644 index 0000000..3dd787c --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/SubmapQuery.srv @@ -0,0 +1,20 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +int32 trajectory_id +int32 submap_index +--- +cartographer_ros_msgs/StatusResponse status +int32 submap_version +cartographer_ros_msgs/SubmapTexture[] textures diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/TrajectoryQuery.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/TrajectoryQuery.srv new file mode 100644 index 0000000..ca5da3b --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/TrajectoryQuery.srv @@ -0,0 +1,18 @@ +# Copyright 2019 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +int32 trajectory_id +--- +cartographer_ros_msgs/StatusResponse status +geometry_msgs/PoseStamped[] trajectory diff --git a/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/WriteState.srv b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/WriteState.srv new file mode 100644 index 0000000..6b40196 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_ros_msgs/srv/WriteState.srv @@ -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. + +string filename +bool include_unfinished_submaps +--- +cartographer_ros_msgs/StatusResponse status diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/CHANGELOG.rst b/carto_ws/src/cartographer_ros-master/cartographer_rviz/CHANGELOG.rst new file mode 100644 index 0000000..0b1064c --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cartographer_rviz +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.0 (2017-11-23) +------------------ +* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0 + +0.2.0 (2017-06-19) +------------------ +* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0 + +0.1.0 (2017-05-18) +------------------ +* First unstable development release diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/CMakeLists.txt b/carto_ws/src/cartographer_ros-master/cartographer_rviz/CMakeLists.txt new file mode 100644 index 0000000..a3d5480 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/CMakeLists.txt @@ -0,0 +1,111 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT 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.12) # Ships with Ubuntu 14.04 (Trusty) + +project(cartographer_rviz) +SET(CMAKE_BUILD_TYPE Debug) + +set(PACKAGE_DEPENDENCIES + cartographer_ros + cartographer_ros_msgs + message_runtime + roscpp + roslib + rviz +) + +if(WIN32) + set(Boost_USE_STATIC_LIBS FALSE) +endif() +find_package(Boost REQUIRED COMPONENTS system iostreams) +find_package(cartographer REQUIRED) +include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") +google_initialize_cartographer_project() + +find_package(absl REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) + +catkin_package( + CATKIN_DEPENDS + message_runtime + ${PACKAGE_DEPENDENCIES} + INCLUDE_DIRS "." +) + +file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") +set(CMAKE_AUTOMOC ON) + +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Core Qt5::Widgets) + include_directories(${Qt5Widgets_INCLUDE_DIRS}) +endif() +add_definitions(-DQT_NO_KEYWORDS) +add_library(${PROJECT_NAME} ${ALL_SRCS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) + +# Add the binary directory first, so that port.h is included after it has +# been generated. +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + $ +) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${EIGEN3_INCLUDE_DIR}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${Boost_INCLUDE_DIRS}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + +set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") +set_target_properties(${PROJECT_NAME} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + +target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) + +# On windows, rviz won't find the DLL in CATKIN_PACKAGE_BIN_DESTINATION, +# but it will in CATKIN_PACKAGE_LIB_DESTINATION? +if(WIN32) + set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +else() + set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +endif() + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${RUNTIME_DESTINATION} +) + +install(FILES rviz_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY ogre_media + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/.clang-format b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/.clang-format new file mode 100644 index 0000000..5650f22 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: Google +DerivePointerAlignment: false diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/drawable_submap.cc new file mode 100644 index 0000000..d2c800f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -0,0 +1,201 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_rviz/drawable_submap.h" + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/memory/memory.h" +#include "cartographer/common/port.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "ros/ros.h" + +namespace cartographer_rviz { + +namespace { + +constexpr std::chrono::milliseconds kMinQueryDelayInMs(250); +constexpr float kAlphaUpdateThreshold = 0.2f; + +const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red); +const Eigen::Vector3d kSubmapIdPosition(0.0, 0.0, 0.3); +constexpr float kSubmapIdCharHeight = 0.2f; +constexpr int kNumberOfSlicesPerSubmap = 2; + +} // namespace + +DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, + ::rviz::DisplayContext* const display_context, + Ogre::SceneNode* const map_node, + ::rviz::Property* const submap_category, + const bool visible, const bool pose_axes_visible, + const float pose_axes_length, + const float pose_axes_radius) + : id_(id), + display_context_(display_context), + submap_node_(map_node->createChildSceneNode()), + submap_id_text_node_(submap_node_->createChildSceneNode()), + pose_axes_(display_context->getSceneManager(), submap_node_, + pose_axes_length, pose_axes_radius), + pose_axes_visible_(pose_axes_visible), + submap_id_text_(QString("(%1,%2)") + .arg(id.trajectory_id) + .arg(id.submap_index) + .toStdString()), + last_query_timestamp_(0) { + for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap; + ++slice_index) { + ogre_slices_.emplace_back(absl::make_unique( + id, slice_index, display_context->getSceneManager(), submap_node_)); + } + // DrawableSubmap creates and manages its visibility property object + // (a unique_ptr is needed because the Qt parent of the visibility + // property is the submap_category object - the BoolProperty needs + // to be destroyed along with the DrawableSubmap) + visibility_ = absl::make_unique<::rviz::BoolProperty>( + "" /* title */, visible, "" /* description */, submap_category, + SLOT(ToggleVisibility()), this); + submap_id_text_.setCharacterHeight(kSubmapIdCharHeight); + submap_id_text_.setColor(kSubmapIdColor); + submap_id_text_.setTextAlignment(::rviz::MovableText::H_CENTER, + ::rviz::MovableText::V_ABOVE); + submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition)); + submap_id_text_node_->attachObject(&submap_id_text_); + TogglePoseMarkerVisibility(); + connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); +} + +DrawableSubmap::~DrawableSubmap() { + // 'query_in_progress_' must be true until the Q_EMIT has happened. Qt then + // makes sure that 'RequestSucceeded' is not called after our destruction. + if (QueryInProgress()) { + rpc_request_future_.wait(); + } + display_context_->getSceneManager()->destroySceneNode(submap_node_); + display_context_->getSceneManager()->destroySceneNode(submap_id_text_node_); +} + +void DrawableSubmap::Update( + const ::std_msgs::Header& header, + const ::cartographer_ros_msgs::SubmapEntry& metadata) { + absl::MutexLock locker(&mutex_); + metadata_version_ = metadata.submap_version; + pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); + submap_node_->setPosition(ToOgre(pose_.translation())); + submap_node_->setOrientation(ToOgre(pose_.rotation())); + display_context_->queueRender(); + visibility_->setName( + QString("%1.%2").arg(id_.submap_index).arg(metadata_version_)); + visibility_->setDescription( + QString("Toggle visibility of this individual submap.

" + "Trajectory %1, submap %2, submap version %3") + .arg(id_.trajectory_id) + .arg(id_.submap_index) + .arg(metadata_version_)); +} + +bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { + absl::MutexLock locker(&mutex_); + // Received metadata version can also be lower if we restarted Cartographer. + const bool newer_version_available = + submap_textures_ == nullptr || + submap_textures_->version != metadata_version_; + const std::chrono::milliseconds now = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + const bool recently_queried = + last_query_timestamp_ + kMinQueryDelayInMs > now; + if (!newer_version_available || recently_queried || query_in_progress_) { + return false; + } + query_in_progress_ = true; + last_query_timestamp_ = now; + rpc_request_future_ = std::async(std::launch::async, [this, client]() { + std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = + ::cartographer_ros::FetchSubmapTextures(id_, client); + absl::MutexLock locker(&mutex_); + query_in_progress_ = false; + if (submap_textures != nullptr) { + // We emit a signal to update in the right thread, and pass via the + // 'submap_texture_' member to simplify the signal-slot connection + // slightly. + submap_textures_ = std::move(submap_textures); + Q_EMIT RequestSucceeded(); + } + }); + return true; +} + +bool DrawableSubmap::QueryInProgress() { + absl::MutexLock locker(&mutex_); + return query_in_progress_; +} + +void DrawableSubmap::SetAlpha(const double current_tracking_z, + const float fade_out_start_distance_in_meters) { + const float fade_out_distance_in_meters = + 2.f * fade_out_start_distance_in_meters; + const double distance_z = + std::abs(pose_.translation().z() - current_tracking_z); + const double fade_distance = + std::max(distance_z - fade_out_start_distance_in_meters, 0.); + const float target_alpha = static_cast( + std::max(0., 1. - fade_distance / fade_out_distance_in_meters)); + + if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold || + target_alpha == 0.f || target_alpha == 1.f) { + current_alpha_ = target_alpha; + } + for (auto& slice : ogre_slices_) { + slice->SetAlpha(current_alpha_); + } + display_context_->queueRender(); +} + +void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) { + ogre_slices_.at(slice_index)->SetVisibility(visible); + ToggleVisibility(); +} + +void DrawableSubmap::UpdateSceneNode() { + absl::MutexLock locker(&mutex_); + for (size_t slice_index = 0; slice_index < ogre_slices_.size() && + slice_index < submap_textures_->textures.size(); + ++slice_index) { + ogre_slices_[slice_index]->Update(submap_textures_->textures[slice_index]); + } + display_context_->queueRender(); +} + +void DrawableSubmap::ToggleVisibility() { + for (auto& ogre_slice : ogre_slices_) { + ogre_slice->UpdateOgreNodeVisibility(visibility_->getBool()); + } + display_context_->queueRender(); +} + +void DrawableSubmap::TogglePoseMarkerVisibility() { + submap_id_text_node_->setVisible(pose_axes_visible_); + pose_axes_.getSceneNode()->setVisible(pose_axes_visible_); +} + +} // namespace cartographer_rviz diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/drawable_submap.h b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/drawable_submap.h new file mode 100644 index 0000000..21e1933 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -0,0 +1,126 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_ +#define CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_ + +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "OgreSceneManager.h" +#include "OgreSceneNode.h" +#include "absl/synchronization/mutex.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros/submap.h" +#include "cartographer_ros_msgs/SubmapEntry.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_rviz/ogre_slice.h" +#include "ros/ros.h" +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/ogre_helpers/axes.h" +#include "rviz/ogre_helpers/movable_text.h" +#include "rviz/properties/bool_property.h" + +namespace cartographer_rviz { + +// Contains all the information needed to render a submap onto the final +// texture representing the whole map. +class DrawableSubmap : public QObject { + Q_OBJECT + + public: + DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id, + ::rviz::DisplayContext* display_context, + Ogre::SceneNode* map_node, ::rviz::Property* submap_category, + bool visible, const bool pose_axes_visible, + float pose_axes_length, float pose_axes_radius); + ~DrawableSubmap() override; + DrawableSubmap(const DrawableSubmap&) = delete; + DrawableSubmap& operator=(const DrawableSubmap&) = delete; + + // Updates the 'metadata' for this submap. If necessary, the next call to + // MaybeFetchTexture() will fetch a new submap texture. + void Update(const ::std_msgs::Header& header, + const ::cartographer_ros_msgs::SubmapEntry& metadata); + + // If an update is needed, it will send an RPC using 'client' to request the + // new data for the submap and returns true. + bool MaybeFetchTexture(ros::ServiceClient* client); + + // Returns whether an RPC is in progress. + bool QueryInProgress(); + + // Sets the alpha of the submap taking into account its slice height and the + // 'current_tracking_z'. 'fade_out_start_distance_in_meters' defines the + // distance in z direction in meters, before which the submap will be shown + // at full opacity. + void SetAlpha(double current_tracking_z, float fade_out_distance_in_meters); + + // Sets the visibility of a slice. It will be drawn if the parent submap + // is also visible. + void SetSliceVisibility(size_t slice_index, bool visible); + + ::cartographer::mapping::SubmapId id() const { return id_; } + int version() const { return metadata_version_; } + bool visibility() const { return visibility_->getBool(); } + void set_visibility(const bool visibility) { + visibility_->setBool(visibility); + } + void set_pose_markers_visibility(const bool visibility) { + pose_axes_visible_ = visibility; + TogglePoseMarkerVisibility(); + } + + Q_SIGNALS: + // RPC request succeeded. + void RequestSucceeded(); + + private Q_SLOTS: + // Callback when an rpc request succeeded. + void UpdateSceneNode(); + void ToggleVisibility(); + void TogglePoseMarkerVisibility(); + + private: + const ::cartographer::mapping::SubmapId id_; + + absl::Mutex mutex_; + ::rviz::DisplayContext* const display_context_; + Ogre::SceneNode* const submap_node_; + Ogre::SceneNode* const submap_id_text_node_; + std::vector> ogre_slices_; + ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); + ::rviz::Axes pose_axes_; + bool pose_axes_visible_; + ::rviz::MovableText submap_id_text_; + std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); + bool query_in_progress_ GUARDED_BY(mutex_) = false; + int metadata_version_ GUARDED_BY(mutex_) = -1; + std::future rpc_request_future_; + std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures_ + GUARDED_BY(mutex_); + float current_alpha_ = 0.f; + std::unique_ptr<::rviz::BoolProperty> visibility_; +}; + +} // namespace cartographer_rviz + +#endif // CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_ diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/ogre_slice.cc new file mode 100644 index 0000000..3062042 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -0,0 +1,157 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_rviz/ogre_slice.h" + +#include +#include + +#include "OgreGpuProgramParams.h" +#include "OgreImage.h" +#include "OgreMaterialManager.h" +#include "OgreTechnique.h" +#include "OgreTextureManager.h" +#include "absl/strings/str_cat.h" +#include "cartographer/common/port.h" + +namespace cartographer_rviz { + +namespace { + +constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; +constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap"; +constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; +constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; + +std::string GetSliceIdentifier( + const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { + return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-", + slice_id); +} + +} // namespace + +Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) { + return Ogre::Vector3(v.x(), v.y(), v.z()); +} + +Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { + return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()); +} + +OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, + Ogre::SceneManager* const scene_manager, + Ogre::SceneNode* const submap_node) + : id_(id), + slice_id_(slice_id), + scene_manager_(scene_manager), + submap_node_(submap_node), + slice_node_(submap_node_->createChildSceneNode()), + manual_object_(scene_manager_->createManualObject(absl::StrCat( + kManualObjectPrefix, GetSliceIdentifier(id, slice_id)))) { + material_ = Ogre::MaterialManager::getSingleton().getByName( + kSubmapSourceMaterialName); + material_ = material_->clone( + absl::StrCat(kSubmapMaterialPrefix, GetSliceIdentifier(id_, slice_id_))); + material_->setReceiveShadows(false); + material_->getTechnique(0)->setLightingEnabled(false); + material_->setCullingMode(Ogre::CULL_NONE); + material_->setDepthBias(-1.f, 0.f); + material_->setDepthWriteEnabled(false); + slice_node_->attachObject(manual_object_); +} + +OgreSlice::~OgreSlice() { + Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); + if (!texture_.isNull()) { + Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); + texture_.setNull(); + } + scene_manager_->destroySceneNode(slice_node_); + scene_manager_->destroyManualObject(manual_object_); +} + +void OgreSlice::Update( + const ::cartographer::io::SubmapTexture& submap_texture) { + slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation())); + slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation())); + // The call to Ogre's loadRawData below does not work with an RG texture, + // therefore we create an RGB one whose blue channel is always 0. + std::vector rgb; + CHECK_EQ(submap_texture.pixels.intensity.size(), + submap_texture.pixels.alpha.size()); + for (size_t i = 0; i < submap_texture.pixels.intensity.size(); ++i) { + rgb.push_back(submap_texture.pixels.intensity[i]); + rgb.push_back(submap_texture.pixels.alpha[i]); + rgb.push_back(0); + } + + manual_object_->clear(); + const float metric_width = submap_texture.resolution * submap_texture.width; + const float metric_height = submap_texture.resolution * submap_texture.height; + manual_object_->begin(material_->getName(), + Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // Bottom left + manual_object_->position(-metric_height, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 1.0f); + // Bottom right + manual_object_->position(-metric_height, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + // Top left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + // Top right + manual_object_->position(0.0f, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 0.0f); + manual_object_->end(); + + Ogre::DataStreamPtr pixel_stream; + pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); + + if (!texture_.isNull()) { + Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); + texture_.setNull(); + } + const std::string texture_name = + absl::StrCat(kSubmapTexturePrefix, GetSliceIdentifier(id_, slice_id_)); + texture_ = Ogre::TextureManager::getSingleton().loadRawData( + texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, submap_texture.width, submap_texture.height, + Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0); + + Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0); + pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA); + Ogre::TextureUnitState* const texture_unit = + pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0) + : pass->createTextureUnitState(); + + texture_unit->setTextureName(texture_->getName()); + texture_unit->setTextureFiltering(Ogre::TFO_NONE); +} + +void OgreSlice::SetAlpha(const float alpha) { + const Ogre::GpuProgramParametersSharedPtr parameters = + material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); + parameters->setNamedConstant("u_alpha", alpha); +} + +void OgreSlice::SetVisibility(bool visibility) { visibility_ = visibility; } + +void OgreSlice::UpdateOgreNodeVisibility(bool submap_visibility) { + slice_node_->setVisible(submap_visibility && visibility_); +} + +} // namespace cartographer_rviz diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/ogre_slice.h b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/ogre_slice.h new file mode 100644 index 0000000..95e8238 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/ogre_slice.h @@ -0,0 +1,80 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_RVIZ_SRC_OGRE_SLICE_H_ +#define CARTOGRAPHER_RVIZ_SRC_OGRE_SLICE_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "OgreManualObject.h" +#include "OgreMaterial.h" +#include "OgreQuaternion.h" +#include "OgreSceneManager.h" +#include "OgreSceneNode.h" +#include "OgreTexture.h" +#include "OgreVector3.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/id.h" + +namespace cartographer_rviz { + +Ogre::Vector3 ToOgre(const Eigen::Vector3d& v); +Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q); + +// A class containing the Ogre code to visualize a slice texture of a submap. +// Member functions are expected to be called from the Ogre thread. +class OgreSlice { + public: + // Attaches a node visualizing the submap 'id' to the 'submap_node' which is + // expected to represent the submap frame. + OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, + Ogre::SceneManager* const scene_manager, + Ogre::SceneNode* const submap_node); + ~OgreSlice(); + + OgreSlice(const OgreSlice&) = delete; + OgreSlice& operator=(const OgreSlice&) = delete; + + // Updates the texture and pose of the submap using new data from + // 'submap_texture'. + void Update(const ::cartographer::io::SubmapTexture& submap_texture); + + // Changes the opacity of the submap to 'alpha'. + void SetAlpha(float alpha); + + // Sets the local visibility of this slice. + void SetVisibility(bool visibility); + + // Updates the SceneNode to be visible if the submap and this slice are + // visible. + void UpdateOgreNodeVisibility(bool submap_visibility); + + private: + // TODO(gaschler): Pack both ids into a struct. + const ::cartographer::mapping::SubmapId id_; + const int slice_id_; + Ogre::SceneManager* const scene_manager_; + Ogre::SceneNode* const submap_node_; + Ogre::SceneNode* const slice_node_; + Ogre::ManualObject* const manual_object_; + Ogre::TexturePtr texture_; + Ogre::MaterialPtr material_; + bool visibility_ = true; +}; + +} // namespace cartographer_rviz + +#endif // CARTOGRAPHER_RVIZ_SRC_OGRE_SLICE_H_ diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/submaps_display.cc b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/submaps_display.cc new file mode 100644 index 0000000..76f5d2d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -0,0 +1,312 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_rviz/submaps_display.h" + +#include "OgreResourceGroupManager.h" +#include "absl/memory/memory.h" +#include "absl/synchronization/mutex.h" +#include "cartographer/mapping/id.h" +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "geometry_msgs/TransformStamped.h" +#include "pluginlib/class_list_macros.h" +#include "ros/package.h" +#include "ros/ros.h" +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/properties/bool_property.h" +#include "rviz/properties/string_property.h" + +namespace cartographer_rviz { + +namespace { + +constexpr int kMaxOnGoingRequestsPerTrajectory = 6; +constexpr char kMaterialsDirectory[] = "/ogre_media/materials"; +constexpr char kGlsl120Directory[] = "/glsl120"; +constexpr char kScriptsDirectory[] = "/scripts"; +constexpr char kDefaultTrackingFrame[] = "base_link"; +constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query"; + +} // namespace + +SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { + submap_query_service_property_ = new ::rviz::StringProperty( + "Submap query service", kDefaultSubmapQueryServiceName, + "Submap query service to connect to.", this, SLOT(Reset())); + tracking_frame_property_ = new ::rviz::StringProperty( + "Tracking frame", kDefaultTrackingFrame, + "Tracking frame, used for fading out submaps.", this); + slice_high_resolution_enabled_ = new ::rviz::BoolProperty( + "High Resolution", true, "Display high resolution slices.", this, + SLOT(ResolutionToggled()), this); + slice_low_resolution_enabled_ = new ::rviz::BoolProperty( + "Low Resolution", false, "Display low resolution slices.", this, + SLOT(ResolutionToggled()), this); + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(""); + trajectories_category_ = new ::rviz::Property( + "Submaps", QVariant(), "List of all submaps, organized by trajectories.", + this); + visibility_all_enabled_ = new ::rviz::BoolProperty( + "All", true, + "Whether submaps from all trajectories should be displayed or not.", + trajectories_category_, SLOT(AllEnabledToggled()), this); + pose_markers_all_enabled_ = new ::rviz::BoolProperty( + "All Submap Pose Markers", true, + "Whether submap pose markers should be displayed or not.", + trajectories_category_, SLOT(PoseMarkersEnabledToggled()), this); + fade_out_start_distance_in_meters_ = + new ::rviz::FloatProperty("Fade-out distance", 1.f, + "Distance in meters in z-direction beyond " + "which submaps will start to fade out.", + this); + const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation( + package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation( + package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem", + ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation( + package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem", + ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); +} + +SubmapsDisplay::~SubmapsDisplay() { + client_.shutdown(); + trajectories_.clear(); + scene_manager_->destroySceneNode(map_node_); +} + +void SubmapsDisplay::Reset() { reset(); } + +void SubmapsDisplay::CreateClient() { + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + submap_query_service_property_->getStdString()); +} + +void SubmapsDisplay::onInitialize() { + MFDClass::onInitialize(); + map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + CreateClient(); +} + +void SubmapsDisplay::reset() { + MFDClass::reset(); + absl::MutexLock locker(&mutex_); + client_.shutdown(); + trajectories_.clear(); + CreateClient(); +} + +void SubmapsDisplay::processMessage( + const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { + absl::MutexLock locker(&mutex_); + map_frame_ = absl::make_unique(msg->header.frame_id); + // In case Cartographer node is relaunched, destroy trajectories from the + // previous instance. + for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + const size_t trajectory_id = submap_entry.trajectory_id; + if (trajectories_.count(trajectory_id) == 0) { + continue; + } + const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps; + const auto it = trajectory_submaps.find(submap_entry.submap_index); + if (it != trajectory_submaps.end() && + it->second->version() > submap_entry.submap_version) { + // Versions should only increase unless Cartographer restarted. + trajectories_.clear(); + break; + } + } + using ::cartographer::mapping::SubmapId; + std::set listed_submaps; + std::set listed_trajectories; + for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index}; + listed_submaps.insert(id); + listed_trajectories.insert(submap_entry.trajectory_id); + if (trajectories_.count(id.trajectory_id) == 0) { + trajectories_.insert(std::make_pair( + id.trajectory_id, + absl::make_unique( + absl::make_unique<::rviz::BoolProperty>( + QString("Trajectory %1").arg(id.trajectory_id), + visibility_all_enabled_->getBool(), + QString( + "List of all submaps in Trajectory %1. The checkbox " + "controls whether all submaps in this trajectory should " + "be displayed or not.") + .arg(id.trajectory_id), + trajectories_category_), + pose_markers_all_enabled_->getBool()))); + } + auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility; + auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps; + auto& pose_markers_visibility = + trajectories_[id.trajectory_id]->pose_markers_visibility; + if (trajectory_submaps.count(id.submap_index) == 0) { + // TODO(ojura): Add RViz properties for adjusting submap pose axes + constexpr float kSubmapPoseAxesLength = 0.3f; + constexpr float kSubmapPoseAxesRadius = 0.06f; + trajectory_submaps.emplace( + id.submap_index, + absl::make_unique( + id, context_, map_node_, trajectory_visibility.get(), + trajectory_visibility->getBool(), + pose_markers_visibility->getBool(), kSubmapPoseAxesLength, + kSubmapPoseAxesRadius)); + trajectory_submaps.at(id.submap_index) + ->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool()); + trajectory_submaps.at(id.submap_index) + ->SetSliceVisibility(1, slice_low_resolution_enabled_->getBool()); + } + trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry); + } + // Remove all deleted trajectories not mentioned in the SubmapList. + for (auto it = trajectories_.begin(); it != trajectories_.end();) { + if (listed_trajectories.count(it->first) == 0) { + it = trajectories_.erase(it); + } else { + ++it; + } + } + // Remove all submaps not mentioned in the SubmapList. + for (const auto& trajectory_by_id : trajectories_) { + const int trajectory_id = trajectory_by_id.first; + auto& trajectory_submaps = trajectory_by_id.second->submaps; + for (auto it = trajectory_submaps.begin(); + it != trajectory_submaps.end();) { + if (listed_submaps.count( + SubmapId{static_cast(trajectory_id), it->first}) == 0) { + it = trajectory_submaps.erase(it); + } else { + ++it; + } + } + } +} + +void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { + absl::MutexLock locker(&mutex_); + // Schedule fetching of new submap textures. + for (const auto& trajectory_by_id : trajectories_) { + int num_ongoing_requests = 0; + for (const auto& submap_entry : trajectory_by_id.second->submaps) { + if (submap_entry.second->QueryInProgress()) { + ++num_ongoing_requests; + } + } + for (auto it = trajectory_by_id.second->submaps.rbegin(); + it != trajectory_by_id.second->submaps.rend() && + num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; + ++it) { + if (it->second->MaybeFetchTexture(&client_)) { + ++num_ongoing_requests; + } + } + } + if (map_frame_ == nullptr) { + return; + } + // Update the fading by z distance. + const ros::Time kLatest(0); + try { + const ::geometry_msgs::TransformStamped transform_stamped = + tf_buffer_.lookupTransform( + *map_frame_, tracking_frame_property_->getStdString(), kLatest); + for (auto& trajectory_by_id : trajectories_) { + for (auto& submap_entry : trajectory_by_id.second->submaps) { + submap_entry.second->SetAlpha( + transform_stamped.transform.translation.z, + fade_out_start_distance_in_meters_->getFloat()); + } + } + } catch (const tf2::TransformException& ex) { + ROS_WARN_THROTTLE(1., "Could not compute submap fading: %s", ex.what()); + } + // Update the map frame to fixed frame transform. + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position, + orientation)) { + map_node_->setPosition(position); + map_node_->setOrientation(orientation); + context_->queueRender(); + } +} + +void SubmapsDisplay::AllEnabledToggled() { + absl::MutexLock locker(&mutex_); + const bool visible = visibility_all_enabled_->getBool(); + for (auto& trajectory_by_id : trajectories_) { + trajectory_by_id.second->visibility->setBool(visible); + } +} + +void SubmapsDisplay::PoseMarkersEnabledToggled() { + absl::MutexLock locker(&mutex_); + const bool visible = pose_markers_all_enabled_->getBool(); + for (auto& trajectory_by_id : trajectories_) { + trajectory_by_id.second->pose_markers_visibility->setBool(visible); + } +} + +void SubmapsDisplay::ResolutionToggled() { + absl::MutexLock locker(&mutex_); + for (auto& trajectory_by_id : trajectories_) { + for (auto& submap_entry : trajectory_by_id.second->submaps) { + submap_entry.second->SetSliceVisibility( + 0, slice_high_resolution_enabled_->getBool()); + submap_entry.second->SetSliceVisibility( + 1, slice_low_resolution_enabled_->getBool()); + } + } +} + +void Trajectory::AllEnabledToggled() { + const bool visible = visibility->getBool(); + for (auto& submap_entry : submaps) { + submap_entry.second->set_visibility(visible); + } +} + +void Trajectory::PoseMarkersEnabledToggled() { + const bool visible = pose_markers_visibility->getBool(); + for (auto& submap_entry : submaps) { + submap_entry.second->set_pose_markers_visibility(visible); + } +} + +Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property, + const bool pose_markers_enabled) + : visibility(std::move(property)) { + ::QObject::connect(visibility.get(), SIGNAL(changed()), this, + SLOT(AllEnabledToggled())); + // Add toggle for submap pose markers as the first entry of the visibility + // property list of this trajectory. + pose_markers_visibility = absl::make_unique<::rviz::BoolProperty>( + QString("Submap Pose Markers"), pose_markers_enabled, + QString("Toggles the submap pose markers of this trajectory."), + visibility.get()); + ::QObject::connect(pose_markers_visibility.get(), SIGNAL(changed()), this, + SLOT(PoseMarkersEnabledToggled())); +} + +} // namespace cartographer_rviz + +PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display) diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/submaps_display.h b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/submaps_display.h new file mode 100644 index 0000000..7244b50 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -0,0 +1,108 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT 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_RVIZ_SRC_SUBMAPS_DISPLAY_H_ +#define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_ + +#include +#include +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/common/port.h" +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_rviz/drawable_submap.h" +#include "rviz/message_filter_display.h" +#include "rviz/properties/bool_property.h" +#include "rviz/properties/float_property.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace cartographer_rviz { + +// TODO(gaschler): This should be a private class in SubmapsDisplay, +// unfortunately, QT does not allow for this. Move the logic out of the struct +// and use just one slot for all changes. +struct Trajectory : public QObject { + Q_OBJECT + + public: + Trajectory(std::unique_ptr<::rviz::BoolProperty> property, + bool pose_markers_enabled); + + std::unique_ptr<::rviz::BoolProperty> visibility; + std::unique_ptr<::rviz::BoolProperty> pose_markers_visibility; + std::map> submaps; + + private Q_SLOTS: + void AllEnabledToggled(); + void PoseMarkersEnabledToggled(); +}; + +// RViz plugin used for displaying maps which are represented by a collection of +// submaps. +// +// We show an X-ray view of the map which is achieved by shipping textures for +// every submap containing pre-multiplied alpha and grayscale values, these are +// then alpha blended together. +class SubmapsDisplay + : public ::rviz::MessageFilterDisplay<::cartographer_ros_msgs::SubmapList> { + Q_OBJECT + + public: + SubmapsDisplay(); + ~SubmapsDisplay() override; + + SubmapsDisplay(const SubmapsDisplay&) = delete; + SubmapsDisplay& operator=(const SubmapsDisplay&) = delete; + + private Q_SLOTS: + void Reset(); + void AllEnabledToggled(); + void PoseMarkersEnabledToggled(); + void ResolutionToggled(); + + private: + void CreateClient(); + + // These are called by RViz and therefore do not adhere to the style guide. + void onInitialize() override; + void reset() override; + void processMessage( + const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override; + void update(float wall_dt, float ros_dt) override; + + ::tf2_ros::Buffer tf_buffer_; + ::tf2_ros::TransformListener tf_listener_; + ros::ServiceClient client_; + ::rviz::StringProperty* submap_query_service_property_; + std::unique_ptr map_frame_; + ::rviz::StringProperty* tracking_frame_property_; + Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame. + std::map> trajectories_ GUARDED_BY(mutex_); + absl::Mutex mutex_; + ::rviz::BoolProperty* slice_high_resolution_enabled_; + ::rviz::BoolProperty* slice_low_resolution_enabled_; + ::rviz::Property* trajectories_category_; + ::rviz::BoolProperty* visibility_all_enabled_; + ::rviz::BoolProperty* pose_markers_all_enabled_; + ::rviz::FloatProperty* fade_out_start_distance_in_meters_; +}; + +} // namespace cartographer_rviz + +#endif // CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_ diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/glsl120.program b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/glsl120.program new file mode 100644 index 0000000..7272c21 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/glsl120.program @@ -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. + */ + +fragment_program cartographer_ros/glsl120/submap.frag glsl +{ + source submap.frag +} + +vertex_program cartographer_ros/glsl120/submap.vert glsl +{ + source submap.vert +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/submap.frag b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/submap.frag new file mode 100644 index 0000000..eb49e2f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/submap.frag @@ -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. + */ + +#version 120 + +varying vec2 out_submap_texture_coordinate; + +uniform sampler2D u_submap; +uniform float u_alpha; + +void main() +{ + vec2 texture_value = texture2D(u_submap, out_submap_texture_coordinate).rg; + float value = u_alpha * texture_value.r; + float alpha = u_alpha * texture_value.g; + gl_FragColor = vec4(value, value, value, alpha); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/submap.vert b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/submap.vert new file mode 100644 index 0000000..864bfa1 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/glsl120/submap.vert @@ -0,0 +1,27 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#version 120 + +attribute vec4 uv0; + +varying vec2 out_submap_texture_coordinate; + +void main() +{ + out_submap_texture_coordinate = vec2(uv0); + gl_Position = ftransform(); +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/scripts/submap.material b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/scripts/submap.material new file mode 100644 index 0000000..1ee71e3 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/ogre_media/materials/scripts/submap.material @@ -0,0 +1,32 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +material cartographer_ros/Submap +{ + technique + { + pass + { + vertex_program_ref cartographer_ros/glsl120/submap.vert {} + + fragment_program_ref cartographer_ros/glsl120/submap.frag + { + param_named u_submap int 0 + param_named u_alpha float 1.0 + } + } + } +} diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/package.xml b/carto_ws/src/cartographer_ros-master/cartographer_rviz/package.xml new file mode 100644 index 0000000..b430389 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/package.xml @@ -0,0 +1,56 @@ + + + + + cartographer_rviz + 1.0.0 + + Cartographer is a system that provides real-time simultaneous localization + and mapping (SLAM) in 2D and 3D across multiple platforms and sensor + configurations. This package provides Cartographer's RViz integration. + + + The Cartographer Authors + + Apache 2.0 + + https://github.com/cartographer-project/cartographer_ros + + + The Cartographer Authors + + + catkin + + git + + cartographer + cartographer_ros + cartographer_ros_msgs + libqt5-core + libqt5-gui + libqt5-widgets + message_runtime + qtbase5-dev + roscpp + roslib + rviz + + + + + diff --git a/carto_ws/src/cartographer_ros-master/cartographer_rviz/rviz_plugin_description.xml b/carto_ws/src/cartographer_ros-master/cartographer_rviz/rviz_plugin_description.xml new file mode 100644 index 0000000..58db708 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/cartographer_rviz/rviz_plugin_description.xml @@ -0,0 +1,26 @@ + + + + + + Displays submaps as a unified map in RViz. + https://github.com/cartographer-project/cartographer_ros + + + diff --git a/carto_ws/src/cartographer_ros-master/docs/CMakeLists.txt b/carto_ws/src/cartographer_ros-master/docs/CMakeLists.txt new file mode 100644 index 0000000..ec58eef --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/CMakeLists.txt @@ -0,0 +1,28 @@ +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required (VERSION 2.8.7) + +project(cartographer_ros_docs) + +find_package(Sphinx) +if(SPHINX_FOUND) + set(OUTPUT_DIR "${PROJECT_BINARY_DIR}/docs/html") + add_custom_target(build_doc_ros ALL + ${SPHINX_EXECUTABLE} -b html + ${CMAKE_CURRENT_SOURCE_DIR}/source + ${CMAKE_CURRENT_BINARY_DIR}/html + COMMENT "Building documentation." + ) +endif() diff --git a/carto_ws/src/cartographer_ros-master/docs/source/algo_walkthrough.rst b/carto_ws/src/cartographer_ros-master/docs/source/algo_walkthrough.rst new file mode 100644 index 0000000..42b056a --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/algo_walkthrough.rst @@ -0,0 +1,366 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +Algorithm walkthrough for tuning +================================ + +Cartographer is a complex system and tuning it requires a good understanding of its inner working. +This page tries to give an intuitive overview of the different subsystems used by Cartographer along with their configuration values. +If you are interested in more than an introduction to Cartographer, you should refer to the Cartographer paper. +It only describes the 2D SLAM but it defines rigourously most of the concepts described here. +Those concepts generally apply to 3D as well. + +W. Hess, D. Kohler, H. Rapp, and D. Andor, +`Real-Time Loop Closure in 2D LIDAR SLAM`_, in +*Robotics and Automation (ICRA), 2016 IEEE International Conference on*. +IEEE, 2016. pp. 1271–1278. + +.. _Real-Time Loop Closure in 2D LIDAR SLAM: https://research.google.com/pubs/pub45466.html + +Overview +-------- + +.. image:: https://raw.githubusercontent.com/cartographer-project/cartographer/master/docs/source/high_level_system_overview.png + :target: https://github.com/cartographer-project/cartographer/blob/master/docs/source/high_level_system_overview.png + +Cartographer can be seen as two separate, but related subsystems. +The first one is **local SLAM** (sometimes also called **frontend** or local trajectory builder). +Its job is to build a succession of **submaps**. +Each submap is meant to be locally consistent but we accept that local SLAM drifts over time. +Most of the local SLAM options can be found in `install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua`_ for 2D and `install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua`_ for 3D. (for the rest of this page we will refer to `TRAJECTORY_BUILDER_nD` for the common options) + +.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_2d.lua +.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_3d.lua + +The other subsystem is **global SLAM** (sometimes called the **backend**). +It runs in background threads and its main job is to find **loop closure constraints**. +It does that by scan-matching **scans** (gathered in **nodes**) against submaps. +It also incorporates other sensor data to get a higher level view and identify the most consistent global solution. +In 3D, it also tries to find the direction of gravity. +Most of its options can be found in `install_isolated/share/cartographer/configuration_files/pose_graph.lua`_ + +.. _install_isolated/share/cartographer/configuration_files/pose_graph.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/pose_graph.lua + +On a higher abstraction, the job of local SLAM is to generate good submaps and the job of global SLAM is to tie them most consistently together. + +Input +----- + +Range finding sensors (for example: LIDARs) provide depth information in multiple directions. +However, some of the measurements are irrelevant for SLAM. +If the sensor is partially covered with dust or if it is directed towards a part of the robot, some of the measured distance can be considered as noise for SLAM. +On the other hand, some of the furthest measurements can also come from undesired sources (reflection, sensor noise) and are irrelevant for SLAM as well. +To tackle those issue, Cartographer starts by applying a bandpass filter and only keeps range values between a certain min and max range. +Those min and max values should be chosen according to the specifications of your robot and sensors. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.min_range + TRAJECTORY_BUILDER_nD.max_range + +.. note:: + + In 2D, Cartographer replaces ranges further than max_range by ``TRAJECTORY_BUILDER_2D.missing_data_ray_length``. It also provides a ``max_z`` and ``min_z`` values to filter 3D point clouds into a 2D cut. + +.. note:: + + In Cartographer configuration files, every distance is defined in meters + +Distances are measured over a certain period of time, while the robot is actually moving. +However, distances are delivered by sensors "in batch" in large ROS messages. +Each of the messages' timestamp can be considered independently by Cartographer to take into account deformations caused by the robot's motion. +The more often Cartographer gets measurements, the better it becomes at unwarping the measurements to assemble a single coherent scan that could have been captured instantly. +It is therefore strongly encouraged to provide as many range data (ROS messages) by scan (a set of range data that can be matched against another scan) as possible. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.num_accumulated_range_data + +Range data is typically measured from a single point on the robot but in multiple angles. +This means that close surfaces (for instance the road) are very often hit and provide lots of points. +On the opposite, far objects are less often hit and offer less points. +In order to reduce the computational weight of points handling, we usually need to subsample point clouds. +However, a simple random sampling would remove points from areas where we already have a low density of measurements and the high-density areas would still have more points than needed. +To address that density problem, we can use a voxel filter that downsamples raw points into cubes of a constant size and only keeps the centroid of each cube. + +A small cube size will result in a more dense data representation, causing more computations. +A large cube size will result in a data loss but will be much quicker. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.voxel_filter_size + +After having applied a fixed-size voxel filter, Cartographer also applies an **adaptive voxel filter**. +This filter tries to determine the optimal voxel size (under a max length) to achieve a target number of points. +In 3D, two adaptive voxel filters are used to generate a high resolution and a low resolution point clouds, their usage will be clarified in :ref:`local-slam`. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length + TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points + +An Inertial Measurement Unit can be an useful source of information for SLAM because it provides an accurate direction of gravity (hence, of the ground) and a noisy but good overall indication of the robot's rotation. +In order to filter the IMU noise, gravity is observed over a certain amount of time. +If you use 2D SLAM, range data can be handled in real-time without an additional source of information so you can choose whether you'd like Cartographer to use an IMU or not. +With 3D SLAM, you need to provide an IMU because it is used as an initial guess for the orientation of the scans, greatly reducing the complexity of scan matching. + + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.use_imu_data + TRAJECTORY_BUILDER_nD.imu_gravity_time_constant + +.. note:: + + In Cartographer configuration files, every time value is defined in seconds + +.. _local-slam: + +Local SLAM +---------- + +Once a scan has been assembled and filtered from multiple range data, it is ready for the local SLAM algorithm. +Local SLAM inserts a new scan into its current submap construction by **scan matching** using an initial guess from the **pose extrapolator**. +The idea behind the pose extrapolator is to use sensor data of other sensors besides the range finder to predict where the next scan should be inserted into the submap. + +Two scan matching strategies are available: + +- The ``CeresScanMatcher`` takes the initial guess as prior and finds the best spot where the scan match fits the submap. + It does this by interpolating the submap and sub-pixel aligning the scan. + This is fast, but cannot fix errors that are significantly larger than the resolution of the submaps. + If your sensor setup and timing is reasonable, using only the ``CeresScanMatcher`` is usually the best choice to make. +- The ``RealTimeCorrelativeScanMatcher`` can be enabled if you do not have other sensors or you do not trust them. + It uses an approach similar to how scans are matched against submaps in loop closure (described later), but instead it matches against the current submap. + The best match is then used as prior for the ``CeresScanMatcher``. + This scan matcher is very expensive and will essentially override any signal from other sensors but the range finder, but it is robust in feature rich environments. + +Either way, the ``CeresScanMatcher`` can be configured to give a certain weight to each of its input. +The weight is a measure of trust into your data, this can be seen as a static covariance. +The unit of weight parameters are dimensionless quantities and can't be compared between each others. +The bigger the weight of a source of data is, the more emphasis Cartographer will put on this source of data when doing scan matching. +Sources of data include occupied space (points from the scan), translation and rotation from the pose extrapolator (or ``RealTimeCorrelativeScanMatcher``) + +.. code-block:: lua + + TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight + TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0 + TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1 + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.translation_weight + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.rotation_weight + +.. note:: + + In 3D, the ``occupied_space_weight_0`` and ``occupied_space_weight_1`` parameters are related, respectively, to the high resolution and low resolution filtered point clouds. + +The ``CeresScanMatcher`` gets its name from `Ceres Solver`_, a library developed at Google to solve non-linear least squares problems. +The scan matching problem is modelled as the minimization of such a problem with the **motion** (a transformation matrix) between two scans being a parameter to determine. +Ceres optimizes the motion using a descent algorithm for a given number of iterations. +Ceres can be configured to adapt the convergence speed to your own needs. + +.. _Ceres Solver: http://ceres-solver.org/ + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.max_num_iterations + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.num_threads + +The ``RealTimeCorrelativeScanMatcher`` can be toggled depending on the trust you have in your sensors. +It works by searching for similar scans in a **search window** which is defined by a maximum distance radius and a maximum angle radius. +When performing scan matching with scans found in this window, a different weight can be chosen for the translational and rotational components. +You can play with those weight if, for example, you know that your robot doesn't rotate a lot. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.use_online_correlative_scan_matching + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.linear_search_window + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.angular_search_window + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.translation_delta_cost_weight + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.rotation_delta_cost_weight + +To avoid inserting too many scans per submaps, once a motion between two scans is found by the scan matcher, it goes through a **motion filter**. +A scan is dropped if the motion that led to it is not considered as significant enough. +A scan is inserted into the current submap only if its motion is above a certain distance, angle or time threshold. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds + TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters + TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians + +A submap is considered as complete when the local SLAM has received a given amount of range data. +Local SLAM drifts over time, global SLAM is used to fix this drift. +Submaps must be small enough so that the drift inside them is below the resolution, so that they are locally correct. +On the other hand, they should be large enough to be distinct for loop closure to work properly. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.submaps.num_range_data + +Submaps can store their range data in a couple of different data structures: +The most widely used representation is called probability grids. +However, in 2D, one can also choose to use Truncated Signed Distance Fields (TSDF). + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type + +Probability grids cut out space into a 2D or 3D table where each cell has a fixed size and contains the odds of being obstructed. +Odds are updated according to "*hits*" (where the range data is measured) and "*misses*" (the free space between the sensor and the measured points). +Both *hits* and *misses* can have a different weight in occupancy probability calculations giving more or less trust to occupied or free space measurements. + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability + TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability + TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit_probability + TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.miss_probability + +In 2D, only one probability grid per submap is stored. +In 3D, for scan matching performance reasons, two *hybrid* probability grids are used. +(the term "hybrid" only refers to an internal tree-like data representation and is abstracted to the user) + +- a low resolution hybrid grid for far measurements +- a high resolution hybrid grid for close measurements + +Scan matching starts by aligning far points of the low resolution point cloud with the low resolution hybrid grid and then refines the pose by aligning the close high resolution points with the high resolution hybrid grid. + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution + TRAJECTORY_BUILDER_3D.submaps.high_resolution + TRAJECTORY_BUILDER_3D.submaps.low_resolution + TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range + TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range + +.. note:: + + Cartographer ROS provides an RViz plugin to visualize submaps. You can select the submaps you want to see from their number. In 3D, RViz only shows 2D projections of the 3D hybrid probability grids (in grayscale). Options are made available in RViz's left pane to switch between the low and high resolution hybrid grids visualization. + +**TODO**: *Documenting TSDF configuration* + +Global SLAM +----------- + +While the local SLAM generates its succession of submaps, a global optimization (usually refered to as "*the optimization problem*" or "*sparse pose adjustment*") task runs in background. +Its role is to re-arrange submaps between each other so that they form a coherent global map. +For instance, this optimization is in charge of altering the currently built trajectory to properly align submaps with regards to loop closures. + +The optimization is run in batches once a certain number of trajectory nodes was inserted. Depending on how frequently you need to run it, you can tune the size of these batches. + +.. code-block:: lua + + POSE_GRAPH.optimize_every_n_nodes + +.. note:: + + Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way to disable global SLAM and concentrate on the behavior of local SLAM. This is usually one of the first thing to do to tune Cartographer. + +The global SLAM is a kind of "*GraphSLAM*", it is essentially a pose graph optimization which works by building **constraints** between **nodes** and submaps and then optimizing the resulting constraints graph. +Constraints can intuitively be thought of as little ropes tying all nodes together. +The sparse pose adjustement fastens those ropes altogether. +The resulting net is called the "*pose graph*". + +.. note:: + + Constraints can be visualized in RViz, it is very handy to tune global SLAM. One can also toggle ``POSE_GRAPH.constraint_builder.log_matches`` to get regular reports of the constraints builder formatted as histograms. + +- Non-global constraints (also known as intra submaps constraints) are built automatically between nodes that are closely following each other on a trajectory. + Intuitively, those "*non-global ropes*" keep the local structure of the trajectory coherent. +- Global constraints (also referred to as loop closure constraints or inter submaps contraints) are regularly searched between a new submap and previous nodes that are considered "*close enough*" in space (part of a certain **search window**) and a strong fit (a good match when running scan matching). + Intuitively, those "*global ropes*" introduce knots in the structure and firmly bring two strands closer. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.max_constraint_distance + POSE_GRAPH.fast_correlative_scan_matcher.linear_search_window + POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_xy_search_window + POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_z_search_window + POSE_GRAPH.fast_correlative_scan_matcher*.angular_search_window + +.. note:: + + In practice, global constraints can do more than finding loop closures on a single trajectory. They can also align different trajectories recorded by multiple robots but we will keep this usage and the parameters related to "global localization" out of the scope of this document. + +To limit the amount of constraints (and computations), Cartographer only considers a subsampled set of all close nodes for constraints building. +This is controlled by a sampling ratio constant. +Sampling too few nodes could result in missed constraints and ineffective loop closures. +Sampling too many nodes would slow the global SLAM down and prevent real-time loop closures. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.sampling_ratio + +When a node and a submap are considered for constraint building, they go through a first scan matcher called the ``FastCorrelativeScanMatcher``. +This scan matcher has been specifically designed for Cartographer and makes real-time loop closures scan matching possible. +The ``FastCorrelativeScanMatcher`` relies on a "*Branch and bound*" mechanism to work at different grid resolutions and efficiently eliminate incorrect matchings. +This mechanism is extensively presented in the Cartographer paper presented earlier in this document. +It works on an exploration tree whose depth can be controlled. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth + +Once the ``FastCorrelativeScanMatcher`` has a good enough proposal (above a minimum score of matching), it is then fed into a Ceres Scan Matcher to refine the pose. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.min_score + POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d + POSE_GRAPH.constraint_builder.ceres_scan_matcher + +When Cartographer runs *the optimization problem*, Ceres is used to rearrange submaps according to multiple *residuals*. +Residuals are calculated using weighted cost functions. +The global optimization has cost functions to take into account plenty of data sources: the global (loop closure) constraints, the non-global (matcher) constraints, the IMU acceleration and rotation measurements, the local SLAM rough pose estimations, an odometry source or a fixed frame (such as a GPS system). +The weights and Ceres options can be configured as described in the :ref:`local-slam` section. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.loop_closure_translation_weight + POSE_GRAPH.constraint_builder.loop_closure_rotation_weight + POSE_GRAPH.matcher_translation_weight + POSE_GRAPH.matcher_rotation_weight + POSE_GRAPH.optimization_problem.*_weight + POSE_GRAPH.optimization_problem.ceres_solver_options + +.. note:: + + One can find useful information about the residuals used in the optimization problem by toggling ``POSE_GRAPH.max_num_final_iterations`` + +As part of its IMU residual, the optimization problem gives some flexibility to the IMU pose and, by default, Ceres is free to optimize the extrinsic calibration between your IMU and tracking frame. +If you don't trust your IMU pose, the results of Ceres' global optimization can be logged and used to improve your extrinsic calibration. +If Ceres doesn't optimize your IMU pose correctly and you trust your extrinsic calibration enough, you can make this pose constant. + +.. code-block:: lua + + POSE_GRAPH.optimization_problem.log_solver_summary + POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d + +In residuals, the influence of outliers is handled by a **Huber loss** function configured with a certain a Huber scale. +The bigger the Huber scale, `the higher is the impact`_ of (potential) outliers. + +.. _the higher is the impact: https://github.com/ceres-solver/ceres-solver/blob/0d3a84fce553c9f7aab331f0895fa7b1856ef5ee/include/ceres/loss_function.h#L172 + +.. code-block:: lua + + POSE_GRAPH.optimization_problem.huber_scale + +Once the trajectory is finished, Cartographer runs a new global optimization with, typically, a lot more iterations than previous global optimizations. +This is done to polish the final result of Cartographer and usually does not need to be real-time so a large number of iterations is often a right choice. + +.. code-block:: lua + + POSE_GRAPH.max_num_final_iterations diff --git a/carto_ws/src/cartographer_ros-master/docs/source/assets_writer.rst b/carto_ws/src/cartographer_ros-master/docs/source/assets_writer.rst new file mode 100644 index 0000000..c0315fe --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/assets_writer.rst @@ -0,0 +1,133 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +.. cartographer SHA: 30f7de1a325d6604c780f2f74d9a345ec369d12d +.. cartographer_ros SHA: 44459e18102305745c56f92549b87d8e91f434fe + +.. _assets_writer: + +Exploiting the map generated by Cartographer ROS +================================================ + +As sensor data come in, the state of a SLAM algorithm such as Cartographer evolves to stay *the current best estimate* of a robot's trajectory and surroundings. +The most accurate localization and mapping Cartographer can offer is therefore the one obtained when the algorithm finishes. +Cartographer can serialize its internal state in a ``.pbstream`` file format which is essentially a compressed protobuf file containing a snapshot of the data structures used by Cartographer internally. + +To run efficiently in real-time, Cartographer throws most of its sensor data away immediately and only works with a small subset of its input, the mapping used internally (and saved in ``.pbstream`` files) is then very rough. +However, when the algorithm finishes and a best trajectory is established, it can be recombined *a posteriori* with the full sensors data to create a high resolution map. + +Cartographer makes this kind of recombination possible using ``cartographer_assets_writer``. +The assets writer takes as input + +1. the original sensors data that has been used to perform SLAM (in a ROS ``.bag`` file), +2. a cartographer state captured while performing SLAM on this sensor data (saved in a ``.pbstream`` file), +3. the sensor extrinsics (i.e. TF data from the bag or an URDF description), +4. and a pipeline configuration, which is defined in a ``.lua`` file. + +The assets writer runs through the ``.bag`` data in batches with the trajectory found in the ``.pbstream``. +The pipeline can be used to color, filter and export SLAM point cloud data into a variety of formats. +There are multiple of such points processing steps that can be interleaved in a pipeline - several ones are already available from `cartographer/io`_. + +Sample Usage +------------ + +When running Cartographer with an offline node, a ``.pbstream`` file is automatically saved. +For instance, with the 3D backpack example: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-14-14-00.bag + roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag + +Watch the output on the commandline until the node terminates. +It will have written ``b3-2016-04-05-14-14-00.bag.pbstream`` which represents the Cartographer state after it processed all data and finished all optimizations. + +When running as an online node, Cartographer doesn't know when your bag (or sensor input) ends so you need to use the exposed services to explicitly finish the current trajectory and make Cartographer serialize its current state: + +.. code-block:: bash + + # Finish the first trajectory. No further data will be accepted on it. + rosservice call /finish_trajectory 0 + + # Ask Cartographer to serialize its current state. + # (press tab to quickly expand the parameter syntax) + rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream', include_unfinished_submaps: "true"}" + +Once you've retrieved your ``.pbstream`` file, you can run the assets writer with the `sample pipeline`_ for the 3D backpack: + +.. _sample pipeline: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua + +.. code-block:: bash + + roslaunch cartographer_ros assets_writer_backpack_3d.launch \ + bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag \ + pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream + +All output files are prefixed with ``--output_file_prefix`` which defaults to the filename of the first bag. +For the last example, if you specify ``points.ply`` in the pipeline configuration file, this will translate to ``${HOME}/Downloads/b3-2016-04-05-14-14-00.bag_points.ply``. + +Configuration +------------- + +The assets writer is modeled as a pipeline of `PointsProcessor`_ steps. +`PointsBatch`_ data flows through each processor and they all have the chance to modify the ``PointsBatch`` before passing it on. + +.. _PointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_processor.h +.. _PointsBatch: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_batch.h + +For example the `assets_writer_backpack_3d.lua`_ pipeline uses ``min_max_range_filter`` to remove points that are either too close or too far from the sensor. +After this, it saves "*X-Rays*" (translucent side views of the map), then recolors the ``PointsBatch``\ s depending on the sensor frame ids and writes another set of X-Rays using these new colors. + +.. _assets_writer_backpack_3d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua + +The available ``PointsProcessor``\ s are all defined in the `cartographer/io`_ sub-directory and documented in their individual header files. + +.. _cartographer/io: https://github.com/cartographer-project/cartographer/tree/f1ac8967297965b8eb6f2f4b08a538e052b5a75b/cartographer/io + +* **color_points**: Colors points with a fixed color by frame_id. +* **dump_num_points**: Passes through points, but keeps track of how many points it saw and output that on Flush. +* **fixed_ratio_sampler**: Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1. makes this filter a no-op. +* **frame_id_filter**: Filters all points with blacklisted frame_id or a non-whitelisted frame id. Note that you can either specify the whitelist or the blacklist, but not both at the same time. +* **write_hybrid_grid**: Creates a hybrid grid of the points with voxels being 'voxel_size' big. 'range_data_inserter' options are used to configure the range data ray tracing through the hybrid grid. +* **intensity_to_color**: Applies ('intensity' - min) / (max - min) * 255 and color the point grey with this value for each point that comes from the sensor with 'frame_id'. If 'frame_id' is empty, this applies to all points. +* **min_max_range_filtering**: Filters all points that are farther away from their 'origin' as 'max_range' or closer than 'min_range'. +* **voxel_filter_and_remove_moving_objects**: Voxel filters the data and only passes on points that we believe are on non-moving objects. +* **write_pcd**: Streams a PCD file to disk. The header is written in 'Flush'. +* **write_ply**: Streams a PLY file to disk. The header is written in 'Flush'. +* **write_probability_grid**: Creates a probability grid with the specified 'resolution'. As all points are projected into the x-y plane the z component of the data is ignored. 'range_data_inserter' options are used to configure the range data ray tracing through the probability grid. +* **write_xray_image**: Creates X-ray cuts through the points with pixels being 'voxel_size' big. +* **write_xyz**: Writes ASCII xyz points. + +First-person visualization of point clouds +------------------------------------------ + +Two ``PointsProcessor``\ s are of particular interest: ``pcd_writing`` and ``ply_writing`` can save a point cloud in a ``.pcd`` or ``.ply`` file format. +These file formats can then be used by specialized software such as `point_cloud_viewer`_ or `meshlab`_ to navigate through the high resolution map. + +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer +.. _meshlab: http://www.meshlab.net/ + +The typical assets writer pipeline for this outcome is composed of an IntensityToColorPointsProcessor_ giving points a non-white color, then a PlyWritingPointsProcessor_ exporting the results to a ``.ply`` point cloud. +An example of such a pipeline is in `assets_writer_backpack_2d.lua`_. + +.. _IntensityToColorPointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/intensity_to_color_points_processor.cc +.. _PlyWritingPointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/ply_writing_points_processor.h +.. _assets_writer_backpack_2d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua + +Once you have the ``.ply``, follow the README of `point_cloud_viewer`_ to generate an on-disk octree data structure which can be viewed by one of the viewers (SDL or web based) in the same repo. +Note that color is required for ``point_cloud_viewer`` to function. + +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer + +.. image:: point_cloud_viewer_demo_3d.jpg diff --git a/carto_ws/src/cartographer_ros-master/docs/source/compilation.rst b/carto_ws/src/cartographer_ros-master/docs/source/compilation.rst new file mode 100644 index 0000000..ace433f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/compilation.rst @@ -0,0 +1,97 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +========================== +Compiling Cartographer ROS +========================== + +System Requirements +=================== + +The Cartographer ROS requirements are the same as `the ones from Cartographer`_. + +The following `ROS distributions`_ are currently supported: + +* Kinetic +* Melodic +* Noetic + +.. _the ones from Cartographer: https://google-cartographer.readthedocs.io/en/latest/#system-requirements +.. _ROS distributions: http://wiki.ros.org/Distributions + +Building & Installation +======================= + +In order to build Cartographer ROS, we recommend using `wstool `_ and `rosdep +`_. For faster builds, we also recommend using +`Ninja `_. + +On Ubuntu Focal with ROS Noetic use these commands to install the above tools: + +.. code-block:: bash + + sudo apt-get update + sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow + +On older distributions: + +.. code-block:: bash + + sudo apt-get update + sudo apt-get install -y python-wstool python-rosdep ninja-build stow + +After the tools are installed, create a new cartographer_ros workspace in 'catkin_ws'. + +.. code-block:: bash + + mkdir catkin_ws + cd catkin_ws + wstool init src + wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall + wstool update -t src + +Now you need to install cartographer_ros' dependencies. +First, we use ``rosdep`` to install the required packages. +The command 'sudo rosdep init' will print an error if you have already executed it since installing ROS. This error can be ignored. + +.. code-block:: bash + + sudo rosdep init + rosdep update + rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y + + # Only on Ubuntu 16 / ROS Kinetic: src/cartographer/scripts/install_proto3.sh + +Cartographer uses the `abseil-cpp`_ library that needs to be manually installed using this script: + +.. code-block:: bash + + src/cartographer/scripts/install_abseil.sh + +Due to conflicting versions you might need to uninstall the ROS abseil-cpp using + +.. code-block:: bash + + sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp + +Build and install. + +.. code-block:: bash + + catkin_make_isolated --install --use-ninja + +This builds Cartographer from the latest HEAD of the master branch. +If you want a specific version, you need to change the version in the cartographer_ros.rosinstall. + +.. _abseil-cpp: https://abseil.io/ diff --git a/carto_ws/src/cartographer_ros-master/docs/source/conf.py b/carto_ws/src/cartographer_ros-master/docs/source/conf.py new file mode 100644 index 0000000..fc8fe8d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/conf.py @@ -0,0 +1,275 @@ +# -*- coding: utf-8 -*- + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cartographer documentation build configuration file, created by +# sphinx-quickstart on Fri Jul 8 10:41:33 2016. +# +# This file is execfile()d with the current directory set to its +# containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys +import os +from datetime import datetime + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.todo', + 'sphinx.ext.mathjax', + 'sphinx.ext.intersphinx', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'Cartographer ROS' +copyright = u'{year} The Cartographer Authors'.format(year=datetime.now().year) + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +#version = '' +# The full version, including alpha/beta/rc tags. +#release = '' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = [] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +#keep_warnings = False + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = [] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +#html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Output file base name for HTML help builder. +htmlhelp_basename = 'CartographerROSdoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + #'papersize': 'letterpaper', + + # The font size ('10pt', '11pt' or '12pt'). + #'pointsize': '10pt', + + # Additional stuff for the LaTeX preamble. + #'preamble': '', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [('index', 'CartographerROS.tex', + u'Cartographer ROS Documentation', + u'The Cartographer Authors', 'manual'),] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [('index', 'cartographer', u'Cartographer ROS Documentation', + [u'The Cartographer Authors'], 1)] + +# If true, show URL addresses after external links. +#man_show_urls = False + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ('index', 'CartographerROS', u'Cartographer ROS Documentation', + u'The Cartographer Authors', 'Cartographer ROS', + 'Provides ROS integration for Cartographer.', 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +#texinfo_no_detailmenu = False + +intersphinx_mapping = { + 'cartographer': ('https://google-cartographer.readthedocs.io/en/latest/', + None) +} diff --git a/carto_ws/src/cartographer_ros-master/docs/source/configuration.rst b/carto_ws/src/cartographer_ros-master/docs/source/configuration.rst new file mode 100644 index 0000000..67a59e0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/configuration.rst @@ -0,0 +1,142 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +========================================= +Lua configuration reference documentation +========================================= + +Note that Cartographer's ROS integration uses `tf2`_, thus all frame IDs are +expected to contain only a frame name (lower-case with underscores) and no +prefix or slashes. See `REP 105`_ for commonly used coordinate frames. + +Note that topic names are given as *base* names (see `ROS Names`_) in +Cartographer's ROS integration. This means it is up to the user of the +Cartographer node to remap, or put them into a namespace. + +The following are Cartographer's ROS integration top-level options, all of which +must be specified in the Lua configuration file: + +map_frame + The ROS frame ID to use for publishing submaps, the parent frame of poses, + usually "map". + +tracking_frame + The ROS frame ID of the frame that is tracked by the SLAM algorithm. If an IMU + is used, it should be at its position, although it might be rotated. A common + choice is "imu_link". + +published_frame + The ROS frame ID to use as the child frame for publishing poses. For example + "odom" if an "odom" frame is supplied by a different part of the system. In + this case the pose of "odom" in the *map_frame* will be published. Otherwise, + setting it to "base_link" is likely appropriate. + +odom_frame + Only used if *provide_odom_frame* is true. The frame between *published_frame* + and *map_frame* to be used for publishing the (non-loop-closed) local SLAM + result. Usually "odom". + +provide_odom_frame + If enabled, the local, non-loop-closed, continuous pose will be published as + the *odom_frame* in the *map_frame*. + +publish_frame_projected_to_2d + If enabled, the published pose will be restricted to a pure 2D pose (no roll, + pitch, or z-offset). This prevents potentially unwanted out-of-plane poses in + 2D mode that can occur due to the pose extrapolation step (e.g. if the pose + shall be published as a 'base-footprint'-like frame) + +use_odometry + If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry + must be provided in this case, and the information will be included in SLAM. + +use_nav_sat + If enabled, subscribes to `sensor_msgs/NavSatFix`_ on the topic "fix". + Navigation data must be provided in this case, and the information will be + included in the global SLAM. + +use_landmarks + If enabled, subscribes to `cartographer_ros_msgs/LandmarkList`_ on the topic + "landmarks". Landmarks must be provided, as `cartographer_ros_msgs/LandmarkEntry`_ within `cartographer_ros_msgs/LandmarkList`_. If `cartographer_ros_msgs/LandmarkEntry`_ data is provided the information + will be included in the SLAM accoding to the ID of the `cartographer_ros_msgs/LandmarkEntry`_. The `cartographer_ros_msgs/LandmarkList`_ should be provided at a sample rate comparable to the other sensors. The list can be empty but has to be provided because Cartographer strictly time orders sensor data in order to make the landmarks deterministic. However it is possible to set the trajectory builder option "collate_landmarks" to false and allow for a non-deterministic but also non-blocking approach. + +num_laser_scans + Number of laser scan topics to subscribe to. Subscribes to + `sensor_msgs/LaserScan`_ on the "scan" topic for one laser scanner, or topics + "scan_1", "scan_2", etc. for multiple laser scanners. + +num_multi_echo_laser_scans + Number of multi-echo laser scan topics to subscribe to. Subscribes to + `sensor_msgs/MultiEchoLaserScan`_ on the "echoes" topic for one laser scanner, + or topics "echoes_1", "echoes_2", etc. for multiple laser scanners. + +num_subdivisions_per_laser_scan + Number of point clouds to split each received (multi-echo) laser scan into. + Subdividing a scan makes it possible to unwarp scans acquired while the + scanners are moving. There is a corresponding trajectory builder option to + accumulate the subdivided scans into a point cloud that will be used for scan + matching. + +num_point_clouds + Number of point cloud topics to subscribe to. Subscribes to + `sensor_msgs/PointCloud2`_ on the "points2" topic for one rangefinder, or + topics "points2_1", "points2_2", etc. for multiple rangefinders. + +lookup_transform_timeout_sec + Timeout in seconds to use for looking up transforms using `tf2`_. + +submap_publish_period_sec + Interval in seconds at which to publish the submap poses, e.g. 0.3 seconds. + +pose_publish_period_sec + Interval in seconds at which to publish poses, e.g. 5e-3 for a frequency of + 200 Hz. + +publish_to_tf + Enable or disable providing of TF transforms. + +publish_tracked_pose + Enable publishing of tracked pose as a `geometry_msgs/PoseStamped`_ to topic "tracked_pose". + +trajectory_publish_period_sec + Interval in seconds at which to publish the trajectory markers, e.g. 30e-3 + for 30 milliseconds. + +rangefinder_sampling_ratio + Fixed ratio sampling for range finders messages. + +odometry_sampling_ratio + Fixed ratio sampling for odometry messages. + +fixed_frame_sampling_ratio + Fixed ratio sampling for fixed frame messages. + +imu_sampling_ratio + Fixed ratio sampling for IMU messages. + +landmarks_sampling_ratio + Fixed ratio sampling for landmarks messages. + +.. _REP 105: http://www.ros.org/reps/rep-0105.html +.. _ROS Names: http://wiki.ros.org/Names +.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html +.. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html +.. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html +.. _sensor_msgs/LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html +.. _sensor_msgs/MultiEchoLaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/MultiEchoLaserScan.html +.. _sensor_msgs/PointCloud2: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html +.. _sensor_msgs/NavSatFix: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html +.. _cartographer_ros_msgs/LandmarkList: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/LandmarkList.msg +.. _cartographer_ros_msgs/LandmarkEntry: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkEntry.msg +.. _tf2: http://wiki.ros.org/tf2 diff --git a/carto_ws/src/cartographer_ros-master/docs/source/data.rst b/carto_ws/src/cartographer_ros-master/docs/source/data.rst new file mode 100644 index 0000000..f92853d --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/data.rst @@ -0,0 +1,337 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +=========== +Public Data +=========== + +2D Cartographer Backpack – Deutsches Museum +=========================================== + +This data was collected using a 2D LIDAR backpack at the +`Deutsches Museum `_. +Each bag contains data from an IMU, data from a horizontal LIDAR intended for 2D +SLAM, and data from an additional vertical (i.e. push broom) LIDAR. + +License +------- + +Copyright 2016 The Cartographer Authors + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +Data +---- + +=================================================================================================================================== ======== ====== ===== ============================ +`ROS Bag `_ Duration Size Floor Known Issues +=================================================================================================================================== ======== ====== ===== ============================ +`b0-2014-07-11-10-58-16.bag `_ 149 s 38 MB 1. OG +`b0-2014-07-11-11-00-49.bag `_ 513 s 135 MB 1. OG +`b0-2014-07-21-12-42-53.bag `_ 244 s 64 MB 1. OG +`b0-2014-07-21-12-49-19.bag `_ 344 s 93 MB EG 1 gap in vertical laser data +`b0-2014-07-21-12-55-35.bag `_ 892 s 237 MB EG +`b0-2014-07-21-13-11-35.bag `_ 615 s 162 MB EG +`b0-2014-08-14-13-23-01.bag `_ 768 s 204 MB 1. OG +`b0-2014-08-14-13-36-48.bag `_ 331 s 87 MB 1. OG +`b0-2014-10-07-12-13-36.bag `_ 470 s 125 MB 1. OG +`b0-2014-10-07-12-34-42.bag `_ 491 s 127 MB 1. OG +`b0-2014-10-07-12-43-25.bag `_ 288 s 77 MB 1. OG +`b0-2014-10-07-12-50-07.bag `_ 815 s 215 MB 1. OG +`b1-2014-09-25-10-11-12.bag `_ 1829 s 480 MB EG +`b1-2014-10-02-14-08-42.bag `_ 930 s 245 MB 1. OG +`b1-2014-10-02-14-33-25.bag `_ 709 s 181 MB 1. OG +`b1-2014-10-07-12-12-04.bag `_ 737 s 194 MB 1. OG +`b1-2014-10-07-12-34-51.bag `_ 766 s 198 MB 1. OG +`b2-2014-11-24-14-20-50.bag `_ 679 s 177 MB 1. OG +`b2-2014-11-24-14-33-46.bag `_ 1285 s 330 MB 1. OG +`b2-2014-12-03-10-14-13.bag `_ 1051 s 275 MB 1. OG +`b2-2014-12-03-10-33-51.bag `_ 356 s 89 MB 1. OG +`b2-2014-12-03-10-40-04.bag `_ 453 s 119 MB 1. OG +`b2-2014-12-12-13-51-02.bag `_ 1428 s 368 MB 1. OG +`b2-2014-12-12-14-18-43.bag `_ 1164 s 301 MB 1. OG +`b2-2014-12-12-14-41-29.bag `_ 168 s 46 MB 1. OG +`b2-2014-12-12-14-48-22.bag `_ 243 s 65 MB 1. OG +`b2-2014-12-17-14-33-12.bag `_ 1061 s 277 MB 1. OG +`b2-2014-12-17-14-53-26.bag `_ 246 s 62 MB 1. OG +`b2-2014-12-17-14-58-13.bag `_ 797 s 204 MB EG +`b2-2015-02-16-12-26-11.bag `_ 901 s 236 MB 1. OG +`b2-2015-02-16-12-43-57.bag `_ 1848 s 475 MB 1. OG +`b2-2015-04-14-14-16-36.bag `_ 1353 s 349 MB 1. OG +`b2-2015-04-14-14-39-59.bag `_ 670 s 172 MB 1. OG +`b2-2015-04-28-13-01-40.bag `_ 618 s 162 MB 1. OG +`b2-2015-04-28-13-17-23.bag `_ 2376 s 613 MB 1. OG +`b2-2015-05-12-12-29-05.bag `_ 942 s 240 MB 1. OG 2 gaps in laser data +`b2-2015-05-12-12-46-34.bag `_ 2281 s 577 MB 1. OG 14 gaps in laser data +`b2-2015-05-26-13-15-25.bag `_ 747 s 195 MB 1. OG +`b2-2015-06-09-14-31-16.bag `_ 1297 s 336 MB 1. OG +`b2-2015-06-25-14-25-51.bag `_ 1071 s 272 MB 1. OG +`b2-2015-07-07-11-27-05.bag `_ 1390 s 362 MB 1. OG +`b2-2015-07-21-13-03-21.bag `_ 894 s 239 MB 1. OG +`b2-2015-08-04-13-39-24.bag `_ 809 s 212 MB 1. OG +`b2-2015-08-18-11-42-31.bag `_ 588 s 155 MB UG +`b2-2015-08-18-11-55-04.bag `_ 504 s 130 MB UG +`b2-2015-08-18-12-06-34.bag `_ 1299 s 349 MB EG +`b2-2015-09-01-11-55-40.bag `_ 1037 s 274 MB UG +`b2-2015-09-01-12-16-13.bag `_ 918 s 252 MB EG +`b2-2015-09-15-14-19-11.bag `_ 859 s 225 MB 1. OG +`b2-2015-11-24-14-12-27.bag `_ 843 s 226 MB 1. OG +`b2-2016-01-19-14-10-47.bag `_ 310 s 81 MB 1. OG +`b2-2016-02-02-14-01-56.bag `_ 787 s 213 MB EG 1 gap in laser data +`b2-2016-03-01-14-09-37.bag `_ 948 s 255 MB EG +`b2-2016-03-15-14-23-01.bag `_ 810 s 215 MB EG +`b2-2016-04-05-14-44-52.bag `_ 360 s 94 MB 1. OG +`b2-2016-04-27-12-31-41.bag `_ 881 s 234 MB 1. OG +=================================================================================================================================== ======== ====== ===== ============================ + +3D Cartographer Backpack – Deutsches Museum +=========================================== + +This data was collected using a 3D LIDAR backpack at the +`Deutsches Museum `_. +Each bag contains data from an IMU and from two Velodyne VLP-16 LIDARs, +one mounted horizontally (i.e. spin axis up) and one vertically +(i.e. push broom). + +License +------- + +Copyright 2016 The Cartographer Authors + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +Data +---- + +==================================================================================================================================================== ======== ====== ================================================ +`ROS Bag `_ Duration Size Known Issues +==================================================================================================================================================== ======== ====== ================================================ +`b3-2015-12-10-12-41-07.bag `_ 1466 s 7.3 GB 1 large gap in data, no intensities +`b3-2015-12-10-13-10-17.bag `_ 718 s 5.5 GB 1 gap in data, no intensities +`b3-2015-12-10-13-31-28.bag `_ 720 s 5.2 GB 2 large gaps in data, no intensities +`b3-2015-12-10-13-55-20.bag `_ 429 s 3.3 GB +`b3-2015-12-14-15-13-53.bag `_ 916 s 7.1 GB no intensities +`b3-2016-01-19-13-26-24.bag `_ 1098 s 8.1 GB no intensities +`b3-2016-01-19-13-50-11.bag `_ 318 s 2.5 GB no intensities +`b3-2016-02-02-13-32-01.bag `_ 47 s 366 MB no intensities +`b3-2016-02-02-13-33-30.bag `_ 1176 s 9.0 GB no intensities +`b3-2016-02-09-13-17-39.bag `_ 529 s 4.0 GB +`b3-2016-02-09-13-31-50.bag `_ 801 s 6.1 GB no intensities +`b3-2016-02-10-08-08-26.bag `_ 3371 s 25 GB +`b3-2016-03-01-13-39-41.bag `_ 382 s 2.9 GB +`b3-2016-03-01-15-42-37.bag `_ 3483 s 17 GB 6 large gaps in data, no intensities +`b3-2016-03-01-16-42-00.bag `_ 313 s 2.4 GB no intensities +`b3-2016-03-02-10-09-32.bag `_ 1150 s 6.6 GB 3 large gaps in data, no intensities +`b3-2016-04-05-13-54-42.bag `_ 829 s 6.1 GB no intensities +`b3-2016-04-05-14-14-00.bag `_ 1221 s 9.1 GB +`b3-2016-04-05-15-51-36.bag `_ 30 s 231 MB +`b3-2016-04-05-15-52-20.bag `_ 377 s 2.7 GB no intensities +`b3-2016-04-05-16-00-55.bag `_ 940 s 6.9 GB no intensities +`b3-2016-04-27-12-25-00.bag `_ 2793 s 23 GB +`b3-2016-04-27-12-56-11.bag `_ 2905 s 21 GB +`b3-2016-05-10-12-56-33.bag `_ 1767 s 13 GB +`b3-2016-06-07-12-42-49.bag `_ 596 s 3.9 GB 3 gaps in horizontal laser data, no intensities +==================================================================================================================================================== ======== ====== ================================================ + +MiR +=========================================== + +This data was collected using `MiR100 `_. +An additional Logitech Webcam C930e Full HD camera was attached on top to +collect images for landmark detection. + +License +------- + +Copyright 2018 The Cartographer Authors + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +Data +---- + +==================================================================================================================================================== ======== ======= +`ROS Bag `_ Duration Size +==================================================================================================================================================== ======== ======= +`landmarks_demo_uncalibrated.bag `_ 180 s 41.7 MB +==================================================================================================================================================== ======== ======= + +PR2 – Willow Garage +=================== + +This is the Willow Garage data set, described in: + +* "An Object-Based Semantic World Model for Long-Term Change Detection and + Semantic Querying.", by Julian Mason and Bhaskara Marthi, IROS 2012. + +More details about these data can be found in: + +* "Unsupervised Discovery of Object Classes with a Mobile Robot", by Julian + Mason, Bhaskara Marthi, and Ronald Parr. ICRA 2014. +* "Object Discovery with a Mobile Robot" by Julian Mason. PhD Thesis, 2013. + +License +------- + +Copyright (c) 2011, Willow Garage +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +* Neither the name of the nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +Data +---- + +===================================================================================================================== ======================= +`ROS Bag `_ Known Issues +===================================================================================================================== ======================= +`2011-08-03-16-16-43.bag `_ Missing base laser data +`2011-08-03-20-03-22.bag `_ +`2011-08-04-12-16-23.bag `_ +`2011-08-04-14-27-40.bag `_ +`2011-08-04-23-46-28.bag `_ +`2011-08-05-09-27-53.bag `_ +`2011-08-05-12-58-41.bag `_ +`2011-08-05-23-19-43.bag `_ +`2011-08-08-09-48-17.bag `_ +`2011-08-08-14-26-55.bag `_ +`2011-08-08-23-29-37.bag `_ +`2011-08-09-08-49-52.bag `_ +`2011-08-09-14-32-35.bag `_ +`2011-08-09-22-31-30.bag `_ +`2011-08-10-09-36-26.bag `_ +`2011-08-10-14-48-32.bag `_ +`2011-08-11-01-31-15.bag `_ +`2011-08-11-08-36-01.bag `_ +`2011-08-11-14-27-41.bag `_ +`2011-08-11-22-03-37.bag `_ +`2011-08-12-09-06-48.bag `_ +`2011-08-12-16-39-48.bag `_ +`2011-08-12-22-46-34.bag `_ +`2011-08-15-17-22-26.bag `_ +`2011-08-15-21-26-26.bag `_ +`2011-08-16-09-20-08.bag `_ +`2011-08-16-18-40-52.bag `_ +`2011-08-16-20-59-00.bag `_ +`2011-08-17-15-51-51.bag `_ +`2011-08-17-21-17-05.bag `_ +`2011-08-18-20-33-16.bag `_ +`2011-08-18-20-52-30.bag `_ +`2011-08-19-10-12-20.bag `_ +`2011-08-19-14-17-55.bag `_ +`2011-08-19-21-35-17.bag `_ +`2011-08-22-10-02-27.bag `_ +`2011-08-22-14-53-33.bag `_ +`2011-08-23-01-11-53.bag `_ +`2011-08-23-09-21-17.bag `_ +`2011-08-24-09-52-14.bag `_ +`2011-08-24-15-01-39.bag `_ +`2011-08-24-19-47-10.bag `_ +`2011-08-25-09-31-05.bag `_ +`2011-08-25-20-14-56.bag `_ +`2011-08-25-20-38-39.bag `_ +`2011-08-26-09-58-19.bag `_ +`2011-08-29-15-48-07.bag `_ +`2011-08-29-21-14-07.bag `_ +`2011-08-30-08-55-28.bag `_ +`2011-08-30-20-49-42.bag `_ +`2011-08-30-21-17-56.bag `_ +`2011-08-31-20-29-19.bag `_ +`2011-08-31-20-44-19.bag `_ +`2011-09-01-08-21-33.bag `_ +`2011-09-02-09-20-25.bag `_ +`2011-09-06-09-04-41.bag `_ +`2011-09-06-13-20-36.bag `_ +`2011-09-08-13-14-39.bag `_ +`2011-09-09-13-22-57.bag `_ +`2011-09-11-07-34-22.bag `_ +`2011-09-11-09-43-46.bag `_ +`2011-09-12-14-18-56.bag `_ +`2011-09-12-14-47-01.bag `_ +`2011-09-13-10-23-31.bag `_ +`2011-09-13-13-44-21.bag `_ +`2011-09-14-10-19-20.bag `_ +`2011-09-15-08-32-46.bag `_ +===================================================================================================================== ======================= + +Magazino +======== + +Datasets recorded on `Magazino robots `_. + +See the `cartographer_magazino `_ +repository for an integration of Magazino robot data for Cartographer. + +See the ``LICENSE`` file in ``cartographer_magazino`` for details on the dataset +license. + +Data +---- + +=================================================================================================================================== ======== ======== ============ +`ROS Bag `_ Duration Size Known Issues +=================================================================================================================================== ======== ======== ============ +`hallway_return.bag `_ 350 s 102.8 MB +`hallway_localization.bag `_ 137 s 40.4 MB +=================================================================================================================================== ======== ======== ============ diff --git a/carto_ws/src/cartographer_ros-master/docs/source/demo_2d.gif b/carto_ws/src/cartographer_ros-master/docs/source/demo_2d.gif new file mode 100644 index 0000000..f6df65a Binary files /dev/null and b/carto_ws/src/cartographer_ros-master/docs/source/demo_2d.gif differ diff --git a/carto_ws/src/cartographer_ros-master/docs/source/demos.rst b/carto_ws/src/cartographer_ros-master/docs/source/demos.rst new file mode 100644 index 0000000..dac214f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/demos.rst @@ -0,0 +1,126 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +====================================== +Running Cartographer ROS on a demo bag +====================================== + +Now that Cartographer and Cartographer's ROS integration are installed, you can +download example bags (e.g. 2D and 3D backpack collections of the +`Deutsches Museum `_) to a +known location, in this case ``~/Downloads``, and use ``roslaunch`` to bring up +the demo. + +The launch files will bring up ``roscore`` and ``rviz`` automatically. + +.. warning:: When you want to run cartographer_ros, you might need to source your ROS environment by running ``source install_isolated/setup.bash`` first (replace bash with zsh if your shell is zsh) + +Deutsches Museum +================ + +Download and launch the 2D backpack demo: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag + roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag + +Download and launch the 3D backpack demo: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag + roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag + +Pure localization +================= + +Pure localization uses 2 different bags. The first one is used to generate the map, the second to run pure localization. + +Download the 2D bags from the Deutsche Museum: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag + +Generate the map (wait until cartographer_offline_node finishes) and then run pure localization: + +.. code-block:: bash + + roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag + roslaunch cartographer_ros demo_backpack_2d_localization.launch \ + load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \ + bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag + +Download the 3D bags from the Deutsche Museum: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag + +Generate the map (wait until cartographer_offline_node finishes) and then run pure localization: + +.. code-block:: bash + + roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag + roslaunch cartographer_ros demo_backpack_3d_localization.launch \ + load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \ + bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag + +Static landmarks +================ + + .. raw:: html + + + + .. code-block:: bash + + # Download the landmarks example bag. + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/mir/landmarks_demo_uncalibrated.bag + + # Launch the landmarks demo. + roslaunch cartographer_mir offline_mir_100_rviz.launch bag_filename:=${HOME}/Downloads/landmarks_demo_uncalibrated.bag + +Revo LDS +======== + +Download and launch an example bag captured from a low-cost Revo Laser Distance Sensor from Neato Robotics vacuum cleaners: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag + roslaunch cartographer_ros demo_revo_lds.launch bag_filename:=${HOME}/Downloads/cartographer_paper_revo_lds.bag + +PR2 +=== + +Download and launch an example bag captured from a PR2 R&D humanoid robot from Willow Garage: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/pr2/2011-09-15-08-32-46.bag + roslaunch cartographer_ros demo_pr2.launch bag_filename:=${HOME}/Downloads/2011-09-15-08-32-46.bag + +Taurob Tracker +============== + +Download and launch an example bag captured from a Taurob Tracker teleoperation robot: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/taurob_tracker/taurob_tracker_simulation.bag + roslaunch cartographer_ros demo_taurob_tracker.launch bag_filename:=${HOME}/Downloads/taurob_tracker_simulation.bag diff --git a/carto_ws/src/cartographer_ros-master/docs/source/faq.rst b/carto_ws/src/cartographer_ros-master/docs/source/faq.rst new file mode 100644 index 0000000..c0e607f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/faq.rst @@ -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. + +========================== +Frequently asked questions +========================== + +Why is laser data rate in the 3D bags higher than the maximum reported 20 Hz rotation speed of the VLP-16? +---------------------------------------------------------------------------------------------------------- + +The VLP-16 in the example bags is configured to rotate at 20 Hz. However, the +frequency of UDP packets the VLP-16 sends is much higher and independent of +the rotation frequency. The example bags contain a `sensor_msgs/PointCloud2`__ +per UDP packet, not one per revolution. + +__ http://www.ros.org/doc/api/sensor_msgs/html/msg/PointCloud2.html + +In the `corresponding Cartographer configuration file`__ you see +`TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160` which means we +accumulate 160 per-UDP-packet point clouds into one larger point cloud, which +incorporates motion estimation by combining constant velocity and IMU +measurements, for matching. Since there are two VLP-16s, 160 UDP packets is +enough for roughly 2 revolutions, one per VLP-16. + +__ https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/configuration_files/backpack_3d.lua + +Why is IMU data required for 3D SLAM but not for 2D? +---------------------------------------------------- + +In 2D, Cartographer supports running the correlative scan matcher, which is normally used for finding loop closure constraints, for local SLAM. +It is computationally expensive but can often render the incorporation of odometry or IMU data unnecessary. +2D also has the benefit of assuming a flat world, i.e. up is implicitly defined. + +In 3D, an IMU is required mainly for measuring gravity. +Gravity is an attractive quantity to measure since it does not drift and is a very strong signal and typically comprises most of any measured accelerations. +Gravity is needed for two reasons: + +1. There are no assumptions about the world in 3D. +To properly world align the resulting trajectory and map, gravity is used to define the z-direction. + +2. Roll and pitch can be derived quite well from IMU readings once the direction of gravity has been established. +This saves work for the scan matcher by reducing the search window in these dimensions. + +How do I build cartographer_ros without rviz support? +----------------------------------------------------- + +The simplest solution is to create an empty file named `CATKIN_IGNORE`__ in the `cartographer_rviz` package directory. + +__ http://wiki.ros.org/catkin/workspaces + +How do I fix the "You called InitGoogleLogging() twice!" error? +--------------------------------------------------------------- + +Building `rosconsole` with the `glog` back end can lead to this error. +Use the `log4cxx` or `print` back end, selectable via the `ROSCONSOLE_BACKEND` CMake argument, to avoid this issue. diff --git a/carto_ws/src/cartographer_ros-master/docs/source/frames_demo_2d.jpg b/carto_ws/src/cartographer_ros-master/docs/source/frames_demo_2d.jpg new file mode 100644 index 0000000..6de5563 Binary files /dev/null and b/carto_ws/src/cartographer_ros-master/docs/source/frames_demo_2d.jpg differ diff --git a/carto_ws/src/cartographer_ros-master/docs/source/getting_involved.rst b/carto_ws/src/cartographer_ros-master/docs/source/getting_involved.rst new file mode 100644 index 0000000..6ada085 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/getting_involved.rst @@ -0,0 +1,35 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +================ +Getting involved +================ + +Cartographer is developed in the open and allows anyone to contribute to the project. +There are multiple ways to get involved! + +If you have question or think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_. + +.. _GitHub issue: https://github.com/cartographer-project/cartographer/issues + +If you have an idea of a significant change that should be documented and discussed before finding its way into Cartographer, you should submit it as a pull request to `the RFCs repository`_ first. +Simpler changes can also be discussed in GitHub issues so that developers can help you get things right from the first try. + +.. _the RFCs repository: https://github.com/cartographer-project/rfcs + +If you want to contribute code or documentation, this is done through `GitHub pull requests`_. +Pull requests need to follow the `contribution guidelines`_. + +.. _GitHub pull requests: https://github.com/cartographer-project/cartographer/pulls +.. _contribution guidelines: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md diff --git a/carto_ws/src/cartographer_ros-master/docs/source/going_further.rst b/carto_ws/src/cartographer_ros-master/docs/source/going_further.rst new file mode 100644 index 0000000..3e94757 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/going_further.rst @@ -0,0 +1,80 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +============= +Going further +============= + +Cartographer is not only a great SLAM algorithm, it also comes with a fully-featured implementation that brings lots of "extra" features. +This page lists some of those less known functionalities. + +More input +========== + +If you have a source of odometry (such as a wheel encoder) publishing on a ``nav_msgs/Odometry`` topic and want to use it to improve Cartographer's localization, you can add an input to your ``.lua`` configuration files: + +.. code-block:: lua + + use_odometry = true + +The messages will be expected on the ``odom`` topic. + +A GPS publishing on a ``sensor_msgs/NavSatFix`` topic named ``fix`` can improve the global SLAM: + +.. code-block:: lua + + use_nav_sat = true + +For landmarks publishing on a ``cartographer_ros_msgs/LandmarkList`` (`message defined in cartographer_ros`_) topic named ``landmark``: + +.. code-block:: lua + + use_landmarks = true + +.. _message defined in cartographer_ros: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkList.msg + +Localization only +================= + +If you have a map you are happy with and want to reduce computations, you can use the localization-only mode of Cartographer which will run SLAM against the existing map and won't build a new one. +This is enabled by running ``cartographer_node`` with a ``-load_state_filename`` argument and by defining the following line in your lua config: + +.. code-block:: lua + + TRAJECTORY_BUILDER.pure_localization_trimmer = { + max_submaps_to_keep = 3, + } + +IMU Calibration +=============== + +When performing the global optimization, Ceres tries to improve the pose between your IMU and range finding sensors. +A well chosen acquisition with lots of loop closure constraints (for instance if your robot goes on a straight line and then back) can improve the quality of those corrections and become a reliable source of pose correction. +You can then use Cartographer as part of your calibration process to improve the quality of your robot's extrinsic calibration. + +Multi-trajectories SLAM +======================= + +Cartographer can perform SLAM from multiple robots emiting data in parallel. +The global SLAM is able to detect shared paths and will merge the maps built by the different robots as soon as it becomes possible. +This is achieved through the usage of two ROS services ``start_trajectory`` and ``finish_trajectory``. (refer to the ROS API reference documentation for more details on their usage) + +Cloud integration with gRPC +=========================== + +Cartographer is built around Protobuf messages which make it very flexible and interoperable. +One of the advantages of that architecture is that it is easy to distribute on machines spread over the Internet. +The typical use case would be a fleet of robots navigating on a known map, they could have their SLAM algorithm run on a remote powerful centralized localization server running a multi-trajectories Cartographer instance. + +**TODO**: Instructions on how to get started with a gRPC Cartographer instance diff --git a/carto_ws/src/cartographer_ros-master/docs/source/index.rst b/carto_ws/src/cartographer_ros-master/docs/source/index.rst new file mode 100644 index 0000000..03b85c0 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/index.rst @@ -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. + +============================ +Cartographer ROS Integration +============================ + +`Cartographer`_ is a system that provides real-time simultaneous localization +and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor +configurations. This project provides Cartographer's ROS integration. + +.. image:: demo_2d.gif + +.. toctree:: + :maxdepth: 2 + + compilation + demos + your_bag + algo_walkthrough + tuning + assets_writer + going_further + getting_involved + configuration + ros_api + data + faq + +.. _Cartographer: https://github.com/cartographer-project/cartographer +.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping diff --git a/carto_ws/src/cartographer_ros-master/docs/source/nodes_graph_demo_2d.jpg b/carto_ws/src/cartographer_ros-master/docs/source/nodes_graph_demo_2d.jpg new file mode 100644 index 0000000..19df2f3 Binary files /dev/null and b/carto_ws/src/cartographer_ros-master/docs/source/nodes_graph_demo_2d.jpg differ diff --git a/carto_ws/src/cartographer_ros-master/docs/source/point_cloud_viewer_demo_3d.jpg b/carto_ws/src/cartographer_ros-master/docs/source/point_cloud_viewer_demo_3d.jpg new file mode 100644 index 0000000..90c5bb0 Binary files /dev/null and b/carto_ws/src/cartographer_ros-master/docs/source/point_cloud_viewer_demo_3d.jpg differ diff --git a/carto_ws/src/cartographer_ros-master/docs/source/ros_api.rst b/carto_ws/src/cartographer_ros-master/docs/source/ros_api.rst new file mode 100644 index 0000000..7227d01 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/ros_api.rst @@ -0,0 +1,235 @@ +.. Copyright 2016 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +=============================== +ROS API reference documentation +=============================== + +.. image:: nodes_graph_demo_2d.jpg + +Cartographer Node +================= + +The `cartographer_node`_ is the SLAM node used for online, real-time SLAM. + +.. _cartographer_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node_main.cc + +Command-line Flags +------------------ + +Call the node with the ``--help`` flag to see all available options. + +Subscribed Topics +----------------- + +The following range data topics are mutually exclusive. At least one source of +range data is required. + +scan (`sensor_msgs/LaserScan`_) + Supported in 2D and 3D (e.g. using an axially rotating planar laser scanner). + If *num_laser_scans* is set to 1 in the :doc:`configuration`, this topic will + be used as input for SLAM. If *num_laser_scans* is greater than 1, multiple + numbered scan topics (i.e. scan_1, scan_2, scan_3, ... up to and including + *num_laser_scans*) will be used as inputs for SLAM. + +echoes (`sensor_msgs/MultiEchoLaserScan`_) + Supported in 2D and 3D (e.g. using an axially rotating planar laser scanner). + If *num_multi_echo_laser_scans* is set to 1 in the :doc:`configuration`, this + topic will be used as input for SLAM. Only the first echo is used. If + *num_multi_echo_laser_scans* is greater than 1, multiple numbered echoes + topics (i.e. echoes_1, echoes_2, echoes_3, ... up to and including + *num_multi_echo_laser_scans*) will be used as inputs for SLAM. + +points2 (`sensor_msgs/PointCloud2`_) + If *num_point_clouds* is set to 1 in the :doc:`configuration`, this topic will + be used as input for SLAM. If *num_point_clouds* is greater than 1, multiple + numbered points2 topics (i.e. points2_1, points2_2, points2_3, ... up to and + including *num_point_clouds*) will be used as inputs for SLAM. + +The following additional sensor data topics may also be provided: + +imu (`sensor_msgs/Imu`_) + Supported in 2D (optional) and 3D (required). This topic will be used as + input for SLAM. + +odom (`nav_msgs/Odometry`_) + Supported in 2D (optional) and 3D (optional). If *use_odometry* is + enabled in the :doc:`configuration`, this topic will be used as input for + SLAM. + +.. TODO: add NavSatFix? Landmarks? + +Published Topics +---------------- + +scan_matched_points2 (`sensor_msgs/PointCloud2`_) + Point cloud as it was used for the purpose of scan-to-submap matching. This + cloud may be both filtered and projected depending on the + :doc:`configuration`. + +submap_list (`cartographer_ros_msgs/SubmapList`_) + List of all submaps, including the pose and latest version number of each + submap, across all trajectories. + +tracked_pose (`geometry_msgs/PoseStamped`_) + Only published if the parameter ``publish_tracked_pose`` is set to ``true``. + The pose of the tracked frame with respect to the map frame. + +Services +-------- + +All services responses include also a ``StatusResponse`` that comprises a ``code`` and a ``message`` field. +For consistency, the integer ``code`` is equivalent to the status codes used in the `gRPC`_ API. + +.. _gRPC: https://developers.google.com/maps-booking/reference/grpc-api/status_codes + +submap_query (`cartographer_ros_msgs/SubmapQuery`_) + Fetches the requested submap. + +start_trajectory (`cartographer_ros_msgs/StartTrajectory`_) + Starts a trajectory using default sensor topics and the provided configuration. + An initial pose can be optionally specified. Returns an assigned trajectory ID. + +trajectory_query (`cartographer_ros_msgs/TrajectoryQuery`_) + Returns the trajectory data from the pose graph. + +finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_) + Finishes the given `trajectory_id`'s trajectory by running a final optimization. + +write_state (`cartographer_ros_msgs/WriteState`_) + Writes the current internal state to disk into `filename`. The file will + usually end up in `~/.ros` or `ROS_HOME` if it is set. This file can be used + as input to the `assets_writer_main` to generate assets like probability + grids, X-Rays or PLY files. + +get_trajectory_states (`cartographer_ros_msgs/GetTrajectoryStates`_) + Returns the IDs and the states of the trajectories. + For example, this can be useful to observe the state of Cartographer from a separate node. + +read_metrics (`cartographer_ros_msgs/ReadMetrics`_) + Returns the latest values of all internal metrics of Cartographer. + The collection of runtime metrics is optional and has to be activated with the ``--collect_metrics`` command line flag in the node. + +Required tf Transforms +---------------------- + +.. image:: frames_demo_2d.jpg + +Transforms from all incoming sensor data frames to the :doc:`configured +` *tracking_frame* and *published_frame* must be available. +Typically, these are published periodically by a `robot_state_publisher` or a +`static_transform_publisher`. + +Provided tf Transforms +---------------------- + +The transformation between the :doc:`configured ` *map_frame* +and *published_frame* is provided unless the parameter ``publish_to_tf`` is set to ``false``. + +If *provide_odom_frame* is enabled in the :doc:`configuration`, additionally a continuous +(i.e. unaffected by loop closure) transform between the :doc:`configured +` *odom_frame* and *published_frame* will be provided. + +.. _robot_state_publisher: http://wiki.ros.org/robot_state_publisher +.. _static_transform_publisher: http://wiki.ros.org/tf#static_transform_publisher +.. _cartographer_ros_msgs/FinishTrajectory: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/FinishTrajectory.srv +.. _cartographer_ros_msgs/SubmapList: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg +.. _cartographer_ros_msgs/SubmapQuery: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv +.. _cartographer_ros_msgs/StartTrajectory: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv +.. _cartographer_ros_msgs/TrajectoryQuery: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/TrajectoryQuery.srv +.. _cartographer_ros_msgs/WriteState: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv +.. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv +.. _cartographer_ros_msgs/ReadMetrics: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv +.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html +.. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html +.. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html +.. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html +.. _sensor_msgs/LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html +.. _sensor_msgs/MultiEchoLaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/MultiEchoLaserScan.html +.. _sensor_msgs/PointCloud2: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html + +Offline Node +============ + +The `offline_node`_ is the fastest way of SLAMing a bag of sensor data. +It does not listen on any topics, instead it reads TF and sensor data out of a set of bags provided on the commandline. +It also publishes a clock with the advancing sensor data, i.e. replaces ``rosbag play``. +In all other regards, it behaves like the ``cartographer_node``. +Each bag will become a separate trajectory in the final state. +Once it is done processing all data, it writes out the final Cartographer state and exits. + +.. _offline_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/offline_node_main.cc + + +Published Topics +---------------- + +In addition to the topics that are published by the online node, this node also publishes: + +~bagfile_progress (`cartographer_ros_msgs/BagfileProgress`_) + Bag files processing progress including detailed information about the bag currently being processed which will be published with a predefined + interval that can be specified using ``~bagfile_progress_pub_interval`` ROS parameter. + +.. _cartographer_ros_msgs/BagfileProgress: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/BagfileProgress.msg + +Parameters +---------- + +~bagfile_progress_pub_interval (double, default=10.0): + The interval of publishing bag files processing progress in seconds. + + +Occupancy grid Node +=================== + +The `occupancy_grid_node`_ listens to the submaps published by SLAM, builds an ROS occupancy_grid out of them and publishes it. +This tool is useful to keep old nodes that require a single monolithic map to work happy until new nav stacks can deal with Cartographer's submaps directly. +Generating the map is expensive and slow, so map updates are in the order of seconds. +You can can selectively include/exclude submaps from frozen (static) or active trajectories with a command line option. +Call the node with the ``--help`` flag to see these options. + +.. _occupancy_grid_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc + +Subscribed Topics +----------------- + +It subscribes to Cartographer's ``submap_list`` topic only. + +Published Topics +---------------- + +map (`nav_msgs/OccupancyGrid`_) + If subscribed to, the node will continuously compute and publish the map. The + time between updates will increase with the size of the map. For faster + updates, use the submaps APIs. + + +Pbstream Map Publisher Node +=========================== + +The `pbstream_map_publisher`_ is a simple node that creates a static occupancy grid out of a serialized Cartographer state (pbstream format). +It is an efficient alternative to the occupancy grid node if live updates are not important. + +.. _pbstream_map_publisher: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc + +Subscribed Topics +----------------- + +None. + +Published Topics +---------------- + +map (`nav_msgs/OccupancyGrid`_) + The published occupancy grid topic is latched. diff --git a/carto_ws/src/cartographer_ros-master/docs/source/tuning.rst b/carto_ws/src/cartographer_ros-master/docs/source/tuning.rst new file mode 100644 index 0000000..53ad0d3 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/tuning.rst @@ -0,0 +1,209 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +.. cartographer SHA: aba4575d937df4c9697f61529200c084f2562584 +.. cartographer_ros SHA: 99c23b6ac7874f7974e9ed808ace841da6f2c8b0 +.. TODO(hrapp): mention insert_free_space somewhere + +Tuning methodology +================== + +Tuning Cartographer is unfortunately really difficult. +The system has many parameters many of which affect each other. +This tuning guide tries to explain a principled approach on concrete examples. + +Built-in tools +-------------- + +Cartographer provides built-in tools for SLAM evaluation that can be particularly useful for measuring the local SLAM quality. +They are stand-alone executables that ship with the core ``cartographer`` library and are hence independent, but compatible with ``cartographer_ros``. +Therefore, please head to the `Cartographer Read the Docs Evaluation site`_ for a conceptual overview and a guide on how to use the tools in practice. + +These tools assume that you have serialized the SLAM state to a ``.pbstream`` file. +With ``cartographer_ros``, you can invoke the ``assets_writer`` to serialize the state - see the :ref:`assets_writer` section for more information. + +.. _Cartographer Read the Docs Evaluation site: https://google-cartographer.readthedocs.io/en/latest/evaluation.html + +Example: tuning local SLAM +-------------------------- + +For this example we'll start at ``cartographer`` commit `aba4575`_ and ``cartographer_ros`` commit `99c23b6`_ and look at the bag ``b2-2016-04-27-12-31-41.bag`` from our test data set. + +At our starting configuration, we see some slipping pretty early in the bag. +The backpack passed over a ramp in the Deutsches Museum which violates the 2D assumption of a flat floor. +It is visible in the laser scan data that contradicting information is passed to the SLAM. +But the slipping also indicates that we trust the point cloud matching too much and disregard the other sensors quite strongly. +Our aim is to improve the situation through tuning. + +.. _aba4575: https://github.com/cartographer-project/cartographer/commit/aba4575d937df4c9697f61529200c084f2562584 +.. _99c23b6: https://github.com/cartographer-project/cartographer_ros/commit/99c23b6ac7874f7974e9ed808ace841da6f2c8b0 + +If we only look at this particular submap, that the error is fully contained in one submap. +We also see that over time, global SLAM figures out that something weird happened and partially corrects for it. +The broken submap is broken forever though. + +.. TODO(hrapp): VIDEO + +Since the problem here is slippage inside a submap, it is a local SLAM issue. +So let's turn off global SLAM to not mess with our tuning. + +.. code-block:: lua + + POSE_GRAPH.optimize_every_n_nodes = 0 + +Correct size of submaps +^^^^^^^^^^^^^^^^^^^^^^^ + +The size of submaps is configured through ``TRAJECTORY_BUILDER_2D.submaps.num_range_data``. +Looking at the individual submaps for this example they already fit the two constraints rather well, so we assume this parameter is well tuned. + +Tuning the ``CeresScanMatcher`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +In our case, the scan matcher can freely move the match forward and backwards without impacting the score. +We'd like to penalize this situation by making the scan matcher pay more for deviating from the prior that it got. +The two parameters controlling this are ``TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight`` and ``rotation_weight``. +The higher, the more expensive it is to move the result away from the prior, or in other words: scan matching has to generate a higher score in another position to be accepted. + +For instructional purposes, let's make deviating from the prior really expensive: + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1e3 + +.. TODO(hrapp): video + +This allows the optimizer to pretty liberally overwrite the scan matcher results. +This results in poses close to the prior, but inconsistent with the depth sensor and clearly broken. +Experimenting with this value yields a better result at ``2e2``. + +.. TODO(hrapp): VIDEO with translation_weight = 2e2 + +Here, the scan matcher used rotation to still slightly mess up the result though. +Setting the ``rotation_weight`` to ``4e2`` leaves us with a reasonable result. + +Verification +^^^^^^^^^^^^ + +To make sure that we did not overtune for this particular issue, we need to run the configuration against other collected data. +In this case, the new parameters did reveal slipping, for example at the beginning of ``b2-2016-04-05-14-44-52.bag``, so we had to lower the ``translation_weight`` to ``1e2``. +This setting is worse for the case we wanted to fix, but no longer slips. +Before checking them in, we normalize all weights, since they only have relative meaning. +The result of this tuning was `PR 428`_. +In general, always try to tune for a platform, not a particular bag. + +.. _PR 428: https://github.com/cartographer-project/cartographer/pull/428 + +Special Cases +------------- + +The default configuration and the above tuning steps are focused on quality. +Only after we have achieved good quality, we can further consider special cases. + +Low Latency +^^^^^^^^^^^ + +By low latency, we mean that an optimized local pose becomes available shortly after sensor input was received, +usually within a second, and that global optimization has no backlog. +Low latency is required for online algorithms, such as robot localization. +Local SLAM, which operates in the foreground, directly affects latency. +Global SLAM builds up a queue of background tasks. +When global SLAM cannot keep up the queue, drift can accumulate indefinitely, +so global SLAM should be tuned to work in real time. + +There are many options to tune the different components for speed, and we list them ordered from +the recommended, straightforward ones to the those that are more intrusive. +It is recommended to only explore one option at a time, starting with the first. +Configuration parameters are documented in the `Cartographer documentation`_. + +.. _Cartographer documentation: https://google-cartographer.readthedocs.io/en/latest/configuration.html + +To tune global SLAM for lower latency, we reduce its computational load +until is consistently keeps up with real-time input. +Below this threshold, we do not reduce it further, but try to achieve the best possible quality. +To reduce global SLAM latency, we can + +- decrease ``optimize_every_n_nodes`` +- increase ``MAP_BUILDER.num_background_threads`` up to the number of cores +- decrease ``global_sampling_ratio`` +- decrease ``constraint_builder.sampling_ratio`` +- increase ``constraint_builder.min_score`` +- for the adaptive voxel filter(s), decrease ``.min_num_points``, ``.max_range``, increase ``.max_length`` +- increase ``voxel_filter_size``, ``submaps.resolution``, decrease ``submaps.num_range_data`` +- decrease search windows sizes, ``.linear_xy_search_window``, ``.linear_z_search_window``, ``.angular_search_window`` +- increase ``global_constraint_search_after_n_seconds`` +- decrease ``max_num_iterations`` + +To tune local SLAM for lower latency, we can + +- increase ``voxel_filter_size`` +- increase ``submaps.resolution`` +- for the adaptive voxel filter(s), decrease ``.min_num_points``, ``.max_range``, increase ``.max_length`` +- decrease ``max_range`` (especially if data is noisy) +- decrease ``submaps.num_range_data`` + +Note that larger voxels will slightly increase scan matching scores as a side effect, +so score thresholds should be increased accordingly. + +Pure Localization in a Given Map +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Pure localization is different from mapping. +First, we expect a lower latency of both local and global SLAM. +Second, global SLAM will usually find a very large number of inter constraints between the frozen trajectory +that serves as a map and the current trajectory. + +To tune for pure localization, we should first enable ``TRAJECTORY_BUILDER.pure_localization = true`` and +strongly decrease ``POSE_GRAPH.optimize_every_n_nodes`` to receive frequent results. +With these settings, global SLAM will usually be too slow and cannot keep up. +As a next step, we strongly decrease ``global_sampling_ratio`` and ``constraint_builder.sampling_ratio`` +to compensate for the large number of constraints. +We then tune for lower latency as explained above until the system reliably works in real time. + +If you run in ``pure_localization``, ``submaps.resolution`` **should be matching** with the resolution of the submaps in the ``.pbstream`` you are running on. +Using different resolutions is currently untested and may not work as expected. + +Odometry in Global Optimization +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If a separate odometry source is used as an input for local SLAM (``use_odometry = true``), we can also tune the global SLAM to benefit from this additional information. + +There are in total four parameters that allow us to tune the individual weights of local SLAM and odometry in the optimization: + +.. code-block:: lua + + POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight + POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight + POSE_GRAPH.optimization_problem.odometry_translation_weight + POSE_GRAPH.optimization_problem.odometry_rotation_weight + +We can set these weights depending on how much we trust either local SLAM or the odometry. +By default, odometry is weighted into global optimization similar to local slam (scan matching) poses. +However, odometry from wheel encoders often has a high uncertainty in rotation. +In this case, the rotation weight can be reduced, even down to zero. + +Still have a problem ? +---------------------- + +If you can't get Cartographer to work reliably on your data, you can open a `GitHub issue`_ asking for help. +Developers are keen to help, but they can only be helpful if you follow `an issue template`_ containing the result of ``rosbag_validate``, a link to a fork of ``cartographer_ros`` with your config and a link to a ``.bag`` file reproducing your problem. + +.. note:: + + There are already lots of GitHub issues with all sorts of problems solved by the developers. Going through `the closed issues of cartographer_ros`_ and `of cartographer`_ is a great way to learn more about Cartographer and maybe find a solution to your problem ! + +.. _GitHub issue: https://github.com/cartographer-project/cartographer_ros/issues +.. _an issue template: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question +.. _the closed issues of cartographer_ros: https://github.com/cartographer-project/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed +.. _of cartographer: https://github.com/cartographer-project/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed diff --git a/carto_ws/src/cartographer_ros-master/docs/source/your_bag.rst b/carto_ws/src/cartographer_ros-master/docs/source/your_bag.rst new file mode 100644 index 0000000..8d33e0f --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/docs/source/your_bag.rst @@ -0,0 +1,142 @@ +.. Copyright 2018 The Cartographer Authors + +.. Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + +.. http://www.apache.org/licenses/LICENSE-2.0 + +.. Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +======================================== +Running Cartographer ROS on your own bag +======================================== + +Now that you've run Cartographer ROS on a couple of provided bags, you can go ahead and make Cartographer work with your own data. +Find a ``.bag`` recording you would like to use for SLAM and go through this tutorial. + +.. warning:: When you want to run cartographer_ros, you might need to source your ROS environment by running ``source install_isolated/setup.bash`` first (replace bash with zsh if your shell is zsh) + +Validate your bag +================= + +Cartographer ROS provides a tool named ``cartographer_rosbag_validate`` to automatically analyze data present in your bag. +It is generally a good idea to run this tool before trying to tune Cartographer for incorrect data. + +It benefits from the experience of the Cartographer authors and can detect a variety of mistakes commonly found in bags. +For instance, if a ``sensor_msgs/Imu`` topic is detected, the tool will make sure that the gravity vector has not been removed from the IMU measurements because the gravity norm is used by Cartographer to determine the direction of the ground. + +The tool can also provide tips on how to improve the quality of your data. +For example, with a Velodyne LIDAR, it is recommended to have one ``sensor_msgs/PointCloud2`` message per UDP packet sent by the sensor instead of one message per revolution. +With that granularity, Cartographer is then able to unwarp the point clouds deformation caused by the robot's motion and results in better reconstruction. + +If you have sourced your Cartographer ROS environment, you can simply run the tool like this: + +.. code-block:: bash + + cartographer_rosbag_validate -bag_filename your_bag.bag + +Create a .lua configuration +=========================== + +Cartographer is highly flexible and can be configured to work on a variety of robots. +The robot configuration is read from a ``options`` data structure that must be defined from a Lua script. +The example configurations are defined in ``src/cartographer_ros/cartographer_ros/configuration_files`` and installed in ``install_isolated/share/cartographer_ros/configuration_files/``. + +.. note:: Ideally, a .lua configuration should be robot-specific and not bag-specific. + +You can start by copying one of the example and then adapt it to your own need. If you want to use 3D SLAM: + +.. code-block:: bash + + cp install_isolated/share/cartographer_ros/configuration_files/backpack_3d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua + +If you want to use 2D SLAM: + +.. code-block:: bash + + cp install_isolated/share/cartographer_ros/configuration_files/backpack_2d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua + +You can then edit ``my_robot.lua`` to suit the needs of your robot. +The values defined in the ``options`` block define how the Cartographer ROS frontend should interface with your bag. +The values defined after the ``options`` paragraph are used to tune the inner-working of Cartographer, we will ignore these for now. + +.. seealso:: The `reference documentation of the Cartographer ROS configuration values`_ and of `the Cartographer configuration values`_. + +.. _reference documentation of the Cartographer ROS configuration values: https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html + +.. _the Cartographer configuration values: https://google-cartographer.readthedocs.io/en/latest/configuration.html + +Among the values you need to adapt, you probably have to provide the TF frame IDs of your environment and robot in ``map_frame``, ``tracking_frame``, ``published_frame`` and ``odom_frame``. + +.. note:: You can either distribute your robot's TF tree from a ``/tf`` topic in your bag or define it in a ``.urdf`` robot definition. + +.. warning:: You should trust your poses! A small offset on the link between your robot and IMU or LIDAR can lead to incoherent map reconstructions. Cartographer can usually correct small pose errors but not everything! + +The other values you need to define are related to the number and type of sensors you would like to use. + +- ``num_laser_scans``: Number of ``sensor_msgs/LaserScan`` topics you'll use. +- ``num_multi_echo_laser_scans``: Number of ``sensor_msgs/MultiEchoLaserScan`` topics you'll use. +- ``num_point_clouds``: Number of ``sensor_msgs/PointCloud2`` topics you'll use. + +You can also enable the usage of landmarks and GPS as additional sources of localization using ``use_landmarks`` and ``use_nav_sat``. The rest of the variables in the ``options`` block should typically be left untouched. + +.. note:: even if you use a 2D SLAM, the landmarks are 3D objects and can mislead you if viewed only on the 2D plane due to their third dimension. + +However, there is one global variable that you absolutely need to adapt to the needs of your bag: ``TRAJECTORY_BUILDER_3D.num_accumulated_range_data`` or ``TRAJECTORY_BUILDER_2D.num_accumulated_range_data``. +This variable defines the number of messages required to construct a full scan (typically, a full revolution). +If you follow ``cartographer_rosbag_validate``'s advices and use 100 ROS messages per scan, you can set this variable to 100. +If you have two range finding sensors (for instance, two LIDARs) providing their full scans all at once, you should set this variable to 2. + +Create .launch files for your SLAM scenarios +============================================ + +You may have noticed that each demo introduced in the previous section was run with a different roslaunch command. +The recommended usage of Cartographer is indeed to provide a custom ``.launch`` file per robot and type of SLAM. +The example ``.launch`` files are defined in ``src/cartographer_ros/cartographer_ros/launch`` and installed in ``install_isolated/share/cartographer_ros/launch/``. + +Start by copying one of the provided example: + +.. code-block:: bash + + cp install_isolated/share/cartographer_ros/launch/backpack_3d.launch install_isolated/share/cartographer_ros/launch/my_robot.launch + cp install_isolated/share/cartographer_ros/launch/demo_backpack_3d.launch install_isolated/share/cartographer_ros/launch/demo_my_robot.launch + cp install_isolated/share/cartographer_ros/launch/offline_backpack_3d.launch install_isolated/share/cartographer_ros/launch/offline_my_robot.launch + cp install_isolated/share/cartographer_ros/launch/demo_backpack_3d_localization.launch install_isolated/share/cartographer_ros/launch/demo_my_robot_localization.launch + cp install_isolated/share/cartographer_ros/launch/assets_writer_backpack_3d.launch install_isolated/share/cartographer_ros/launch/assets_writer_my_robot.launch + +- ``my_robot.launch`` is meant to be used on the robot to execute SLAM online (in real time) with real sensors data. +- ``demo_my_robot.launch`` is meant to be used from a development machine and expects a ``bag_filename`` argument to replay data from a recording. This launch file also spawns a rviz window configured to visualize Cartographer's state. +- ``offline_my_robot.launch`` is very similar to ``demo_my_robot.launch`` but tries to execute SLAM as fast as possible. This can make map building significantly faster. This launch file can also use multiple bag files provided to the ``bag_filenames`` argument. +- ``demo_my_robot_localization.launch`` is very similar to ``demo_my_robot.launch`` but expects a ``load_state_filename`` argument pointing to a ``.pbstream`` recording of a previous Cartographer execution. The previous recording will be used as a pre-computed map and Cartographer will only perform localization on this map. +- ``assets_writer_my_robot.launch`` is used to extract data out of a ``.pbstream`` recording of a previous Cartographer execution. + +Again, a few adaptations need to be made to those files to suit your robot. + +- Every parameter given to ``-configuration_basename`` should be adapted to point to ``my_robot.lua``. +- If you decided to use a ``.urdf`` description of your robot, you should place your description in ``install_isolated/share/cartographer_ros/urdf`` and adapt the ``robot_description`` parameter to point to your file name. +- If you decided to use ``/tf`` messages, you can remove the ``robot_description`` parameter, the ``robot_state_publisher`` node and the lines statring with ``-urdf``. +- If the topic names published by your bag or sensors don't match the ones expected by Cartographer ROS, you can use ```` elements to redirect your topics. The expected topic names depend on the type of range finding devices you use. + +.. note:: + + - The IMU topic is expected to be named "imu" + - If you use only one ``sensor_msgs/LaserScan`` topic, it is expected to be named ``scan``. If you have more, they should be named ``scan_1``, ``scan_2`` etc... + - If you use only one ``sensor_msgs/MultiEchoLaserScan`` topic, it is expected to be named ``echoes``. If you have more, they should be named ``echoes_1``, ``echoes_2`` etc... + - If you use only one ``sensor_msgs/PointCloud2`` topic, it is expected be named ``points2``. If you have more, they should be named ``points2_1``, ``points2_2``, etc... + +Try your configuration +====================== + +Everything is setup! You can now start Cartographer with: + +.. code-block:: bash + + roslaunch cartographer_ros my_robot.launch bag_filename:=/path/to/your_bag.bag + +If you are lucky enough, everything should already work as expected. +However, you might have some problems that require tuning. diff --git a/carto_ws/src/cartographer_ros-master/scripts/catkin_test_results.sh b/carto_ws/src/cartographer_ros-master/scripts/catkin_test_results.sh new file mode 100644 index 0000000..5c7b3ea --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/catkin_test_results.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +. /opt/ros/${ROS_DISTRO}/setup.sh + +cd catkin_ws +catkin_test_results $@ diff --git a/carto_ws/src/cartographer_ros-master/scripts/check_access_token.sh b/carto_ws/src/cartographer_ros-master/scripts/check_access_token.sh new file mode 100644 index 0000000..da82968 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/check_access_token.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +# Usage: ./check_access_token.sh ACCESS_TOKEN +# Returns non-zero exit code if ACCESS_TOKEN is invalid. + +if [ "$#" -ne 1 ]; then + echo "Please provide an access token to $0" 1>&2 + exit 1 +fi +token=$1 + +set -e +function on_error { + echo "Failed to validate GitHub access token!" 1>&2 + exit 1 +} +trap on_error ERR + +test_response=$(curl -s https://api.github.com/?access_token=${token}) + +echo $test_response | grep -ivq "bad credentials" +echo $"GitHub access token is valid." diff --git a/carto_ws/src/cartographer_ros-master/scripts/install.sh b/carto_ws/src/cartographer_ros-master/scripts/install.sh new file mode 100644 index 0000000..3c34abf --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/install.sh @@ -0,0 +1,32 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +. /opt/ros/${ROS_DISTRO}/setup.sh + +cd catkin_ws + +# Build, install, and test. +# +# It's necessary to use the '--install' flag for every call to +# 'catkin_make_isolated' in order to avoid the use of 'devel_isolated' as the +# 'CMAKE_INSTALL_PREFIX' for non-test targets. This in itself is important to +# avoid any issues caused by using 'CMAKE_INSTALL_PREFIX' during the +# configuration phase of the build (e.g. cartographer/common/config.h.cmake). +export BUILD_FLAGS="--use-ninja --install-space /opt/cartographer_ros --install" +catkin_make_isolated ${BUILD_FLAGS} $@ diff --git a/carto_ws/src/cartographer_ros-master/scripts/install_debs.sh b/carto_ws/src/cartographer_ros-master/scripts/install_debs.sh new file mode 100644 index 0000000..0af4648 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/install_debs.sh @@ -0,0 +1,39 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +# Install CMake, Ninja, stow. +sudo apt-get update +sudo apt-get install -y lsb-release cmake ninja-build stow + +# Install GMock library and header files for newer distributions. +if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]] +then + sudo apt-get install -y libgmock-dev +fi + +. /opt/ros/${ROS_DISTRO}/setup.sh + +cd catkin_ws + +# Install rosdep dependencies. +rosdep update +rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y + +# Update rosconsole-bridge to fix build issue with Docker image for Kinetic +sudo apt-get install ros-${ROS_DISTRO}-rosconsole-bridge -y diff --git a/carto_ws/src/cartographer_ros-master/scripts/load_docker_cache.sh b/carto_ws/src/cartographer_ros-master/scripts/load_docker_cache.sh new file mode 100644 index 0000000..4c37247 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/load_docker_cache.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cache intermediate Docker layers. For a description of how this works, see: +# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html + +set -o errexit +set -o verbose +set -o pipefail + +if [ -f ${DOCKER_CACHE_FILE} ]; then + gunzip -c ${DOCKER_CACHE_FILE} | docker load; +fi diff --git a/carto_ws/src/cartographer_ros-master/scripts/prepare_catkin_workspace.sh b/carto_ws/src/cartographer_ros-master/scripts/prepare_catkin_workspace.sh new file mode 100644 index 0000000..77b870e --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/prepare_catkin_workspace.sh @@ -0,0 +1,35 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +. /opt/ros/${ROS_DISTRO}/setup.sh + +# Create a new workspace in 'catkin_ws'. +mkdir -p catkin_ws/src +cd catkin_ws/src +wstool init + +# Merge the cartographer_ros.rosinstall file and fetch code for dependencies. +wstool merge ../../cartographer_ros/cartographer_ros.rosinstall +# We default to master and wstool seems to have a bug when it's being set twice. +# TODO(MichaelGrupp): wstool is unmaintained, use vcstool. +if [ ${CARTOGRAPHER_VERSION} != "master" ]; then + wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y +fi +wstool remove cartographer_ros +wstool update diff --git a/carto_ws/src/cartographer_ros-master/scripts/prepare_jenkins_catkin_workspace.sh b/carto_ws/src/cartographer_ros-master/scripts/prepare_jenkins_catkin_workspace.sh new file mode 100644 index 0000000..9c96542 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/prepare_jenkins_catkin_workspace.sh @@ -0,0 +1,33 @@ +#!/bin/sh + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit +set -o verbose + +. /opt/ros/${ROS_DISTRO}/setup.sh + +# Create a new workspace in 'catkin_ws'. +mkdir -p catkin_ws/src +cd catkin_ws/src +wstool init + +# Merge the cartographer_ros.rosinstall file and fetch code for dependencies. +wstool merge ../../cartographer_ros/cartographer_ros.rosinstall +wstool merge -y https://raw.githubusercontent.com/cartographer-project/cartographer_fetch/master/cartographer_fetch.rosinstall +wstool merge -y https://raw.githubusercontent.com/magazino/cartographer_magazino/master/cartographer_magazino.rosinstall +wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y +wstool remove cartographer_ros +wstool update diff --git a/carto_ws/src/cartographer_ros-master/scripts/ros_entrypoint.sh b/carto_ws/src/cartographer_ros-master/scripts/ros_entrypoint.sh new file mode 100644 index 0000000..56b3291 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/ros_entrypoint.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set -o errexit + +source "/opt/ros/${ROS_DISTRO}/setup.bash" +source "/opt/cartographer_ros/setup.bash" +exec "$@" diff --git a/carto_ws/src/cartographer_ros-master/scripts/save_docker_cache.sh b/carto_ws/src/cartographer_ros-master/scripts/save_docker_cache.sh new file mode 100644 index 0000000..4f5dc59 --- /dev/null +++ b/carto_ws/src/cartographer_ros-master/scripts/save_docker_cache.sh @@ -0,0 +1,30 @@ +#!/bin/bash + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Cache intermediate Docker layers. For a description of how this works, see: +# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html + +set -o errexit +set -o verbose +set -o pipefail + +if [[ ${TRAVIS_BRANCH} == "master" ]] && + [[ ${TRAVIS_PULL_REQUEST} == "false" ]]; then + mkdir -p $(dirname ${DOCKER_CACHE_FILE}) + IMAGE_NAMES=$(docker history -q cartographer_ros:${ROS_RELEASE} | grep -v '') + docker save ${IMAGE_NAMES} | gzip > ${DOCKER_CACHE_FILE}.new + mv ${DOCKER_CACHE_FILE}.new ${DOCKER_CACHE_FILE} +fi diff --git a/readme.txt b/readme.txt new file mode 100644 index 0000000..81e1c16 --- /dev/null +++ b/readme.txt @@ -0,0 +1,11 @@ +注意：融合代码运行 +设备号： +main.cpp中第20行，“Ranging r(9, 640, 480);“,"9"代表设备号video9。注意查看摄像头设备号，并相应更改。 + +ranging.cpp中 +代码第121行，y_b = int(y_b+y_b*0.03)“0.03”是针对地毯、抹布、体重秤的测距参数，测距如果偏大，则增大该值，如果测距偏小，则减小该值。该测距效果体现以rviz上雷达显示为准，视频的显示与之无关。 +代码第236行，disp_pixel_x = (int)(disp_pixel + disp_pixel * 0.12) “0.12”这个值与测距有关，测距结果见图片中物体上方d：显示，标定后测距如果有误差可以调整该值，增大会使测距值减小。 + +激光和雷达标定使用Calibration Toolkit工具，标定过程可以参考https://blog.csdn.net/yourgreatfather/article/details/88245566，标定后的参数替换fusion.h对应的参数。 +fusion.h中的r_c、t、r_y为矫正矩阵，r_c矫正旋转偏移量、t矫正平移偏移量、r_y矫正像素坐标系y轴方向偏移。 +fusion.h中的c2lR和c2lT为双目测距的坐标系到雷达坐标系之间的转换和平移矩阵（二维坐标系），可利用测量工具测算得出，实际使用过程中可进行细微矫正。 diff --git a/requirement.txt b/requirement.txt new file mode 100644 index 0000000..dfda1d6 --- /dev/null +++ b/requirement.txt @@ -0,0 +1,2 @@ +opencv4.x +lidar.h, ladar.cppʹõĿ⺯͹ٷLD14״ͬ \ No newline at end of file diff --git a/后处理/map.cpp b/后处理/map.cpp new file mode 100644 index 0000000..a63611d --- /dev/null +++ b/后处理/map.cpp @@ -0,0 +1,52 @@ +#include"ros/ros.h" +#include "nav_msgs/OccupancyGrid.h" +#include "std_msgs/String.h" +#include +#include +#include + + + + +void callback(const nav_msgs::OccupancyGrid::ConstPtr& mapmsg) +{ + int height,width,k=0; + cv::Mat map(2000,2000,CV_8UC1); + height = mapmsg->info.height; + width = mapmsg->info.width; + std::vector compression_params; + compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION); + compression_params.push_back(0); + + for (int i = 0; i < 2000; i++) + { + for(int j = 0; j < 2000; j++) + { + map.at(i,j) = mapmsg->data[k]; + k++; + } + + } + + cv::imshow("map",map); + if (cv::waitKey(1)==27) + { + cv::imwrite("map.png", map, compression_params); + exit(0); + } + +} + + + +int main(int argc, char *argv[]) +{ + //init_keyboard(); + setlocale(LC_ALL,""); + ros::init(argc,argv,"listener_map"); + ros::NodeHandle nh; + ros::Subscriber map_sub = nh.subscribe("map",1,callback); + ros::spin(); + return 0; + +} diff --git a/后处理/map.out b/后处理/map.out new file mode 100644 index 0000000..b4c2e42 Binary files /dev/null and b/后处理/map.out differ diff --git a/后处理/物体形状/.idea/.gitignore b/后处理/物体形状/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/后处理/物体形状/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/后处理/物体形状/.idea/.name b/后处理/物体形状/.idea/.name new file mode 100644 index 0000000..11a5d8e --- /dev/null +++ b/后处理/物体形状/.idea/.name @@ -0,0 +1 @@ +main.py \ No newline at end of file diff --git a/后处理/物体形状/.idea/inspectionProfiles/profiles_settings.xml b/后处理/物体形状/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/后处理/物体形状/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/后处理/物体形状/.idea/misc.xml b/后处理/物体形状/.idea/misc.xml new file mode 100644 index 0000000..10ea69d --- /dev/null +++ b/后处理/物体形状/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/后处理/物体形状/.idea/modules.xml b/后处理/物体形状/.idea/modules.xml new file mode 100644 index 0000000..78a2c55 --- /dev/null +++ b/后处理/物体形状/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/后处理/物体形状/.idea/物体形状.iml b/后处理/物体形状/.idea/物体形状.iml new file mode 100644 index 0000000..b2df7a1 --- /dev/null +++ b/后处理/物体形状/.idea/物体形状.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/后处理/物体形状/__pycache__/config.cpython-37.pyc b/后处理/物体形状/__pycache__/config.cpython-37.pyc new file mode 100644 index 0000000..40bdf35 Binary files /dev/null and b/后处理/物体形状/__pycache__/config.cpython-37.pyc differ diff --git a/后处理/物体形状/config.py b/后处理/物体形状/config.py new file mode 100644 index 0000000..c6cfb95 --- /dev/null +++ b/后处理/物体形状/config.py @@ -0,0 +1,123 @@ +import argparse + + + +parser = argparse.ArgumentParser() + +parser.add_argument('--acreage', type=int, default=400, help='Minimum wall area') +parser.add_argument('--length', type=int, default=120, help='Minimum value of wall length in line detection') +parser.add_argument('--k', type=int, default=45, help='door width') +parser.add_argument('--m', type=int, default=100, help='wall length') +parser.add_argument('--log-imgs', type=int, default=16, help='number of images for W&B logging, max 100') +parser.add_argument('--workers', type=int, default=8, help='maximum number of dataloader workers') + +'''语义物体参数设置''' +parser.add_argument('--min_samples_115', type=int, default=4, help='Minimum number of samples') +parser.add_argument('--max_eps_115', type=int, default=4, help='Maximum cluster radius') +parser.add_argument('--shape_115', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_115', type=int, default=1, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_116', type=int, default=10, help='Minimum number of samples') +parser.add_argument('--max_eps_116', type=int, default=15, help='Maximum cluster radius') +parser.add_argument('--shape_116', type=int, default=0, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_116', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_117', type=int, default=10, help='Minimum number of samples') +parser.add_argument('--max_eps_117', type=int, default=15, help='Maximum cluster radius') +parser.add_argument('--shape_117', type=int, default=0, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_117', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_118', type=int, default=16, help='Minimum number of samples') +parser.add_argument('--max_eps_118', type=int, default=28, help='Maximum cluster radius') +parser.add_argument('--shape_118', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_118', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_119', type=int, default=20, help='Minimum number of samples') +parser.add_argument('--max_eps_119', type=int, default=28, help='Maximum cluster radius') +parser.add_argument('--shape_119', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_119', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_120', type=int, default=20, help='Minimum number of samples') +parser.add_argument('--max_eps_120', type=int, default=24, help='Maximum cluster radius') +parser.add_argument('--shape_120', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_120', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_121', type=int, default=12, help='Minimum number of samples') +parser.add_argument('--max_eps_121', type=int, default=10, help='Maximum cluster radius') +parser.add_argument('--shape_121', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_121', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_150', type=int, default=12, help='Minimum number of samples') +parser.add_argument('--max_eps_150', type=int, default=20, help='Maximum cluster radius') +parser.add_argument('--shape_150', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_150', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_151', type=int, default=20, help='Minimum number of samples') +parser.add_argument('--max_eps_151', type=int, default=28, help='Maximum cluster radius') +parser.add_argument('--shape_151', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_151', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_152', type=int, default=16, help='Minimum number of samples') +parser.add_argument('--max_eps_152', type=int, default=60, help='Maximum cluster radius') +parser.add_argument('--shape_152', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_152', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_153', type=int, default=20, help='Minimum number of samples') +parser.add_argument('--max_eps_153', type=int, default=28, help='Maximum cluster radius') +parser.add_argument('--shape_153', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_153', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_154', type=int, default=5, help='Minimum number of samples') +parser.add_argument('--max_eps_154', type=int, default=8, help='Maximum cluster radius') +parser.add_argument('--shape_154', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_154', type=int, default=1, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_155', type=int, default=12, help='Minimum number of samples') +parser.add_argument('--max_eps_155', type=int, default=40, help='Maximum cluster radius') +parser.add_argument('--shape_155', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_155', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_156', type=int, default=30, help='Minimum number of samples') +parser.add_argument('--max_eps_156', type=int, default=56, help='Maximum cluster radius') +parser.add_argument('--shape_156', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_156', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_200', type=int, default=10, help='Minimum number of samples') +parser.add_argument('--max_eps_200', type=int, default=6, help='Maximum cluster radius') +parser.add_argument('--shape_200', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_200', type=int, default=1, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_201', type=int, default=4, help='Minimum number of samples') +parser.add_argument('--max_eps_201', type=int, default=4, help='Maximum cluster radius') +parser.add_argument('--shape_201', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_201', type=int, default=1, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_202', type=int, default=40, help='Minimum number of samples') +parser.add_argument('--max_eps_202', type=int, default=80, help='Maximum cluster radius') +parser.add_argument('--shape_202', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_202', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + + +parser.add_argument('--min_samples_203', type=int, default=50, help='Minimum number of samples') +parser.add_argument('--max_eps_203', type=int, default=80, help='Maximum cluster radius') +parser.add_argument('--shape_203', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_203', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_204', type=int, default=12, help='Minimum number of samples') +parser.add_argument('--max_eps_204', type=int, default=80, help='Maximum cluster radius') +parser.add_argument('--shape_204', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_204', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + + +parser.add_argument('--min_samples_205', type=int, default=10, help='Minimum number of samples') +parser.add_argument('--max_eps_205', type=int, default=14, help='Maximum cluster radius') +parser.add_argument('--shape_205', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_205', type=int, default=0, help='0 means by outline, 1 means regular rectangle') + +parser.add_argument('--min_samples_209', type=int, default=4, help='Minimum number of samples') +parser.add_argument('--max_eps_209', type=int, default=4, help='Maximum cluster radius') +parser.add_argument('--shape_209', type=int, default=1, help='Shape 0 is a circle, shape 1 is a rectangle') +parser.add_argument('--size_209', type=int, default=1, help='0 means by outline, 1 means regular rectangle') + + +opt = parser.parse_args() \ No newline at end of file diff --git a/后处理/物体形状/main.py b/后处理/物体形状/main.py new file mode 100644 index 0000000..09546eb --- /dev/null +++ b/后处理/物体形状/main.py @@ -0,0 +1,1046 @@ +import copy +import math + +import cv2 +import numpy as np +from matplotlib import pyplot +from numpy import where, mean +from scipy import optimize +from sklearn.cluster import OPTICS +from config import opt + + +# 图片预处理 +def process_map(img, opt): + def get_wall_area(map, stats, area): + ''' + . map 获取墙壁的图像 + . stats 连通域信息 + . area 面积 + ''' + temp = [] + for i in range(1, num_labels): + if stats[i, 4] > area: + temp.append(i) + wall_area = np.zeros((map.shape[0], map.shape[1]), np.uint8) + for i in range(0, len(temp)): + mask = labels == temp[i] + wall_area[:, :][mask] = 255 + return wall_area + + def get_NONwall_area(map, stats, area): + ''' + . map 获取墙壁的图像 + . stats 连通域信息 + . area 面积 + ''' + temp = [] + for i in range(1, num_labels): + if stats[i, 4] <= area: + temp.append(i) + wall_area = np.zeros((map.shape[0], map.shape[1]), np.uint8) + for i in range(0, len(temp)): + mask = labels == temp[i] + wall_area[:, :][mask] = 255 + return wall_area + + def get_rotate_angle(wall_area, length, opt): + edges = cv2.Canny(wall_area, 50, 150, apertureSize=3) + lines = cv2.HoughLines(edges, 1, np.pi / 180, length) + + # output = np.zeros((img.shape[0], img.shape[1], 3), np.uint8) + alfa = 0 + for line in lines: + rho = line[0][0] + theta = line[0][1] + a = np.cos(theta) + b = np.sin(theta) + x0 = a * rho + y0 = b * rho + x1 = int(x0 + 1000 * (-b)) + y1 = int(y0 + 1000 * (a)) + x2 = int(x0 - 1000 * (-b)) + y2 = int(y0 - 1000 * (a)) + alfa = alfa + np.mod(theta, 90 / 360 * np.pi) + alfa = theta + # print(theta) + # cv2.line(output, (x1, y1), (x2, y2), (0, 0, 255), 2) + wall_area_ = rotate_bound(wall_area, -np.mod(theta, 90 / 360 * np.pi) / np.pi * 180) # 摆正图像 + # wall_area_ = cv2.dilate(wall_area_, kernel) # 腐蚀膨胀，去除墙上语义点 + connect_line = connect_door(wall_area_, opt.k, opt.m) # 封闭区域 + connect_line_ = rotate_bound(connect_line, np.mod(theta, 90 / 360 * np.pi) / np.pi * 180) + # cv2.namedWindow("wall", 0) + # cv2.imshow("wall", output) + # cv2.waitKey(0) + + alfa = alfa / lines.shape[0] + print(alfa) + + # cv2.namedWindow("wall", 0) + # cv2.imshow("wall", output) + # cv2.waitKey(0) + return connect_line_ + + def rotate_bound(image, angle): + ''' + . 旋转图片 + . @param image opencv读取后的图像 + . @param angle (逆)旋转角度 + ''' + + # img = cv2.imread("img/1.jpg") + (h, w) = image.shape[:2] # 返回(高,宽,色彩通道数),此处取前两个值返回 + # 抓取旋转矩阵(应用角度的负值顺时针旋转)。参数1为旋转中心点;参数2为旋转角度,正的值表示逆时针旋转;参数3为各向同性的比例因子 + M = cv2.getRotationMatrix2D((w / 2, h / 2), -angle, 1.0) + # 计算图像的新边界维数 + + # 调整旋转矩阵以考虑平移 + + # 执行实际的旋转并返回图像 + return cv2.warpAffine(image, M, (image.shape[1], image.shape[0])) # borderValue 缺省，默认是黑色 + # return M + + def get_map(img, ratio): + """ + :param img: + :param ratio: 获取地图，2表示有效面积的2倍 + :return: + """ + ratio = (ratio - 1) / 2 + b = np.nonzero(img) + x_min = np.min(b[0]) + y_min = np.min(b[1]) + x_max = np.max(b[0]) + y_max = np.max(b[1]) + x_l = x_max - x_min + y_l = y_max - y_min + if x_l < 150: + ratio = ratio + 0.5 + # map = np.zeros((x_max - x_min, y_max - y_min)) + map = img[int(max(x_min - ratio * x_l, 0)):int(min(x_max + x_l * ratio, img.shape[1])), + int(max(0, y_min - ratio * y_l)):int(min(img.shape[0], ratio * y_l + y_max))] + ''' + 去掉未占用区域 + ''' + map[map < 70] = 255 + map[map <= 100] = 0 + map[map == 0] = 1 + map[map == 255] = 0 + map[map == 1] = 255 + "墙壁灰度值为1" + return map + + def draw_rgb(map): + def rgb_semantics(number): + output = np.array((5, 5, 5), np.uint8) + if number == 115: + '0号物体' + output[0] = 232 + output[1] = 221 + output[2] = 203 + elif number == 116: + '1号物体' + output[0] = 205 + output[1] = 179 + output[2] = 128 + elif number == 117: + '2号物体' + output[0] = 3 + output[1] = 101 + output[2] = 100 + elif number == 118: + '3号物体' + output[0] = 3 + output[1] = 54 + output[2] = 73 + elif number == 119: + '4号物体' + output[0] = 3 + output[1] = 22 + output[2] = 52 + elif number == 120: + '5号物体' + output[0] = 237 + output[1] = 222 + output[2] = 139 + elif number == 121: + '6号物体' + output[0] = 251 + output[1] = 178 + output[2] = 23 + elif number == 150: + '7号物体' + output[0] = 96 + output[1] = 143 + output[2] = 159 + elif number == 151: + '8号物体' + output[0] = 1 + output[1] = 77 + output[2] = 103 + elif number == 152: + '9号物体' + output[0] = 254 + output[1] = 67 + output[2] = 101 + elif number == 153: + '10号物' + output[0] = 252 + output[1] = 157 + output[2] = 154 + elif number == 154: + '11号物体' + output[0] = 249 + output[1] = 205 + output[2] = 173 + elif number == 155: + '12号物体' + output[0] = 200 + output[1] = 200 + output[2] = 169 + elif number == 156: + '13号物体' + output[0] = 131 + output[1] = 175 + output[2] = 155 + elif number == 200: + '14号物体' + output[0] = 229 + output[1] = 187 + output[2] = 129 + elif number == 201: + '15号物体' + output[0] = 161 + output[1] = 23 + output[2] = 21 + elif number == 202: + '16号物体' + output[0] = 118 + output[1] = 77 + output[2] = 57 + elif number == 203: + '17号物体' + output[0] = 17 + output[1] = 63 + output[2] = 61 + elif number == 204: + '18号物体' + output[0] = 60 + output[1] = 79 + output[2] = 57 + elif number == 205: + '19号物体' + output[0] = 95 + output[1] = 92 + output[2] = 51 + elif number == 206: + '20号物体' + output[0] = 179 + output[1] = 214 + output[2] = 110 + elif number == 207: + '21号物体' + output[0] = 227 + output[1] = 160 + output[2] = 93 + elif number == 208: + '22号物体' + output[0] = 178 + output[1] = 190 + output[2] = 126 + elif number == 209: + '23号物体' + output[0] = 56 + output[1] = 13 + output[2] = 49 + else: + output[0] = 5 + output[1] = 5 + output[2] = 5 + return output + + output = np.zeros((map.shape[0], map.shape[1], 3), np.uint8) + output_b = copy.copy(map) + output_g = copy.copy(map) + output_r = copy.copy(map) + + for i in range(np.array(Intensity_value).shape[0]): + intensity = Intensity_value[i] + output_r[map == intensity] = rgb_semantics(intensity)[0] + output_g[map == intensity] = rgb_semantics(intensity)[1] + output_b[map == intensity] = rgb_semantics(intensity)[2] + + output[:, :, 0] = output_b + output[:, :, 1] = output_g + output[:, :, 2] = output_r + return output + + # 行扫描，间隔k时，进行填充，填充值为1 + + # 将门口连起来 + def connect_door(wall_area, k, m): + """ + + :param wall_area: + :param k: 门宽 + :param m: 墙壁长度 + :return: + """ + + def edge_connection(img, size, k, m): + for i in range(size): + Yi = np.where(img[i, :] > 220) + # print(Yi) + if len(Yi[0]) >= m: # 可调整 (墙壁长度) + for j in range(0, len(Yi[0]) - 1): + if Yi[0][j + 1] - Yi[0][j] <= k: + img[i, Yi[0][j]:Yi[0][j + 1]] = 255 + return img + + img = copy.copy(wall_area) + g = edge_connection(img, img.shape[0], k, m) # k设的是门的宽度, m是判为墙壁的长度 + g = cv2.rotate(g, 0) + g = edge_connection(g, img.shape[1], k, m) + g = cv2.rotate(g, 2) + g = g.astype(np.uint8) + return g + + '定义语义强度' + Intensity_value = [115, 116, 117, 118, 119, 120, 121, 150, 151, 152, 153, + 154, 155, 156, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209] + # Intensity_value = [153] + map = get_map(img, 1.2) + '插值处理，提高分辨率' + map = cv2.resize(map, (map.shape[1] * 2, map.shape[0] * 2), interpolation=cv2.INTER_NEAREST) + + num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(map, connectivity=8) + wall_area = get_wall_area(map, stats, opt.acreage) # 获取墙壁区域 + NON_wall_area = get_NONwall_area(map, stats, opt.acreage) + NON_wall_area = NON_wall_area / 255 + NON_wall_area = NON_wall_area * map + + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + wall_area = cv2.dilate(wall_area, kernel) + + connect_line_ = get_rotate_angle(wall_area, opt.length, opt) # 连接 + + # wall_area_ = rotate_bound(wall_area, -alfa / np.pi * 180) # 摆正图像 + # # wall_area_ = cv2.dilate(wall_area_, kernel) # 腐蚀膨胀，去除墙上语义点 + # + # connect_line = connect_door(wall_area_, opt.k, opt.m) # 封闭区域 + # connect_line_ = rotate_bound(connect_line, alfa / np.pi * 180) + connect_line_[connect_line_ < 150] = 0 + connect_line_[connect_line_ > 0] = 255 + result = connect_line_ + NON_wall_area + result[result > 255] = 255 + result = result.astype(np.uint8) + result[result == 0] = 254 + result[result == 255] = 0 + result[result == 254] = 255 + # result = filter_wrong_point(result, Intensity_value) + # output_rgb = draw_rgb(NON_wall_area) + # connect_line_rgb = np.zeros((map.shape[0], map.shape[1], 3), np.uint8) + # connect_line_rgb[:, :, 0] = copy.copy(connect_line_) + # connect_line_rgb[:, :, 1] = copy.copy(connect_line_) + # connect_line_rgb[:, :, 2] = copy.copy(connect_line_) + + # connect_line_rgb[connect_line_rgb < 65] = 0 + # result_rgb = connect_line_rgb + output_rgb + + # cv2.namedWindow("input_map", 0) + # cv2.imshow("input_map", map) + # cv2.namedWindow("wall", 0) + # cv2.imshow("wall", connect_line) + # cv2.namedWindow("map", 0) + # cv2.imshow("map", map) + # cv2.namedWindow("output", 0) + # cv2.imshow('output', result_rgb) + # cv2.namedWindow("result", 0) + # cv2.imshow('result', result) + # cv2.waitKey() + # cv2.destroyAllWindows() + # cv2.imwrite("result_rgb.png", result_rgb) + cv2.imwrite("result.png", result) + return result + + +# 绘制物体形状 +def obj_outline(map, Semantic_map, opt): + ''' + . 绘制物体形状 + . @param map 语义地图 + . @param Semantic_map 场景分类图 + . @param opt 参数 + ''' + + # 根据不同语义物体得到不同参数 + def get_parameter(item, opt): + ''' + . 得到物体参数 + . @param item 物体的强度值 + . @param opt 物体参数 + ''' + if (item == 115): + min_samples = opt.min_samples_115 + max_eps = opt.max_eps_115 + shape = opt.shape_115 # 0是圆形，1是矩形 + size = opt.size_115 # 0是按轮廓，1是直接画正矩形 + color = [232, 221, 203] + elif (item == 116): + min_samples = opt.min_samples_116 + max_eps = opt.max_eps_116 + shape = opt.shape_116 # 0是圆形，1是矩形 + size = opt.size_116 # 0是按轮廓，1是直接画正矩形 + color = [205, 179, 128] + elif (item == 117): + min_samples = opt.min_samples_117 + max_eps = opt.max_eps_117 + shape = opt.shape_117 # 0是圆形，1是矩形 + size = opt.size_117 # 0是按轮廓，1是直接画正矩形 + color = [3, 101, 100] + elif (item == 118): + min_samples = opt.min_samples_118 + max_eps = opt.max_eps_118 + shape = opt.shape_118 # 0是圆形，1是矩形 + size = opt.size_118 # 0是按轮廓，1是直接画正矩形 + color = [3, 54, 73] + elif (item == 119): + min_samples = opt.min_samples_119 + max_eps = opt.max_eps_119 + shape = opt.shape_119 # 0是圆形，1是矩形 + size = opt.size_119 # 0是按轮廓，1是直接画正矩形 + color = [3, 22, 52] + elif (item == 120): + min_samples = opt.min_samples_120 + max_eps = opt.max_eps_120 + shape = opt.shape_120 # 0是圆形，1是矩形 + size = opt.size_120 # 0是按轮廓，1是直接画正矩形 + color = [237, 22, 139] + elif (item == 121): + min_samples = opt.min_samples_121 + max_eps = opt.max_eps_121 + shape = opt.shape_121 # 0是圆形，1是矩形 + size = opt.size_121 # 0是按轮廓，1是直接画正矩形 + color = [251, 178, 23] + elif (item == 150): + min_samples = opt.min_samples_150 + max_eps = opt.max_eps_150 + shape = opt.shape_150 # 0是圆形，1是矩形 + size = opt.size_150 # 0是按轮廓，1是直接画正矩形 + color = [96, 143, 159] + elif (item == 151): + min_samples = opt.min_samples_151 + max_eps = opt.max_eps_151 + shape = opt.shape_151 # 0是圆形，1是矩形 + size = opt.size_151 # 0是按轮廓，1是直接画正矩形 + color = [1, 77, 103] + elif (item == 152): + min_samples = opt.min_samples_152 + max_eps = opt.max_eps_152 + shape = opt.shape_152 # 0是圆形，1是矩形 + size = opt.size_152 # 0是按轮廓，1是直接画正矩形 + color = [254, 67, 101] + elif (item == 153): + min_samples = opt.min_samples_153 + max_eps = opt.max_eps_153 + shape = opt.shape_153 # 0是圆形，1是矩形 + size = opt.size_153 # 0是按轮廓，1是直接画正矩形 + color = [252, 157, 154] + elif (item == 154): + min_samples = opt.min_samples_154 + max_eps = opt.max_eps_154 + shape = opt.shape_154 # 0是圆形，1是矩形 + size = opt.size_154 # 0是按轮廓，1是直接画正矩形 + color = [249, 205, 173] + elif (item == 155): + min_samples = opt.min_samples_155 + max_eps = opt.max_eps_155 + shape = opt.shape_155 # 0是圆形，1是矩形 + size = opt.size_155 # 0是按轮廓，1是直接画正矩形 + color = [200, 200, 169] + elif (item == 156): + min_samples = opt.min_samples_156 + max_eps = opt.max_eps_156 + shape = opt.shape_156 # 0是圆形，1是矩形 + size = opt.size_156 # 0是按轮廓，1是直接画正矩形 + color = [131, 175, 155] + elif (item == 200): + min_samples = opt.min_samples_200 + max_eps = opt.max_eps_200 + shape = opt.shape_200 # 0是圆形，1是矩形 + size = opt.size_200 # 0是按轮廓，1是直接画正矩形 + color = [229, 187, 129] + elif (item == 201): + min_samples = opt.min_samples_201 + max_eps = opt.max_eps_201 + shape = opt.shape_201 # 0是圆形，1是矩形 + size = opt.size_201 # 0是按轮廓，1是直接画正矩形 + color = [161, 23, 21] + elif (item == 202): + min_samples = opt.min_samples_202 + max_eps = opt.max_eps_202 + shape = opt.shape_202 # 0是圆形，1是矩形 + size = opt.size_202 # 0是按轮廓，1是直接画正矩形 + color = [118, 77, 57] + elif (item == 203): + min_samples = opt.min_samples_203 + max_eps = opt.max_eps_203 + shape = opt.shape_203 # 0是圆形，1是矩形 + size = opt.size_203 # 0是按轮廓，1是直接画正矩形 + color = [17, 63, 61] + elif (item == 204): + min_samples = opt.min_samples_204 + max_eps = opt.max_eps_204 + shape = opt.shape_204 # 0是圆形，1是矩形 + size = opt.size_204 # 0是按轮廓，1是直接画正矩形 + color = [60, 79, 57] + elif (item == 205): + min_samples = opt.min_samples_205 + max_eps = opt.max_eps_205 + shape = opt.shape_205 # 0是圆形，1是矩形 + size = opt.size_205 # 0是按轮廓，1是直接画正矩形 + color = [95, 92, 51] + elif (item == 209): + min_samples = opt.min_samples_209 + max_eps = opt.max_eps_209 + shape = opt.shape_209 # 0是圆形，1是矩形 + size = opt.size_209 # 0是按轮廓，1是直接画正矩形 + color = [56, 13, 49] + + return min_samples, max_eps, shape, size, color + + # 画物体轮廓 + def draw_outline(X, item, min_samples, max_eps, shape, color, size, output): + ''' + . 得到物体参数 + . @param X 语义物体的行列坐标 + . @param item 语义物体的强度值 + . @param min_samples 聚类的最小样本数 + . @param max_eps 聚类的最大半径 + . @param shape 语义物体绘制形状参数（ 0代表绘制圆形，1代表绘制矩形） + . @param color 语义物体的色彩 + . @param size 语义物体绘制轮廓的参数（0代表按轮廓绘制，1代表绘制正矩形） + . @param output 已经绘制过某个物体形状的 map图片 + ''' + + # 聚类（OPTICS算法） + # X = X.astype(np.uint8) + yhat = OPTICS(min_samples=min_samples, max_eps=max_eps, cluster_method='dbscan').fit_predict(X) + # 检索唯一群集 + clusters = np.unique(yhat) + # 画散点图 + # 为每个群集的样本创建散点图 + # for cluster in clusters: + # # 获取此群集的示例的行索引 + # row_ix = where(yhat == cluster) + # # 创建这些样本的散布 + # pyplot.scatter(X[row_ix, 0], X[row_ix, 1]) + # # 绘制散点图 + # pyplot.show() + ''' + 找最小外接矩形 (绘制形状) + ''' + color = color[::-1] + for cluster in clusters: + # 获取此群集的示例的行索引，将每个物体从map中提取出来 + if (cluster == -1): + continue + row_ix = where(yhat == cluster) + # print(row_ix[0].shape[0]) + if row_ix[0].shape[0] <= 1: + continue + if (item == 115 and row_ix[0].shape[0] <= 2): + continue + if (item == 116 and row_ix[0].shape[0] <= 4): + continue + if (item == 117 and row_ix[0].shape[0] <= 4): + continue + if (item == 118 and row_ix[0].shape[0] <= 2): + continue + if (item == 119 and row_ix[0].shape[0] <= 2): + continue + if (item == 120 and row_ix[0].shape[0] <= 2): + continue + if (item == 121 and row_ix[0].shape[0] <= 4): + continue + if (item == 150 and row_ix[0].shape[0] <= 4): + continue + if (item == 151 and row_ix[0].shape[0] <= 4): + continue + if (item == 152 and row_ix[0].shape[0] <= 4): + continue + if (item == 153 and row_ix[0].shape[0] <= 2): + continue + if (item == 154 and row_ix[0].shape[0] <= 2): + continue + if (item == 155 and row_ix[0].shape[0] <= 2): + continue + if (item == 156 and row_ix[0].shape[0] <= 2): + continue + if (item == 200 and row_ix[0].shape[0] <= 2): + continue + if (item == 201 and row_ix[0].shape[0] <= 2): + continue + if (item == 202 and row_ix[0].shape[0] <= 2): + continue + if (item == 203 and row_ix[0].shape[0] <= 2): + continue + if (item == 204 and row_ix[0].shape[0] <= 2): + continue + if (item == 205 and row_ix[0].shape[0] <= 2): + continue + if (item == 209 and row_ix[0].shape[0] <= 2): + continue + obj = np.zeros((map.shape[0], map.shape[1]), np.uint8) + row = X[row_ix, 0].astype(int) + col = X[row_ix, 1].astype(int) + obj[row, col] = 1 + # cv2.namedWindow("obj", 0) + # cv2.imshow('obj',obj) + if (shape == 1): + # 计算外接矩形 + box = outer_rectangle(obj, size) + # color = color[::-1] + + # 画图 + if (size == 0): + # 外接矩形缩放 + # vertices = shrink_rectangle(box) + vertices = box + output = cv2.drawContours(output, [vertices], 0, color, -1, lineType=cv2.LINE_4) + elif (size == 1): + # 外接矩形缩放 + # vertices = shrink_rectangle(box) + vertices = box + cv2.rectangle(output, (vertices[0, 0], vertices[0, 1]), (vertices[2, 0], vertices[2, 1]), color, -1) + elif (shape == 0): + circle_x = col + circle_y = row + center, radius = fit_circle(circle_x, circle_y) + # binary, contours, hierarchy = cv2.findContours(obj, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + # center, radius = outer_circle(obj) + cv2.circle(output, center, radius, color, -1) + + return output + + # 计算最小外接矩形 + def outer_rectangle(obj, size): + ''' + . 计算最小外接矩形 + . @param obj 语义物体的位置信息 + . @param size 语义物体绘制轮廓的参数（0代表按轮廓绘制，1代表绘制正矩形） + ''' + binary, contours, hierarchy = cv2.findContours(obj, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + contour = [] + for cont in contours: + contour.extend(cont) + if (size == 0): + min_rect = cv2.minAreaRect(np.array(contour)) + box = cv2.boxPoints(min_rect) + # box = np.round(box) + box = np.int0(box) + + # 排序 + temp = np.where(box == np.min(box[:, 0])) # box的第0列是列，第1列是行 + # print(temp[0].shape[0]) + if temp[0].shape[0] > 1: + left = np.min(box[:, 0]) + right = np.max(box[:, 0]) + up = np.min(box[:, 1]) + down = np.max(box[:, 1]) + top_point_x = left + top_point_y = up + right_point_x = right + right_point_y = up + bottom_point_x = right + bottom_point_y = down + left_point_x = left + left_point_y = down + vertices = np.array( + [[top_point_x, top_point_y], [right_point_x, right_point_y], [bottom_point_x, bottom_point_y], + [left_point_x, left_point_y]]) + return box + left_point_x = np.min(box[:, 0]) + right_point_x = np.max(box[:, 0]) + top_point_y = np.min(box[:, 1]) + bottom_point_y = np.max(box[:, 1]) + + left_point_y = box[:, 1][np.where(box[:, 0] == left_point_x)][0] + right_point_y = box[:, 1][np.where(box[:, 0] == right_point_x)][0] + top_point_x = box[:, 0][np.where(box[:, 1] == top_point_y)][0] + bottom_point_x = box[:, 0][np.where(box[:, 1] == bottom_point_y)][0] + + vertices = np.array( + [[top_point_x, top_point_y], [right_point_x, right_point_y], [bottom_point_x, bottom_point_y], + [left_point_x, left_point_y]]) + return box + elif (size == 1): + x, y, w, h = cv2.boundingRect(np.array(contour)) + vertices = np.array([[x, y], [x + w, y], [x + w, y + h], + [x, y + h]]) + return vertices + + # 计算最小外接圆 + def outer_circle(obj): + ''' + . 计算最小外接圆 + . @param obj 语义物体的位置信息 + ''' + binary, contours, hierarchy = cv2.findContours(obj, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) + contour = [] + for cont in contours: + contour.extend(cont) + + (x, y), radius = cv2.minEnclosingCircle(np.array(contour)) + + center = (int(round(x)), int(round(y))) + radius = int(round(radius)) + print(x, y, radius) + return center, radius + + # 计算最小拟合圆 + def calc_R(xc, yc): + return np.sqrt((x - xc) ** 2 + (y - yc) ** 2) + + def f_2(c): + Ri = calc_R(*c) + return Ri - Ri.mean() + + def fit_circle(x, y): + ''' + . 计算最小外接圆 + . @param x 语义物体的所在列 + . @param x 语义物体的所在行 + ''' + x_m = mean(x) + y_m = mean(y) + center_estimate = x_m, y_m + center_2, _ = optimize.leastsq(f_2, center_estimate) + xc_2, yc_2 = center_2 + Ri_2 = calc_R(xc_2, yc_2) + # 拟合圆的半径 + R_2 = Ri_2.mean() + center = (int(round(xc_2)), int(round(yc_2))) + radius = int(round(R_2)) + print(xc_2, yc_2, radius) + return center, radius + + # 外接矩形内缩 + def shrink_rectangle(box): + ''' + . 外接矩形内缩 + . @param box 语义物体的轮廓信息 + ''' + temp = np.where(box == np.min(box[:, 0])) # box的第0列是列，第1列是行 + # print(temp[0].shape[0]) + if temp[0].shape[0] > 1: + left = np.min(box[:, 0]) + right = np.max(box[:, 0]) + up = np.min(box[:, 1]) + down = np.max(box[:, 1]) + top_point_x = left + 1 + top_point_y = up + 1 + right_point_x = right - 1 + right_point_y = up + 1 + bottom_point_x = right - 1 + bottom_point_y = down - 1 + left_point_x = left + 1 + left_point_y = down - 1 + vertices = np.array( + [[top_point_x, top_point_y], [right_point_x, right_point_y], [bottom_point_x, bottom_point_y], + [left_point_x, left_point_y]]) + return vertices + + left_point_x = np.min(box[:, 0]) + right_point_x = np.max(box[:, 0]) + top_point_y = np.min(box[:, 1]) + bottom_point_y = np.max(box[:, 1]) + + left_point_y = box[:, 1][np.where(box[:, 0] == left_point_x)][0] + right_point_y = box[:, 1][np.where(box[:, 0] == right_point_x)][0] + top_point_x = box[:, 0][np.where(box[:, 1] == top_point_y)][0] + bottom_point_x = box[:, 0][np.where(box[:, 1] == bottom_point_y)][0] + + vertices = np.array( + [[top_point_x, top_point_y], [right_point_x, right_point_y], [bottom_point_x, bottom_point_y], + [left_point_x, left_point_y]]) + + # print(vertices) + + if top_point_x > bottom_point_x: + vertices[0, 0] = vertices[0, 0] - 1 + vertices[0, 1] = vertices[0, 1] + 1 + vertices[2, 0] = vertices[2, 0] + 1 + vertices[2, 1] = vertices[2, 1] - 1 + if top_point_x < bottom_point_x: + vertices[0, 0] = vertices[0, 0] + 1 + vertices[0, 1] = vertices[0, 1] + 1 + vertices[2, 0] = vertices[2, 0] - 1 + vertices[2, 1] = vertices[2, 1] - 1 + if right_point_y > left_point_y: + vertices[1, 0] = vertices[1, 0] - 1 + vertices[1, 1] = vertices[1, 1] - 1 + vertices[3, 0] = vertices[3, 0] + 1 + vertices[3, 1] = vertices[3, 1] + 1 + if right_point_y < left_point_y: + vertices[1, 0] = vertices[1, 0] - 1 + vertices[1, 1] = vertices[1, 1] + 1 + vertices[3, 0] = vertices[3, 0] + 1 + vertices[3, 1] = vertices[3, 1] - 1 + return vertices + + # 添加图例 + def draw_legend(img, item, color, k): + obj_name = {'115': 'Dog basin', '116': 'Bar chair base', '117': 'Fan base', '118': 'Washing machine', + '119': 'Refrigerator', '120': 'Toilet', '121': 'Weighing scale', '150': 'Wire', '152': 'Desk', + '153': 'Carpet', '154': 'Rag', '155': 'Tea table', '156': 'TV cabinet', '200': 'Slippers', + '201': 'Sock', '202': 'Wardrobe', '203': 'Bed', '204': 'Sofa', '205': 'Chair'} + color = color[::-1] + text = obj_name[str(item)] + font = cv2.FONT_HERSHEY_DUPLEX + # cv2.rectangle(img, (output.shape[1] - 140, 10 + k * 17), (output.shape[1] - 120, 20 + k * 17), color, -1) + # cv2.putText(img, text, (output.shape[1] - 100, 20 + k * 17), font, 0.4, color, 1, cv2.LINE_AA) + cv2.rectangle(img, (10, 10 + k * 17), (30, 20 + k * 17), color, -1) + cv2.putText(img, text, (40, 20 + k * 17), font, 0.4, color, 1, cv2.LINE_AA) + return img + + # 语义物体的强度值 + # obj_value = [152, 121, 200, 115, 205, 117, 203] + # obj_value = [115, 116, 117, 118, 119, 120, 121, 150, 151, 152, 153, + # 154, 155, 156, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209] + # obj_value = [152, 117] + obj_value = [] + obj_legend = [] + # output是最后画上物体形状的总图 + output = Semantic_map + # output = np.zeros((map.shape[0], map.shape[1], 3), np.uint8) + # output[:, :, 0] = map + # output[:, :, 1] = map + # output[:, :, 2] = map + + # cv2.namedWindow("map", 0) + # cv2.imshow("map", map) + k = 0 + + # 对每个物体绘制形状 + for item in iter(obj_value): + if ((map == item).any()): + # min_samples, max_eps, shape, size, color = get_parameter(item) + min_samples, max_eps, shape, size, color = get_parameter(item, opt) + y, x = np.where(map == item) # y是行，x是列 + X = np.zeros((x.shape[0], 2)) + X[:, 0] = y + X[:, 1] = x + output = draw_outline(X, item, min_samples, max_eps, shape, color, size, output) + + if item not in obj_legend: + output = draw_legend(output, item, color, k) + k = k + 1 + + img1 = cv2.cvtColor(output, cv2.COLOR_BGR2GRAY) # 方便统计尺寸 + + return output + + +# 场景识别 +def get_labels(img): + def get_wall_area(map, stats, area): + ''' + . map 获取墙壁的图像 + . stats 连通域信息 + . area 面积 + ''' + temp = [] + for i in range(1, num_labels): + if stats[i, 4] > area: # 第i个区域的面积大于阈值，则为墙壁 + temp.append(i) + wall_area = np.zeros((map.shape[0], map.shape[1]), np.uint8) + for i in range(0, len(temp)): + mask = labels == temp[i] + wall_area[:, :][mask] = 255 + return wall_area + + # def draw_legend(img,scene,color,k): + # obj_name = {'0':'Dog basin','1':'Fan base','2':'Weighing scale','3':'Desk','4':'Slippers'} + # color = color[::-1] + # text = obj_name[str(scene)] + # font = cv2.FONT_HERSHEY_DUPLEX + # cv2.rectangle(img, (output.shape[1] - 140, 50+k*17), (output.shape[1] - 120, 60+k*17), color, -1) + # cv2.putText(img, text, (output.shape[1] - 100, 60+k*17), font, 0.4, color, 1, cv2.LINE_AA) + # return img + + def Scene(obj_label): + ''' + :param obj_label: + :return: index "bedroom","livingroom","bathroom","kitchen" ,"unknow" + ''' + print(obj_label) + Intensity_value = [115, 116, 117, 118, 119, 120, 121, 150, 151, 152, 153, + 154, 155, 156, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209] + p_sce = np.ones((1, 5)) * 0.25 + p_sce[0][4] = 0 + + p_obj_sce = np.array([[0, 0.2, 0, 0], + [0, 0, 0, 0], + [0.2, 0.4, 0, 0.01], + [0.01, 0.04, 0.8, 0], + [0.02, 0.2, 0, 0.8], + [0, 0, 0.99, 0], + [0.6, 0.4, 0.1, 0], + [0.6, 0.6, 0, 0.1], + [0.4, 0.4, 0, 0], + [0, 0.4, 0, 0], + [0, 0, 0, 0], + [0.1, 0.1, 0.2, 0.1], + [0.1, 0.7, 0, 0], + [0.4, 0.4, 0, 0], + [0.45, 0.34, 0.32, 0], + [0.27, 0.26, 0, 0], + [0.8, 0, 0, 0], + [0.99, 0, 0, 0], + [0.05, 0.9, 0, 0], + [0, 0, 0, 0], + [0, 0, 0, 0], + [0, 0, 0, 0], + [0, 0, 0, 0] + ]) + p_temp = 1.0 + p_obj = 0.3 # 目标检测出的置信度 + pre_scene = np.zeros((1, 5)) + if (len(obj_label) == 0): + pre_scene[0][4] = 1 + else: + for i in range(4): + for j in range(len(obj_label)): + k = obj_label[j] + sum_temp = np.sum(p_obj_sce[k]) + if (sum_temp == 0): + pre_scene[0][4] = 1 + continue + m = p_obj_sce[k, i] + p_temp = (1.0 - (m / sum_temp * p_obj)) * p_temp + pre_scene[0][i] = 1 - p_temp + p_temp = 1.0 + if np.max(pre_scene[0, 0:4]) > 0.1: + pre_scene[0][4] = 0 + sce_index = np.argmax(pre_scene) + print(pre_scene) + return sce_index + + Intensity_value = [115, 116, 117, 118, 119, 120, 121, 150, 151, 152, 153, + 154, 155, 156, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209] + '只保留墙壁空间' + temp = copy.copy(img) + temp[temp == 0] = 254 + temp[temp == 255] = 0 + temp[temp == 254] = 255 + temp[temp < 254] = 0 + Semantic_map = np.zeros((img.shape[0], img.shape[1], 3), np.uint8) + temp = temp.astype(np.uint8) + num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(temp, connectivity=8) + + wall_area = get_wall_area(img, stats, 400) + wall_area[wall_area == 0] = 254 + wall_area[wall_area == 255] = 0 + wall_area[wall_area == 254] = 255 + wall_area_color = np.zeros((wall_area.shape[0], wall_area.shape[1], 3), np.uint8) + wall_area_color[:, :, 0] = wall_area + wall_area_color[:, :, 1] = wall_area + wall_area_color[:, :, 2] = wall_area + '提取房间' + room_num_labels, room_labels, room_stats, room_centroids = cv2.connectedComponentsWithStats(wall_area, + connectivity=8) + + k = 0 + scene_legend = [] + + for i in range(2, room_num_labels): + if room_stats[i, 4] < 200: + continue + room = np.zeros((wall_area.shape[0], wall_area.shape[1])) + obj_label = [] + mask = room_labels == i + room[:, :][mask] = 1 + Semantic_area = room * img + # cv2.imshow('area', Semantic_area) + # cv2.waitKey(1000) + Semantic_area = list(Semantic_area.flatten()) + Semantic_area = list(set(Semantic_area)) + Semantic_area.sort() + for i in range(len(Semantic_area)): + if Semantic_area[i] in Intensity_value: + obj_label.append(Intensity_value.index(Semantic_area[i])) + # obj_label_test = [] # 用于测试 + scene = Scene(obj_label) + print(scene) + + # print(obj_label) + if scene == 0: + Semantic_map[:, :, 0][mask] = 70 + Semantic_map[:, :, 1][mask] = 80 + Semantic_map[:, :, 2][mask] = 90 + color = [70, 80, 90] + if scene == 1: + Semantic_map[:, :, 0][mask] = 100 + Semantic_map[:, :, 1][mask] = 180 + Semantic_map[:, :, 2][mask] = 120 + color = [100, 180, 120] + if scene == 2: + Semantic_map[:, :, 0][mask] = 210 + Semantic_map[:, :, 1][mask] = 67 + Semantic_map[:, :, 2][mask] = 170 + color = [210, 67, 170] + if scene == 3: + Semantic_map[:, :, 0][mask] = 150 + Semantic_map[:, :, 1][mask] = 48 + Semantic_map[:, :, 2][mask] = 88 + color = [150, 48, 88] + # if scene == 4: + # Semantic_map[:, :, 0][mask] = 134 + # Semantic_map[:, :, 1][mask] = 231 + # Semantic_map[:, :, 2][mask] = 143 + # color = [134, 231, 143] + # if scene not in scene_legend: + # scene_legend.append(scene) + # Semantic_map = draw_legend(Semantic_map, scene, color, k) + # k = k + 1 + Semantic_map = Semantic_map + wall_area_color + return Semantic_map + + +if __name__ == "__main__": + img = cv2.imread("map.png", 0) + cv2.flip(img, 0, img) # 图片翻转 + map = process_map(img, opt) + ''' + 对床、洗衣机和马桶自己赋语义 + ''' + # 床 + map[276:292, 212:224] = 203 + map[276:288, 282:296] = 203 + map[332:334, 208:210] = 255 + map[330:344, 210:220] = 203 + map[326:344, 282:294] = 203 + + # 洗衣机 + map[368:392, 438:448] = 118 + + # 马桶 + map[340:356, 428:448] = 120 + + # 袜子 + map[238:244, 531:533] = 201 + map[242:244, 529:533] = 201 + + # 电线 + map[259:263, 509:511] = 150 + map[261:263, 507:509] = 150 + + cv2.imwrite("mappp.jpg", map) + Semantic_map = get_labels(map) + output = obj_outline(map, Semantic_map, opt) + + cv2.imwrite('output.png', output) + cv2.namedWindow("output", 0) + cv2.imshow("output", output) + cv2.waitKey(0) diff --git a/后处理/物体形状/map.png b/后处理/物体形状/map.png new file mode 100644 index 0000000..51f1e96 Binary files /dev/null and b/后处理/物体形状/map.png differ diff --git a/后处理/物体形状/requirement.txt b/后处理/物体形状/requirement.txt new file mode 100644 index 0000000..e9ca66d --- /dev/null +++ b/后处理/物体形状/requirement.txt @@ -0,0 +1,5 @@ +numpy +opencv-python==3.4.2.16 +matplotlib +scipy +sklearn \ No newline at end of file diff --git a/编译命令.txt b/编译命令.txt new file mode 100644 index 0000000..6c013c2 --- /dev/null +++ b/编译命令.txt @@ -0,0 +1,8 @@ +//融合代码 +g++ -std=c++11 ./*.cpp -o main.out -I /usr/local/include -I /usr/local/include/opencv4 -I /usr/include -I /opt/ros/melodic/include -I /home/firefly/Desktop/object_detect_sdk_1804/install/include -L /usr/local/lib -l opencv_core -l opencv_imgproc -l opencv_imgcodecs -l opencv_highgui -l opencv_dnn -l opencv_videoio -l opencv_calib3d -L /usr/lib/obj_sdk -l rknn_sdk -l rknnrt -l roscpp -l roslib -l rosconsole -l roscpp_serialization -L /opt/ros/melodic/lib -l rostime -l boost_system -l pthread -l udev + +//编译cartographer +catkin_make_isolated --install --use-ninja -DFORCE_DEBUG_BUILD=True + +1. map.cpp 的编译命令 +g++ -g -std=c++11 ./map.cpp -o map.out -I /usr/lo \ No newline at end of file diff --git a/融合/.vs/ProjectSettings.json b/融合/.vs/ProjectSettings.json new file mode 100644 index 0000000..e257ff9 --- /dev/null +++ b/融合/.vs/ProjectSettings.json @@ -0,0 +1,3 @@ +{ + "CurrentProjectSetting": "无配置" +} \ No newline at end of file diff --git a/融合/.vs/VSWorkspaceState.json b/融合/.vs/VSWorkspaceState.json new file mode 100644 index 0000000..e85a99b --- /dev/null +++ b/融合/.vs/VSWorkspaceState.json @@ -0,0 +1,7 @@ +{ + "ExpandedNodes": [ + "" + ], + "SelectedNode": "\\fusion.h", + "PreviewInSolutionExplorer": false +} \ No newline at end of file diff --git a/融合/.vs/slnx.sqlite b/融合/.vs/slnx.sqlite new file mode 100644 index 0000000..05c2109 Binary files /dev/null and b/融合/.vs/slnx.sqlite differ diff --git a/融合/.vs/融合/v16/.suo b/融合/.vs/融合/v16/.suo new file mode 100644 index 0000000..c8ee1d0 Binary files /dev/null and b/融合/.vs/融合/v16/.suo differ diff --git a/融合/.vs/融合/v16/Browse.VC.db b/融合/.vs/融合/v16/Browse.VC.db new file mode 100644 index 0000000..7753261 Binary files /dev/null and b/融合/.vs/融合/v16/Browse.VC.db differ diff --git a/融合/LICENSE b/融合/LICENSE new file mode 100644 index 0000000..18b5522 --- /dev/null +++ b/融合/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2022 jzq + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH +REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY +AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, +INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM +LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR +OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR +PERFORMANCE OF THIS SOFTWARE. diff --git a/融合/README.en.md b/融合/README.en.md new file mode 100644 index 0000000..2da566c --- /dev/null +++ b/融合/README.en.md @@ -0,0 +1,36 @@ +# LCFusion + +#### Description +激光雷达信息和视觉信息融合 + +#### Software Architecture +Software architecture description + +#### Installation + +1. xxxx +2. xxxx +3. xxxx + +#### Instructions + +1. xxxx +2. xxxx +3. xxxx + +#### Contribution + +1. Fork the repository +2. Create Feat_xxx branch +3. Commit your code +4. Create Pull Request + + +#### Gitee Feature + +1. You can use Readme\_XXX.md to support different languages, such as Readme\_en.md, Readme\_zh.md +2. Gitee blog [blog.gitee.com](https://blog.gitee.com) +3. Explore open source project [https://gitee.com/explore](https://gitee.com/explore) +4. The most valuable open source project [GVP](https://gitee.com/gvp) +5. The manual of Gitee [https://gitee.com/help](https://gitee.com/help) +6. The most popular members [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/) diff --git a/融合/README.md b/融合/README.md new file mode 100644 index 0000000..38c6f3c --- /dev/null +++ b/融合/README.md @@ -0,0 +1,37 @@ +# LCFusion + +#### 介绍 +激光雷达信息和视觉信息融合 + +#### 软件架构 +软件架构说明 + + +#### 安装教程 + +1. xxxx +2. xxxx +3. xxxx + +#### 使用说明 + +1. xxxx +2. xxxx +3. xxxx + +#### 参与贡献 + +1. Fork 本仓库 +2. 新建 Feat_xxx 分支 +3. 提交代码 +4. 新建 Pull Request + + +#### 特技 + +1. 使用 Readme\_XXX.md 来支持不同的语言，例如 Readme\_en.md, Readme\_zh.md +2. Gitee 官方博客 [blog.gitee.com](https://blog.gitee.com) +3. 你可以 [https://gitee.com/explore](https://gitee.com/explore) 这个地址来了解 Gitee 上的优秀开源项目 +4. [GVP](https://gitee.com/gvp) 全称是 Gitee 最有价值开源项目，是综合评定出的优秀开源项目 +5. Gitee 官方提供的使用手册 [https://gitee.com/help](https://gitee.com/help) +6. Gitee 封面人物是一档用来展示 Gitee 会员风采的栏目 [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/) diff --git a/融合/fusion.cpp b/融合/fusion.cpp new file mode 100644 index 0000000..bedcf01 --- /dev/null +++ b/融合/fusion.cpp @@ -0,0 +1,681 @@ +#include +#include "fusion.h" +// 角度 -> 弧度转换 +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) +// fusion函数 +char LCFusion::fusion(std::vector &camInfo) +{ + projectedPoints.clear(); + if (camInfo.size() < 3) // 判断返回的双目测距信息是否为空，正常为3 + { + std::cout << "There is no detection box!" << std::endl; + return -1; + } + Mat boxes = camInfo[1], dwha = camInfo[2]; + projectedPoints.clear(); + + if (this->upScan.pointCloud.empty()) + { + std::cout << "There is no lidar data!" << std::endl; + return -2; + } + + if (boxes.empty()) + { + std::cout << "There is no detection box!" << std::endl; + return -1; + } + else + { + draw_point(camInfo); // 画出雷达映射到图像中的点 + std::cout << "Fusion start!" << std::endl; + // 大物体 + vector clusterlable = clusters_bboxs(boxes); // 得到每簇雷达映射的检测框标签（检测框索引值） + optimize_clusterlable(clusterlable, boxes); // 优化检测框（针对镂空物体） + big_fusion(clusterlable, boxes, dwha); // 大物体赋语义 + + //小物体 + small_box_without_dis(boxes, dwha); // 无距离信息的检测框映射 + small_fusion(boxes, dwha); // 小物体赋语义 + } + return 1; +} + +void LCFusion::big_fusion(vector &clusterlable, Mat &bboxs, Mat &dwha) // 大物体赋语义 +{ + std::cout << "big_fusion" << std::endl; + for (int i = 0; i < clusterlable.size(); i++) // 遍历每簇雷达 + { + int bbox_num = clusterlable[i]; + + if (bbox_num != -1) // -1代表无检测框与之对应 + { + int bbox_cls = bboxs.at(bbox_num, 5); + if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) // 排除扩散物体（风扇底座、吧台椅） + { + //std::cout << "bboxcls:" << bbox_cls << std::endl; + for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) // 遍历一簇雷达的所有点赋语义 + { + int laser_index = this->upScan.clustedCloud[i][j].at(0, 3); + this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1]; + } + } + else + { + if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10) // 扩散物体雷达点应该很少 + { + //cout << "******start draw circle******" << endl; + draw_circle(i, bbox_num, bboxs, dwha); // 扩散物体赋语义 + } + + } + } + } +} +void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) // 小物体u赋语义 +{ + std::cout << "small_fusion" << std::endl; + for (int i = 0; i < bboxs.rows; i++) + { + int c = bboxs.at(i, 5); + float x_mim = bboxs.at(i, 0); + float y_min = bboxs.at(i, 1); + float x_max = bboxs.at(i, 2); + float y_max = bboxs.at(i, 3); + std::cout << "cls:" << c << " xmin:" << x_mim << " ymin:" << y_min << " xmax:" << x_max << " ymax:" << y_max << std::endl; + if (class_index[c] == 0 || class_index[c] == 2 || (class_index[c] == 4 || class_index[c] == 5)) // 判断小物体u + { + + Mat temp(2, 1, CV_32F); + double d = dwha.at(i, 0); + double a = dwha.at(i, 3); // 取距离和角度 + if (d == -1) + continue; + //std::cout << "d:" << d << " a: " << a << std::endl; + temp.at(0, 0) = float((dwha.at(i, 0) / 100.0) * cos(dwha.at(i, 3))); + temp.at(1, 0) = float((dwha.at(i, 0) / 100.0) * sin(dwha.at(i, 3))); + temp = c2lR * temp + c2lT; // 相机坐标系-> 雷达坐标系转换 + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); // 转极坐标 + + float angle = 0.0; // 得到雷达点索引 + if (y >= 0) + angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + double data_angle = atan((dwha.at(i, 1)) / (200 * dis)); + data_angle /= this->upScan.angleInc; // 由测距宽度信息得到需更改的雷达点数的一半 + angle = (int)angle - (int)data_angle; // 起始雷达索引 + int num_point = 2 * (int)data_angle + 1; // 更改的雷达点数 + + int len = this->upScan.distance.size(); + for (int i = 0; i < num_point; i++) + { + if ((angle + i) < 0 || (angle + i) > (len - 1)) // 防止越界 + continue; + + this->upScan.distance[angle + i] = dis; + this->upScan.intensities[angle + i] = class_intensity[c + 1]; + } + } + else if (class_index[c] == 1 || class_index[c] == 3 ||class_index[c] == 6) + { + int len = this->upScan.distance.size(); + Mat z = r->Z.clone(); + if (countNonZero(z) < 1) + { + return; + } + if (z.cols >= 2) + { + int begin_index = 0; + for(int j = z.cols-1; j >=0; j-- ) + { + int d = z.at(0, j); + if (d != 0) + { + begin_index = j; + break; + } + } + cout << "tizhongcheng " << z.cols << endl; + float d_begin = z.at(0, begin_index)/1000.; + float a_begin = z.at(1, begin_index); + cout << "d_begin: " << d_begin << " a_begin: " << a_begin << endl; + Mat temp(2, 1, CV_32F); + temp.at(0, 0) = float(d_begin * cos(a_begin)); + temp.at(1, 0) = float(d_begin * sin(a_begin)); + temp = c2lR * temp + c2lT; + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); + float angle_index = 0.0; // 得到雷达点索引 + if (y >= 0) + angle_index = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle_index = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + angle_index = (int)angle_index; + if (angle_index >= 0 && angle_index upScan.distance[angle_index] = dis; + this->upScan.intensities[angle_index] = class_intensity[c + 1]; + } + cout << "dis: " << dis << " angle_index: " << angle_index << endl; + angle_index += 1; + + for (int i = begin_index-1; i >= 0; i--) + { + float temp_d = z.at(0, i)/1000.; + float temp_a = z.at(1, i); + temp.at(0, 0) = float(temp_d * cos(temp_a)); + temp.at(1, 0) = float(temp_d * sin(temp_a)); + temp = c2lR * temp + c2lT; + y = temp.at(0, 0); + x = temp.at(1, 0); + dis = sqrt(x * x + y * y); + float temp_angle = 0.0; // 得到雷达点索引 + if (y >= 0) + temp_angle = acos(x / dis); + else + temp_angle =2 * M_PI - acos(x / dis); + float angle_now = this->upScan.angleMin + this->upScan.angleInc*angle_index; + if (temp_angle >= angle_now) + { + if (angle_index >= 0 && angle_index < len) // 防止越界 + { + this->upScan.distance[angle_index] = dis; + // cout << "dis: " << dis << " angle_index: " << angle_index << endl; + this->upScan.intensities[angle_index] = class_intensity[c + 1]; + } + angle_index += 1; + } + } + r->Z = Mat::zeros(2, 1, CV_32FC1); + } + } + } +} + +void LCFusion::optimize_clusterlable(vector &clusterlable, Mat &bboxs) // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇 +{ + for (int i = 0; i < bboxs.rows; i++) // 寻找每个框对应的所有雷达簇 + { + vector temp_cluster_num; + Mat temp_bbox = bboxs.row(i).clone(); + for (int j = 0; j < clusterlable.size(); j++) + { + if (clusterlable[j] == i) + { + temp_cluster_num.push_back(j); + } + } + if (temp_cluster_num.size() == 0) + continue; + choose_forceground(temp_cluster_num, temp_bbox, clusterlable); // 筛选镂空物体的前景 + } +} + +void LCFusion::choose_forceground(vector &cluster_num, Mat &bbox, vector &clusterlable) // 筛选镂空物体的前景 +{ + int box_class = bbox.at(0, 5); + if ((class_index[box_class] >= 15 && class_index[box_class] <= 18 || class_index[box_class] == 12)&& cluster_num.size() > 1) + { + cout << "class " << box_class << " have " << cluster_num.size() << " clusters" << endl; + int total_point = 0; + for (int i = 0; i < cluster_num.size(); i++) // 计算框内雷达点总数 + { + total_point += (this->upScan.clustedCloud[cluster_num[i]].size()); + } + for (int j = 0; j < cluster_num.size(); j++) // 计算每个雷达簇占比，大于0.2认为是背景 + { + float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size(); + if (num_cluster / total_point > 0.2) + { + clusterlable[cluster_num[j]] = -1; + } + } + } +} + +void LCFusion::draw_point(std::vector &camInfo) // 画出映射到图像的雷达点 +{ + projectedPoints.clear(); + for (int i = 0; i < this->upScan.pointCloud.rows; i++) + { + Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone(); + Mat uv = pointcloud_pixel(temp_pointcloud); + int x = uv.at(0, 0); + int y = uv.at(1, 0); + if (x > 0 && x < 640 && y > 0 && y < 480) + { + Point2d pt_uv(x, y); + projectedPoints.push_back(pt_uv); + } + } + +} + +// void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) +// { +// upScan.distance.clear(); +// upScan.intensities.clear(); +// float pointAngle; +// upScan.angleInc = angleInc; + +// for (int i = 0; i < laserData.len; i++) +// { +// pointAngle = angle_min + i * upScan.angleInc; +// if (pointAngle > cameraAnglemin) +// { +// if (pointAngle > cameraAnglemax) +// { +// upScan.angleMax = pointAngle - upScan.angleInc; +// break; +// } +// upScan.distance.push_back(laserData.distance[i] / 1000.f); +// upScan.intensities.push_back(0); +// if (i == (laserData.len - 1)) +// upScan.angleMax = pointAngle; +// // upScan.intensities.push_back(laserData.intensities[i]); +// } +// } +// upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc; +// this->scan_to_pointcloud(); +// this->cluster(); +// } +void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) // 雷达点和相机视野同步 +{ + upScan.distance.clear(); + upScan.intensities.clear(); + upScan.angleMin = angle_min; + upScan.angleInc = angleInc; + upScan.angleMax = ANGLE_TO_RADIAN(laserData.angle_max); + float pointAngle; + for (int i = 0; i < laserData.len; i++) + { + upScan.distance.push_back(laserData.distance[i] / 1000.f); + pointAngle = angle_min + i * upScan.angleInc; + if (pointAngle > cameraAnglemin && pointAngle < cameraAnglemax) + upScan.intensities.push_back(1); // 视野范围内强度赋1 + else + upScan.intensities.push_back(0); // 视野范围外强度赋0 + } + + this->scan_to_pointcloud(); // 雷达极坐标到二维坐标转换 + this->cluster(); // 雷达聚类 +} + +void LCFusion::scan_to_pointcloud() // 雷达极坐标到二维坐标转换 +{ + u_int32_t len = upScan.distance.size(); + Mat pointcloud(len, 4, CV_32F); + + for (int i = 0; i < len; i++) + { + float dis = this->upScan.distance[i]; + float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis; + float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis; + pointcloud.at(i, 0) = x_temp; + pointcloud.at(i, 1) = y_temp; + pointcloud.at(i, 2) = 0; + pointcloud.at(i, 3) = i; + } + this->upScan.pointCloud = pointcloud.clone(); +} + +uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) // 无用 +{ + int x = uv.at(0, 0); + int y = uv.at(1, 0); + // 实验测试，过滤类别 + std::set filterCls = {0, 7, 11, 14, 15, 21}; + for (uchar i = 0; i < boxes.rows; i++) + { + int clsIdx = boxes.at(i, 5); + if (filterCls.find(clsIdx) != filterCls.end()) + continue; + + float xmin = boxes.at(i, 0); + float xmax = boxes.at(i, 2); + float ymin = boxes.at(i, 1); + float ymax = boxes.at(i, 3); + + // 实验测试，过滤过大的误检框 + float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + if (ratio > 0.7) + { + boxflag[i] += 1; + continue; + } + + // 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况 + if (x >= xmin && x <= xmax && y >= ymin && y <= ymax) + { + boxflag[i] += 1; + return class_intensity[clsIdx + 1]; + } + } + return 0; +} + +std::vector LCFusion::clusters_bboxs(Mat &bboxs) // 所有的聚类的雷达簇和所有的检测框的匹配 +{ + std::vector clusterlable; + for (int i = 0; i < this->upScan.clustedCloud.size(); i++) // 遍历每簇雷达 + { + std::vector temp_cluster = this->upScan.clustedCloud[i]; + int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); // 为一簇雷达寻找匹配的检测框 + clusterlable.push_back(temp_clusters_bbox); + } + return clusterlable; +} + +int LCFusion::cluster_bboxs(vector &one_cluster, Mat &bboxs) // 为一簇雷达寻找匹配的检测框 +{ + vector temp_result; + Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]); + Mat end = pointcloud_pixel(one_cluster[0]); // 一簇雷达起始点和终止点在像素坐标系的坐标 + for (int i = 0; i < bboxs.rows; i++) + { + Mat bbox = bboxs.row(i).clone(); + int cls = bbox.at(0, 5); + float ratio = ratio_in_1box(begin, end, bbox); // 在框中占比 + if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) // 适用于大物体 + temp_result.push_back(i); + } + if (temp_result.size() < 1) // 若一簇雷达落入多个框里，选择宽度较小的框 + return -1; + else if (temp_result.size() < 2) + return temp_result[0]; + else + { + int min = temp_result[0]; + int width_min = bboxs.at(min, 2) - bboxs.at(min, 0); + for (int i = 0; i < temp_result.size(); i++) + { + int width_new = bboxs.at(temp_result[i], 2) - bboxs.at(temp_result[i], 0); + if (width_new < width_min) + { + min = temp_result[i]; + width_min = width_new; + } + } + return min; + } + return -1; +} + +Mat LCFusion::pointcloud_pixel(Mat &pointcloud) // 二维雷达点到像素坐标系转换 +{ + Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3)); + Mat camPoint = rotate * cutPointCloud.t() + translation; + // float ppp = camPoint.at(2, 0); + if (camPoint.at(2, 0) <= 0) + { + uv = (Mat_(3, 1) << -1, -1, -1); + return uv; + } + + float scaleX = camPoint.at(0, 0) / camPoint.at(2, 0); + float scaleY = camPoint.at(1, 0) / camPoint.at(2, 0); + float scaleD = scaleX * scaleX + scaleY * scaleY; + float tempD = 1 + distCoeff.at(0, 0) * scaleD + distCoeff.at(0, 1) * scaleD * scaleD + distCoeff.at(0, 4) * scaleD * scaleD * scaleD; + + camPoint.at(0, 0) = scaleX * tempD + 2 * distCoeff.at(0, 2) * scaleX * scaleY + + distCoeff.at(0, 3) * (scaleD + 2 * scaleX * scaleX); + + camPoint.at(1, 0) = scaleY * tempD + distCoeff.at(0, 2) * (scaleD + 2 * scaleY * scaleY) + + 2 * distCoeff.at(0, 3) * scaleX * scaleY; + + camPoint.at(2, 0) = 1.0; + uv = cameraMat * camPoint; // uv为像素坐标 + uv = p2r * uv + t; // 矫正 + return uv; +} + +float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) // 计算一簇雷达落入检测框的比例 +{ + int x_begin = begin.at(0, 0), x_end = end.at(0, 0), y_begin = begin.at(1, 0), y_end = end.at(1, 0); + int xmin = box.at(0, 0), ymin = box.at(0, 1), xmax = box.at(0, 2), ymax = box.at(0, 3); + if (y_begin < ymin || y_begin > ymax) + return 0; + if (x_begin < xmin) + { + if (x_end < xmin) + return 0; + else if (x_end < xmax) + return (float)(x_end - xmin) / (x_end - x_begin); + else + return (float)(xmax - xmin) / (x_end - x_begin); + } + else if (x_begin < xmax) + { + if (x_end < xmax) + return 1; + else + return (float)(xmax - x_begin) / (x_end - x_begin); + } + else + return 0; +} + +void LCFusion::cluster() // 聚类 +{ + this->upScan.clustedCloud.clear(); + int i = 0; + + for (; i < this->upScan.distance.size();) + { + std::vector temp; + int j = i; + temp.push_back(this->upScan.pointCloud.row(i)); + for (; j < this->upScan.distance.size() - 1; j++) + { + + // 距离的6%作为分类的阈值 + if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 ) + { + temp.push_back(this->upScan.pointCloud.row(j + 1)); + } + else + { + i = j + 1; + break; + } + } + this->upScan.clustedCloud.push_back(temp); + if (j == this->upScan.distance.size() - 1) + break; + if (i == upScan.distance.size() - 1) + { + temp.push_back(upScan.pointCloud.row(i)); + this->upScan.clustedCloud.push_back(temp); + break; + } + } + + this->clusterIdx.clear(); + int ii = 0, totoalNum = 0; + for (auto c : this->upScan.clustedCloud) + { + for (int jj = 0; jj < c.size(); jj++) + { + this->clusterIdx.push_back(ii); + } + totoalNum += c.size(); + ii++; + } + //std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl; +} +void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) // 扩散物体生成圆弧状伪激光雷达 +{ + vector center_point = find_circle_center(cluster_num); + float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at(bbox_num, 1)/200.f, + d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc); + int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at(bbox_num, 5); + float laser_index = center_point[1] - half_data_num; + //cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl; + for (int i = 0; i < num_laser_circle; i++) + { + float theta = upScan.angleMin + laser_index * upScan.angleInc; + if (laser_index < 0 || laser_index > upScan.distance.size()) + { + laser_index++; + continue; + } + float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2); + if (data >= 0) + { + //cout << "draw_one_point" << endl; + upScan.distance[laser_index] = circle(d, alfa ,theta, data); + upScan.intensities[laser_index] = class_intensity[cls + 1]; + } + laser_index++; + } +} +vector LCFusion::find_circle_center(int cluster_num) // 寻找扩散物体圆弧的圆心 +{ + vector center_point(2); + int index = 0; + float total_dis = 0; + int num = upScan.clustedCloud[cluster_num].size(); + int no_zero_dis = 0; // 非零距离和 + for (int i = 0 ; i < num; i++) + { + index = upScan.clustedCloud[cluster_num][i].at(0, 3); + if (upScan.distance[index] != 0) + no_zero_dis++; + total_dis += upScan.distance[index]; + } + center_point[0] = total_dis/ no_zero_dis; + index -= (num/2); + float center_laser_index = index; + center_point[1] = center_laser_index; + return center_point; +} +float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度 +{ + float ro = d * cos(theta - alfa) - sqrt(data); + return ro > 0 ? ro:0; +} +vector LCFusion::width_ladar(const int* coordinates) +{ + int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3]; + + std::vector mid = r-> pic2cam(640 / 2, 480); //�ҵ��������� + std::vector loc_tar_start = r-> pic2cam(x1, 480); // �ҵ�Ŀ�����ʼλ�� + std::vector loc_tar_end = r-> pic2cam(x2, 480); //�ҵ�Ŀ�����ֹλ�� + //ת���ɻ���ֵ + float alfa_start = atan((loc_tar_start[0] - mid[0]) / r-> q.at(2, 3)); + float alfa_end = atan((loc_tar_end[0] - mid[0]) / r-> q.at(2, 3)); + std::vector ladar_range; + ladar_range.push_back(alfa_start); + ladar_range.push_back(alfa_end); + return ladar_range; +} +void LCFusion::small_box_without_dis(Mat & bboxs, Mat & dwha) // 无距离检测框到像素坐标系映射，过程和small_fusion类似 +{ + std::cout << "small_box_without_dis" << std::endl; + double set_dis = 2.0; // 设置距离为2米 + for (int i = 0; i < bboxs.rows; i++) + { + int c = bboxs.at(i, 5); + int x_mim = bboxs.at(i, 0); + int y_min = bboxs.at(i, 1); + int x_max = bboxs.at(i, 2); + int y_max = bboxs.at(i, 3); + double d = dwha.at(i, 0); + int coordinates[4] = {x_mim, y_min, x_max, y_max}; + if ((class_index[c] >= 0 && class_index[c] <= 1) || (class_index[c] >= 3 && class_index[c] <= 6)) + { + + Mat temp(2, 1, CV_32F); + + + if (d == -1) + { + vector box_angle = width_ladar(coordinates); + //std::cout << "d:" << d << " a: " << a << std::endl; + temp.at(0, 0) = float(set_dis * cos(box_angle[0])); + temp.at(1, 0) = float(set_dis * sin(box_angle[0])); + temp = c2lR * temp + c2lT; + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); + float angle_end = 0.0; + if (y >= 0) + angle_end = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle_end = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + angle_end = int(angle_end); + // std:: cout << "angle end: " << angle_end << std::endl; + temp.at(0, 0) = float(set_dis * cos(box_angle[1])); + temp.at(1, 0) = float(set_dis * sin(box_angle[1])); + temp = c2lR * temp + c2lT; + y = temp.at(0, 0); + x = temp.at(1, 0); + dis = sqrt(x * x + y * y); + float angle_start = 0.0; + if (y >= 0) + angle_start = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle_start = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + angle_start = int(angle_start); + // std:: cout << "angle start: " << angle_start << std::endl; + if (angle_end-angle_start <= 0) + continue; + int num_point = angle_end-angle_start+1; + for (int i = 0; i < num_point; i++) + { + if ((angle_start + i) < 0 || (angle_start + i) > (upScan.distance.size() - 1)) + continue; + this->upScan.intensities[angle_start + i] = 0; + } + } + } + } +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) // 设置雷达发布数据（原始雷达） +{ + float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); + float angle_max = ANGLE_TO_RADIAN(laserData.angle_max); + uint32_t len = laserData.len; + float angle_inc = (angle_max - angle_min) / (len - 1.); + scan.header.frame_id = "down_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_min = angle_min; + scan.angle_max = angle_max; + scan.angle_increment = angle_inc; + scan.ranges.resize(len); + scan.intensities.resize(len); + scan.time_increment = 1. / 2400.; + for (int i = 0; i < len; i++) + { + scan.ranges[i] = laserData.distance[i] / 1000.f; + scan.intensities[i] = 0; + } +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) // 设置雷达发布数据（语义雷达） +{ + scan.header.frame_id = "up_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_increment = upScan.angleInc; + scan.ranges.resize(upScan.distance.size()); + scan.intensities.resize(upScan.distance.size()); + scan.angle_min = upScan.angleMin; + scan.angle_max = upScan.angleMax; + for (int i = 0; i < upScan.distance.size(); i++) + { + scan.ranges[i] = upScan.distance[i]; + scan.intensities[i] = upScan.intensities[i]; + } + scan.time_increment = 1. / 2400.; +} + + diff --git a/融合/fusion.h b/融合/fusion.h new file mode 100644 index 0000000..360e97e --- /dev/null +++ b/融合/fusion.h @@ -0,0 +1,98 @@ +#pragma once +#include +#include +#include +#include +#include "lidar.h" +#include "ranging.h" + +using namespace cv; +using namespace std; + +struct ImgScan +{ + float angleMin; //弧度 + float angleMax; + float angleInc; + std::vector distance; // 米 + std::vector intensities; + Mat pointCloud; + std::vector> clustedCloud; +}; +class Ranging; //友元类 +class LCFusion +{ +public: + LCFusion(Ranging* ranging) + { + r = ranging; + rotate = cameraExtrinsicMat.rowRange(0, 3).colRange(0, 3).t(); // 雷达 -> 相机坐标系旋转矩阵 + translation = cameraExtrinsicMat.col(3).rowRange(0, 3); + translation = -rotate * translation; // 雷达 -> 相机坐标系平移矩阵 + p2r = r_y * r_c; // 像素坐标系矫正矩阵（旋转） + }; + char fusion(std::vector &camInfo); // fusion函数 + uchar lidar2box(Mat uv, Mat boxes, uchar *boxFlag); // 无用 + void filter_laser(const FrameData &lidarInfo, float angle_min, float angle_inc); // 雷达点和相机视野同步 + void scan_to_pointcloud(); // 雷达极坐标到二维坐标转换 + std::vector clusters_bboxs(Mat &bboxs); // 所有的聚类的雷达簇和所有的检测框的匹配 + int cluster_bboxs(std::vector &one_cluster, Mat &bboxs); // 寻找一簇雷达簇匹配的检测框 + void optimize_clusterlable(vector & clusterlable, Mat & bboxs); // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇 + void choose_forceground(vector & cluster_num, Mat & bbox, vector & clusterlable); // 针对镂空物体选择对应的雷达簇，筛选过程 + void big_fusion(vector & clusterlable, Mat & bboxs, Mat &dwha); // 大物体赋语义 + void small_fusion(Mat & bboxs, Mat & dwha); // 小物体赋语义 + Mat pointcloud_pixel(Mat &pointcloud); // 二维雷达点到像素坐标系转换 + void cluster(); // 聚类 + float ratio_in_1box(Mat &begin, Mat &end, Mat &box); // 计算一簇雷达落入检测框的比例 + void draw_point(std::vector &camInfo); // 画出雷达在图像中的映射点 + void draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha); // 扩散物体生成圆弧状伪激光雷达 + vector find_circle_center(int cluster_num); // 寻找扩散物体圆弧的圆心 + float circle(float d, float alfa ,float theta, float data); // 圆弧角度和距离的数学关系 + vector width_ladar(const int* coordinates); + void small_box_without_dis(Mat & bboxs, Mat & dwha); // 检测框到雷达坐标系的映射关系 + void set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData); // 设置雷达发布数据（原始雷达） + void set_laser_data(sensor_msgs::LaserScan& scan); // 设置雷达发布数据（语义雷达） + + ImgScan upScan; + // 调试使用，将雷达点映射到图像中显示 + vector projectedPoints; // 雷达 -> 像素坐标系映射的点 + std::vector clusterIdx; + +private: + Ranging* r; + //float cameraAnglemin = 1.117, cameraAnglemax = 2.0071; + float cameraAnglemin = 1.345, cameraAnglemax = 2.0595; // 相机视野角度 + Mat rotate, translation, p2r; // 雷达点映射矫正 + Mat cameraExtrinsicMat = (Mat_(4, 4) << 9.9710325100937192e-01, 5.9677185357037893e-02, + -4.7156551765403301e-02, 2.6591864512557874e-02, + 4.7136795335845721e-02, 1.7395474156941537e-03, + 9.9888692878636431e-01, 7.4034983851991365e-02, + 5.9692791457662736e-02, -9.9821621281296091e-01, + -1.0784828889205400e-03, -9.6176066499866750e-02, 0., 0., 0., 1.); + + Mat cameraMat = (Mat_(3, 3) << 5.0280958247840368e+02, 0., 3.1925413622163182e+02, 0., + 5.0206199606708202e+02, 2.1999125419411467e+02, 0., 0., 1.); + + Mat distCoeff = (Mat_(1, 5) << 1.2903666948900971e-01, -9.2734018203156590e-02, + -7.0528508350750380e-03, 8.3183285124261049e-04, + 7.2727934388070986e-02); + + // Mat c2lR = (Mat_(2, 2) << 0.99664806, 0.0802792, -0.0802792, 0.99664806); + Mat c2lR = (Mat_(2, 2) << 0.98163, 0.19081, -0.19081, 0.98163); //11 + //Mat c2lR = (Mat_(2, 2) << 0.9703, 0.24192, -0.24192, 0.9703); //13 + Mat c2lT = (Mat_(2, 1) << 0.15, 0.01); + //Mat c2lT = (Mat_(2, 1) << 0.13693697, 0.05094143); + Mat r_c = (Mat_(3, 3) << 9.977e-1, 6.7744e-02, 0., -6.7744e-02, 9.977e-1, 0., 0., 0., 1.); + Mat t = (Mat_(3, 1) << 50., 0., 0.); + Mat r_y = (Mat_(3, 3) << 1., 0., 0., 0., 1.19444e+00, 0., 0., 0., 1.); + /*int class_intensity[24] = { + 0, 120, 130, 130, 130, 130, 130, 130, 146, 146, + 146, 162, 162, 178, 178, 196, 196, 212, 212, 228, + 228, 0, 0, 0}; */ + int class_intensity[25] = { + 0, 115, 116, 117, 118, 119, 120, 121, 150, 151, + 152, 153, 154, 155, 156, 200, 201, 202, 203, 204, + 205, 206, 207, 208, 209}; // 物体类别和强度映射关系 + + map class_index = {{-1, 23}, {0, 0}, {1, 13}, {2, 14}, {3, 7}, {4, 8}, {5, 9}, {6, 1}, {7, 2}, {8, 19}, {9, 18}, {10, 6}, {11, 3}, {12, 15}, {13, 10}, {14, 4}, {15, 5}, {16, 11}, {17, 16}, {18, 12}, {19, 17}, {20, 20}, {21, 21}, {22, 22}}; +}; // 类别映射，方便大小物体判断 diff --git a/融合/fusion.out b/融合/fusion.out new file mode 100644 index 0000000..94f787e Binary files /dev/null and b/融合/fusion.out differ diff --git a/融合/fusion.txt b/融合/fusion.txt new file mode 100644 index 0000000..b097b1a --- /dev/null +++ b/融合/fusion.txt @@ -0,0 +1,491 @@ +#include +#include "fusion.h" +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) +char LCFusion::fusion(std::vector &camInfo) +{ + Mat boxes = camInfo[1], dwha = camInfo[2]; + projectedPoints.clear(); + + if (this->upScan.pointCloud.empty()) + { + std::cout << "There is no lidar data!" << std::endl; + return -2; + } + + if (boxes.empty()) + { + std::cout << "There is no detection box!" << std::endl; + return -1; + } + else + { + draw_point(); + std::cout << "Fusion start!" << std::endl; + // 大物体 + vector clusterlable = clusters_bboxs(boxes); + optimize_clusterlable(clusterlable, boxes); + big_fusion(clusterlable, boxes, dwha); + + //小物体 + small_fusion(boxes, dwha); + } + return 1; +} + +void LCFusion::big_fusion(vector &clusterlable, Mat &bboxs, Mat &dwha) +{ + std::cout << "big_fusion" << std::endl; + for (int i = 0; i < clusterlable.size(); i++) + { + int bbox_num = clusterlable[i]; + + if (bbox_num != -1) + { + int bbox_cls = bboxs.at(bbox_num, 5); + if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) + { + //std::cout << "bboxcls:" << bbox_cls << std::endl; + for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) + { + int laser_index = this->upScan.clustedCloud[i][j].at(0, 3); + this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1]; + } + } + else + { + if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10) + { + cout << "******start draw circle******" << endl; + draw_circle(i, bbox_num, bboxs, dwha); + } + + } + } + } +} +void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) +{ + std::cout << "small_fusion" << std::endl; + for (int i = 0; i < bboxs.rows; i++) + { + int c = bboxs.at(i, 5); + std::cout << "cls:" << c << std::endl; + if (class_index[c] >= 0 && class_index[c] <= 6) + { + + Mat temp(2, 1, CV_32F); + double d = dwha.at(i, 0); + double a = dwha.at(i, 3); + if (d == -1) + continue; + //std::cout << "d:" << d << " a: " << a << std::endl; + temp.at(0, 0) = float((dwha.at(i, 0) / 100.0) * cos(dwha.at(i, 3))); + temp.at(1, 0) = float((dwha.at(i, 0) / 100.0) * sin(dwha.at(i, 3))); + temp = c2lR * temp + c2lT; + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); + + float angle = 0.0; + if (y >= 0) + angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + double data_angle = atan((dwha.at(i, 1)) / (200 * dis)); + data_angle /= this->upScan.angleInc; + angle = (int)angle - (int)data_angle; + int num_point = 2 * (int)data_angle + 1; + + int len = this->upScan.distance.size(); + for (int i = 0; i < num_point; i++) + { + if ((angle + i) < 0 || (angle + i) > (len - 1)) + continue; + + this->upScan.distance[angle + i] = dis; + this->upScan.intensities[angle + i] = class_intensity[c + 1]; + } + } + } +} + +void LCFusion::optimize_clusterlable(vector &clusterlable, Mat &bboxs) +{ + for (int i = 0; i < bboxs.rows; i++) + { + vector temp_cluster_num; + Mat temp_bbox = bboxs.row(i).clone(); + for (int j = 0; j < clusterlable.size(); j++) + { + if (clusterlable[j] == i) + { + temp_cluster_num.push_back(j); + } + } + if (temp_cluster_num.size() == 0) + continue; + choose_forceground(temp_cluster_num, temp_bbox, clusterlable); + } +} + +void LCFusion::choose_forceground(vector &cluster_num, Mat &bbox, vector &clusterlable) +{ + int box_class = bbox.at(0, 5); + if (class_index[box_class] >= 15 && class_index[box_class] <= 18 && cluster_num.size() > 3) + { + int total_point = 0; + for (int i = 0; i < cluster_num.size(); i++) + { + total_point += (this->upScan.clustedCloud[cluster_num[i]].size()); + } + for (int j = 0; j < cluster_num.size(); j++) + { + float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size(); + if (num_cluster / total_point > 0.2) + { + clusterlable[cluster_num[j]] = -1; + } + } + } +} + +void LCFusion::draw_point() +{ + projectedPoints.clear(); + for (int i = 0; i < this->upScan.pointCloud.rows; i++) + { + Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone(); + Mat uv = pointcloud_pixel(temp_pointcloud); + int x = uv.at(0, 0); + int y = uv.at(1, 0); + if (x > 0 && x < 640 && y > 0 && y < 480) + { + Point2d pt_uv(x, y); + projectedPoints.push_back(pt_uv); + } + } +} + +void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) +{ + upScan.distance.clear(); + upScan.intensities.clear(); + float pointAngle; + upScan.angleInc = angleInc; + + for (int i = 0; i < laserData.len; i++) + { + pointAngle = angle_min + i * upScan.angleInc; + if (pointAngle > cameraAnglemin) + { + if (pointAngle > cameraAnglemax) + { + upScan.angleMax = pointAngle - upScan.angleInc; + break; + } + upScan.distance.push_back(laserData.distance[i] / 1000.f); + upScan.intensities.push_back(0); + if (i == (laserData.len - 1)) + upScan.angleMax = pointAngle; + // upScan.intensities.push_back(laserData.intensities[i]); + } + } + upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc; + this->scan_to_pointcloud(); + this->cluster(); +} + +void LCFusion::scan_to_pointcloud() +{ + u_int32_t len = upScan.distance.size(); + Mat pointcloud(len, 4, CV_32F); + + for (int i = 0; i < len; i++) + { + float dis = this->upScan.distance[i]; + float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis; + float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis; + pointcloud.at(i, 0) = x_temp; + pointcloud.at(i, 1) = y_temp; + pointcloud.at(i, 2) = 0.0; + pointcloud.at(i, 3) = i; + } + this->upScan.pointCloud = pointcloud.clone(); +} + +uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) +{ + int x = uv.at(0, 0); + int y = uv.at(1, 0); + // 实验测试，过滤类别 + std::set filterCls = {0, 7, 11, 14, 15, 21}; + for (uchar i = 0; i < boxes.rows; i++) + { + int clsIdx = boxes.at(i, 5); + if (filterCls.find(clsIdx) != filterCls.end()) + continue; + + float xmin = boxes.at(i, 0); + float xmax = boxes.at(i, 2); + float ymin = boxes.at(i, 1); + float ymax = boxes.at(i, 3); + + // 实验测试，过滤过大的误检框 + float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + if (ratio > 0.7) + { + boxflag[i] += 1; + continue; + } + + // 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况 + if (x >= xmin && x <= xmax && y >= ymin && y <= ymax) + { + boxflag[i] += 1; + return class_intensity[clsIdx + 1]; + } + } + return 0; +} + +std::vector LCFusion::clusters_bboxs(Mat &bboxs) +{ + std::vector clusterlable; + for (int i = 0; i < this->upScan.clustedCloud.size(); i++) + { + std::vector temp_cluster = this->upScan.clustedCloud[i]; + int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); + clusterlable.push_back(temp_clusters_bbox); + } + return clusterlable; +} + +int LCFusion::cluster_bboxs(vector &one_cluster, Mat &bboxs) +{ + vector temp_result; + Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]); + Mat end = pointcloud_pixel(one_cluster[0]); + for (int i = 0; i < bboxs.rows; i++) + { + Mat bbox = bboxs.row(i).clone(); + int cls = bbox.at(0, 5); + float ratio = ratio_in_1box(begin, end, bbox); + if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) + temp_result.push_back(i); + } + if (temp_result.size() < 1) + return -1; + else if (temp_result.size() < 2) + return temp_result[0]; + else + { + int min = temp_result[0]; + int width_min = bboxs.at(min, 2) - bboxs.at(min, 0); + for (int i = 0; i < temp_result.size(); i++) + { + int width_new = bboxs.at(temp_result[i], 2) - bboxs.at(temp_result[i], 0); + if (width_new < width_min) + { + min = temp_result[i]; + width_min = width_new; + } + } + return min; + } + return -1; +} + +Mat LCFusion::pointcloud_pixel(Mat &pointcloud) +{ + Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3)); + Mat camPoint = rotate * cutPointCloud.t() + translation; + // float ppp = camPoint.at(2, 0); + if (camPoint.at(2, 0) <= 0) + { + uv = (Mat_(3, 1) << -1, -1, -1); + return uv; + } + + float scaleX = camPoint.at(0, 0) / camPoint.at(2, 0); + float scaleY = camPoint.at(1, 0) / camPoint.at(2, 0); + float scaleD = scaleX * scaleX + scaleY * scaleY; + float tempD = 1 + distCoeff.at(0, 0) * scaleD + distCoeff.at(0, 1) * scaleD * scaleD + distCoeff.at(0, 4) * scaleD * scaleD * scaleD; + + camPoint.at(0, 0) = scaleX * tempD + 2 * distCoeff.at(0, 2) * scaleX * scaleY + + distCoeff.at(0, 3) * (scaleD + 2 * scaleX * scaleX); + + camPoint.at(1, 0) = scaleY * tempD + distCoeff.at(0, 2) * (scaleD + 2 * scaleY * scaleY) + + 2 * distCoeff.at(0, 3) * scaleX * scaleY; + + camPoint.at(2, 0) = 1.0; + uv = cameraMat * camPoint; + uv = p2r * uv + t; + return uv; +} + +float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) +{ + int x_begin = begin.at(0, 0), x_end = end.at(0, 0), y_begin = begin.at(1, 0), y_end = end.at(1, 0); + int xmin = box.at(0, 0), ymin = box.at(0, 1), xmax = box.at(0, 2), ymax = box.at(0, 3); + if (y_begin < ymin || y_begin > ymax) + return 0; + if (x_begin < xmin) + { + if (x_end < xmin) + return 0; + else if (x_end < xmax) + return (float)(x_end - xmin) / (x_end - x_begin); + else + return (float)(xmax - xmin) / (x_end - x_begin); + } + else if (x_begin < xmax) + { + if (x_end < xmax) + return 1; + else + return (float)(xmax - x_begin) / (x_end - x_begin); + } + else + return 0; +} + +void LCFusion::cluster() +{ + this->upScan.clustedCloud.clear(); + int i = 0; + + for (; i < this->upScan.distance.size();) + { + std::vector temp; + int j = i; + temp.push_back(this->upScan.pointCloud.row(i)); + for (; j < this->upScan.distance.size() - 1; j++) + { + + // 距离的6%作为分类的阈值 + if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 ) + { + temp.push_back(this->upScan.pointCloud.row(j + 1)); + } + else + { + i = j + 1; + break; + } + } + this->upScan.clustedCloud.push_back(temp); + if (j == this->upScan.distance.size() - 1) + break; + if (i == upScan.distance.size() - 1) + { + temp.push_back(upScan.pointCloud.row(i)); + this->upScan.clustedCloud.push_back(temp); + break; + } + } + + this->clusterIdx.clear(); + int ii = 0, totoalNum = 0; + for (auto c : this->upScan.clustedCloud) + { + for (int jj = 0; jj < c.size(); jj++) + { + this->clusterIdx.push_back(ii); + } + totoalNum += c.size(); + ii++; + } + //std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl; +} +void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) +{ + vector center_point = find_circle_center(cluster_num); + float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at(bbox_num, 1)/200.f, + d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc); + int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at(bbox_num, 5); + float laser_index = center_point[1] - half_data_num; + //cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl; + for (int i = 0; i < num_laser_circle; i++) + { + float theta = upScan.angleMin + laser_index * upScan.angleInc; + if (laser_index < 0 || laser_index > upScan.distance.size()) + { + laser_index++; + continue; + } + float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2); + if (data >= 0) + { + //cout << "draw_one_point" << endl; + upScan.distance[laser_index] = circle(d, alfa ,theta, data); + upScan.intensities[laser_index] = class_intensity[cls + 1]; + } + laser_index++; + } +} +vector LCFusion::find_circle_center(int cluster_num) +{ + vector center_point(2); + int index = 0; + float total_dis = 0; + int num = upScan.clustedCloud[cluster_num].size(); + int no_zero_dis = 0; + for (int i = 0 ; i < num; i++) + { + index = upScan.clustedCloud[cluster_num][i].at(0, 3); + if (upScan.distance[index] != 0) + no_zero_dis++; + total_dis += upScan.distance[index]; + } + center_point[0] = total_dis/ no_zero_dis; + index -= (num/2); + float center_laser_index = index; + center_point[1] = center_laser_index; + return center_point; +} +float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度 +{ + float ro = d * cos(theta - alfa) - sqrt(data); + return ro > 0 ? ro:0; +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) +{ + float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); + float angle_max = ANGLE_TO_RADIAN(laserData.angle_max); + uint32_t len = laserData.len; + float angle_inc = (angle_max - angle_min) / (len - 1.); + scan.header.frame_id = "down_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_min = angle_min; + scan.angle_max = angle_max; + scan.angle_increment = angle_inc; + scan.ranges.resize(len); + scan.intensities.resize(len); + scan.time_increment = 1. / 2400.; + for (int i = 0; i < len; i++) + { + scan.ranges[i] = laserData.distance[i] / 1000.f; + scan.intensities[i] = 0; + } +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) +{ + scan.header.frame_id = "up_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_increment = upScan.angleInc; + scan.ranges.resize(upScan.distance.size()); + scan.intensities.resize(upScan.distance.size()); + scan.angle_min = upScan.angleMin; + scan.angle_max = upScan.angleMax; + for (int i = 0; i < upScan.distance.size(); i++) + { + scan.ranges[i] = upScan.distance[i]; + scan.intensities[i] = upScan.intensities[i]; + } + scan.time_increment = 1. / 2400.; +} + + diff --git a/融合/fusion24.txt b/融合/fusion24.txt new file mode 100644 index 0000000..e676dd0 --- /dev/null +++ b/融合/fusion24.txt @@ -0,0 +1,489 @@ +#include +#include "fusion.h" +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) +char LCFusion::fusion(std::vector &camInfo) +{ + Mat boxes = camInfo[1], dwha = camInfo[2]; + projectedPoints.clear(); + + if (this->upScan.pointCloud.empty()) + { + std::cout << "There is no lidar data!" << std::endl; + return -2; + } + + if (boxes.empty()) + { + std::cout << "There is no detection box!" << std::endl; + return -1; + } + else + { + draw_point(); + std::cout << "Fusion start!" << std::endl; + // 大物体 + vector clusterlable = clusters_bboxs(boxes); + optimize_clusterlable(clusterlable, boxes); + big_fusion(clusterlable, boxes, dwha); + + //小物体 + small_fusion(boxes, dwha); + } + return 1; +} + +void LCFusion::big_fusion(vector &clusterlable, Mat &bboxs, Mat &dwha) +{ + std::cout << "big_fusion" << std::endl; + for (int i = 0; i < clusterlable.size(); i++) + { + int bbox_num = clusterlable[i]; + + if (bbox_num != -1) + { + int bbox_cls = bboxs.at(bbox_num, 5); + if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) + { + //std::cout << "bboxcls:" << bbox_cls << std::endl; + for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) + { + int laser_index = this->upScan.clustedCloud[i][j].at(0, 3); + this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1]; + } + } + else + { + if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10) + { + cout << "******start draw circle******" << endl; + draw_circle(i, bbox_num, bboxs, dwha); + } + + } + } + } +} +void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) +{ + std::cout << "small_fusion" << std::endl; + for (int i = 0; i < bboxs.rows; i++) + { + int c = bboxs.at(i, 5); + std::cout << "cls:" << c << std::endl; + if (class_index[c] >= 0 && class_index[c] <= 6) + { + + Mat temp(2, 1, CV_32F); + double d = dwha.at(i, 0); + double a = dwha.at(i, 3); + if (d == -1) + continue; + std::cout << "d:" << d << " a: " << a << std::endl; + temp.at(0, 0) = float((dwha.at(i, 0) / 100.0) * cos(dwha.at(i, 3))); + temp.at(1, 0) = float((dwha.at(i, 0) / 100.0) * sin(dwha.at(i, 3))); + temp = c2lR * temp + c2lT; + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); + + float angle = 0.0; + if (y >= 0) + angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + double data_angle = atan((dwha.at(i, 1)) / (200 * dis)); + data_angle /= this->upScan.angleInc; + angle = (int)angle - (int)data_angle; + int num_point = 2 * (int)data_angle + 1; + + int len = this->upScan.distance.size(); + for (int i = 0; i < num_point; i++) + { + if ((angle + i) < 0 || (angle + i) > (len - 1)) + continue; + + this->upScan.distance[angle + i] = dis; + this->upScan.intensities[angle + i] = class_intensity[c + 1]; + } + } + } +} + +void LCFusion::optimize_clusterlable(vector &clusterlable, Mat &bboxs) +{ + for (int i = 0; i < bboxs.rows; i++) + { + vector temp_cluster_num; + Mat temp_bbox = bboxs.row(i).clone(); + for (int j = 0; j < clusterlable.size(); j++) + { + if (clusterlable[j] == i) + { + temp_cluster_num.push_back(j); + } + } + if (temp_cluster_num.size() == 0) + continue; + choose_forceground(temp_cluster_num, temp_bbox, clusterlable); + } +} + +void LCFusion::choose_forceground(vector &cluster_num, Mat &bbox, vector &clusterlable) +{ + int box_class = bbox.at(0, 5); + if (class_index[box_class] >= 15 && class_index[box_class] <= 18 && cluster_num.size() > 3) + { + int total_point = 0; + for (int i = 0; i < cluster_num.size(); i++) + { + total_point += (this->upScan.clustedCloud[cluster_num[i]].size()); + } + for (int j = 0; j < cluster_num.size(); j++) + { + float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size(); + if (num_cluster / total_point > 0.2) + { + clusterlable[cluster_num[j]] = -1; + } + } + } +} + +void LCFusion::draw_point() +{ + projectedPoints.clear(); + for (int i = 0; i < this->upScan.pointCloud.rows; i++) + { + Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone(); + Mat uv = pointcloud_pixel(temp_pointcloud); + int x = uv.at(0, 0); + int y = uv.at(1, 0); + if (x > 0 && x < 640 && y > 0 && y < 480) + { + Point2d pt_uv(x, y); + projectedPoints.push_back(pt_uv); + } + } +} + +void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) +{ + upScan.distance.clear(); + upScan.intensities.clear(); + float pointAngle; + upScan.angleInc = angleInc; + + for (int i = 0; i < laserData.len; i++) + { + pointAngle = angle_min + i * upScan.angleInc; + if (pointAngle > cameraAnglemin) + { + if (pointAngle > cameraAnglemax) + { + upScan.angleMax = pointAngle - upScan.angleInc; + break; + } + upScan.distance.push_back(laserData.distance[i] / 1000.f); + upScan.intensities.push_back(0); + // upScan.intensities.push_back(laserData.intensities[i]); + } + } + upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc; + this->scan_to_pointcloud(); + this->cluster(); +} + +void LCFusion::scan_to_pointcloud() +{ + u_int32_t len = upScan.distance.size(); + Mat pointcloud(len, 4, CV_32F); + + for (int i = 0; i < len; i++) + { + float dis = this->upScan.distance[i]; + float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis; + float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis; + pointcloud.at(i, 0) = x_temp; + pointcloud.at(i, 1) = y_temp; + pointcloud.at(i, 2) = 0.0; + pointcloud.at(i, 3) = i; + } + this->upScan.pointCloud = pointcloud.clone(); +} + +uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) +{ + int x = uv.at(0, 0); + int y = uv.at(1, 0); + // 实验测试，过滤类别 + std::set filterCls = {0, 7, 11, 14, 15, 21}; + for (uchar i = 0; i < boxes.rows; i++) + { + int clsIdx = boxes.at(i, 5); + if (filterCls.find(clsIdx) != filterCls.end()) + continue; + + float xmin = boxes.at(i, 0); + float xmax = boxes.at(i, 2); + float ymin = boxes.at(i, 1); + float ymax = boxes.at(i, 3); + + // 实验测试，过滤过大的误检框 + float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + if (ratio > 0.7) + { + boxflag[i] += 1; + continue; + } + + // 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况 + if (x >= xmin && x <= xmax && y >= ymin && y <= ymax) + { + boxflag[i] += 1; + return class_intensity[clsIdx + 1]; + } + } + return 0; +} + +std::vector LCFusion::clusters_bboxs(Mat &bboxs) +{ + std::vector clusterlable; + for (int i = 0; i < this->upScan.clustedCloud.size(); i++) + { + std::vector temp_cluster = this->upScan.clustedCloud[i]; + int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); + clusterlable.push_back(temp_clusters_bbox); + } + return clusterlable; +} + +int LCFusion::cluster_bboxs(vector &one_cluster, Mat &bboxs) +{ + vector temp_result; + for (int i = 0; i < bboxs.rows; i++) + { + Mat begin = pointcloud_pixel(one_cluster[0]); + Mat end = pointcloud_pixel(one_cluster[one_cluster.size() - 1]); + Mat bbox = bboxs.row(i).clone(); + int cls = bbox.at(0, 5); + float ratio = ratio_in_1box(begin, end, bbox); + if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) + temp_result.push_back(i); + } + if (temp_result.size() < 1) + return -1; + else if (temp_result.size() < 2) + return temp_result[0]; + else + { + int min = temp_result[0]; + int width_min = bboxs.at(min, 2) - bboxs.at(min, 0); + for (int i = 0; i < temp_result.size(); i++) + { + if (min = temp_result[i]) + continue; + else + { + int width_new = bboxs.at(i, 2) - bboxs.at(i, 0); + if (width_new < width_min) + { + min = i; + width_min = width_new; + } + } + } + return min; + } + return -1; +} + +Mat LCFusion::pointcloud_pixel(Mat &pointcloud) +{ + Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3)); + Mat camPoint = rotate * cutPointCloud.t() + translation; + // float ppp = camPoint.at(2, 0); + if (camPoint.at(2, 0) <= 0) + { + uv = (Mat_(3, 1) << -1, -1, -1); + return uv; + } + + float scaleX = camPoint.at(0, 0) / camPoint.at(2, 0); + float scaleY = camPoint.at(1, 0) / camPoint.at(2, 0); + float scaleD = scaleX * scaleX + scaleY * scaleY; + float tempD = 1 + distCoeff.at(0, 0) * scaleD + distCoeff.at(0, 1) * scaleD * scaleD + distCoeff.at(0, 4) * scaleD * scaleD * scaleD; + + camPoint.at(0, 0) = scaleX * tempD + 2 * distCoeff.at(0, 2) * scaleX * scaleY + + distCoeff.at(0, 3) * (scaleD + 2 * scaleX * scaleX); + + camPoint.at(1, 0) = scaleY * tempD + distCoeff.at(0, 2) * (scaleD + 2 * scaleY * scaleY) + + 2 * distCoeff.at(0, 3) * scaleX * scaleY; + + camPoint.at(2, 0) = 1.0; + uv = cameraMat * camPoint; + uv = p2r * uv + t; + return uv; +} + +float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) +{ + int x_end = begin.at(0, 0), x_begin = end.at(0, 0), y_end = begin.at(1, 0), y_begin = end.at(1, 0); + int xmin = box.at(0, 0), ymin = box.at(0, 1), xmax = box.at(0, 2), ymax = box.at(0, 3); + if (y_begin < ymin || y_begin > ymax) + return 0; + if (x_begin < xmin) + { + if (x_end < xmin) + return 0; + else if (x_end < xmax) + return (float)(x_end - xmin) / (x_end - x_begin); + else + return (float)(xmax - xmin) / (x_end - x_begin); + } + else if (x_begin < xmax) + { + if (x_end < xmax) + return 1; + else + return (float)(xmax - x_begin) / (x_end - x_begin); + } + else + return 0; +} + +void LCFusion::cluster() +{ + this->upScan.clustedCloud.clear(); + int i = 0; + + for (; i < this->upScan.distance.size();) + { + std::vector temp; + int j = i; + temp.push_back(this->upScan.pointCloud.row(i)); + for (; j < this->upScan.distance.size() - 1; j++) + { + + // 距离的6%作为分类的阈值 + if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 || this->upScan.distance[j + 1] == 0) + { + temp.push_back(this->upScan.pointCloud.row(j + 1)); + } + else + { + i = j + 1; + break; + } + } + this->upScan.clustedCloud.push_back(temp); + if (j == this->upScan.distance.size() - 1) + break; + if (i == upScan.distance.size() - 1) + { + temp.push_back(upScan.pointCloud.row(i)); + this->upScan.clustedCloud.push_back(temp); + break; + } + } + + this->clusterIdx.clear(); + int ii = 0, totoalNum = 0; + for (auto c : this->upScan.clustedCloud) + { + for (int jj = 0; jj < c.size(); jj++) + { + this->clusterIdx.push_back(ii); + } + totoalNum += c.size(); + ii++; + } + std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl; +} +void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) +{ + vector center_point = find_circle_center(cluster_num); + float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at(bbox_num, 1)/200.f, + d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc); + int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at(bbox_num, 5); + float laser_index = center_point[1] - half_data_num; + cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl; + for (int i = 0; i < num_laser_circle; i++) + { + float theta = upScan.angleMin + laser_index * upScan.angleInc; + if (laser_index < 0 || laser_index > upScan.distance.size()) + { + laser_index++; + continue; + } + float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2); + if (data >= 0) + { + cout << "draw_one_point" << endl; + upScan.distance[laser_index] = circle(d, alfa ,theta, data); + upScan.intensities[laser_index] = class_intensity[cls + 1]; + } + laser_index++; + } +} +vector LCFusion::find_circle_center(int cluster_num) +{ + vector center_point(2); + int index = 0; + float total_dis = 0; + int num = upScan.clustedCloud[cluster_num].size(); + for (int i = 0 ; i < num; i++) + { + index = upScan.clustedCloud[cluster_num][i].at(0, 3); + total_dis += upScan.distance[index]; + } + center_point[0] = total_dis/num; + index -= (num/2); + float center_laser_index = index; + center_point[1] = center_laser_index; + return center_point; +} +float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度 +{ + float ro = d * cos(theta - alfa) - sqrt(data); + return ro > 0 ? ro:0; +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) +{ + float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); + float angle_max = ANGLE_TO_RADIAN(laserData.angle_max); + uint32_t len = laserData.len; + float angle_inc = (angle_max - angle_min) / (len - 1.); + scan.header.frame_id = "down_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_min = angle_min; + scan.angle_max = angle_max; + scan.angle_increment = angle_inc; + scan.ranges.resize(len); + scan.intensities.resize(len); + scan.time_increment = 1. / 2400.; + for (int i = 0; i < len; i++) + { + scan.ranges[i] = laserData.distance[i] / 1000.f; + scan.intensities[i] = 0; + } +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) +{ + scan.header.frame_id = "up_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_increment = upScan.angleInc; + scan.ranges.resize(upScan.distance.size()); + scan.intensities.resize(upScan.distance.size()); + scan.angle_min = upScan.angleMin; + scan.angle_max = upScan.angleMax; + for (int i = 0; i < upScan.distance.size(); i++) + { + scan.ranges[i] = upScan.distance[i]; + scan.intensities[i] = upScan.intensities[i]; + } + scan.time_increment = 1. / 2400.; +} diff --git a/融合/lidar.cpp b/融合/lidar.cpp new file mode 100644 index 0000000..66ecfa3 --- /dev/null +++ b/融合/lidar.cpp @@ -0,0 +1,583 @@ +/** + * @file lipkg.cpp + * @author LD Robot + * @version V01 + * @brief + * @note + * @attention COPYRIGHT LDROBOT + **/ + +#include "lidar.h" +#include +#include + +// #ifdef USE_SLBI +// #include "slbi.h" +// #endif + +// #ifdef USE_SLBF +// #include "slbf.h" +// #endif +//#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000) +#define MAX_ACK_BUF_LEN 2304000 + +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) + +static const uint8_t CrcTable[256] = + { + 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, + 0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, + 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, + 0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, + 0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, + 0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, + 0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, + 0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, + 0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, + 0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, + 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, + 0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, + 0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, + 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, + 0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, + 0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, + 0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, + 0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, + 0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, + 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, + 0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, + 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8}; + +CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) : mRxThread(nullptr), + mRxCount(0), + mReadCallback(nullptr), + version(ver) +{ + mComHandle = -1; +} +CmdInterfaceLinux::~CmdInterfaceLinux() +{ + Close(); +} +bool CmdInterfaceLinux::Open(std::string &port_name) +{ + int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); + + mComHandle = open(port_name.c_str(), flags); + if (-1 == mComHandle) + { + std::cout << "CmdInterfaceLinux::Open " << port_name << " error !" << std::endl; + return false; + } + + // get port options + struct termios options; + if (-1 == tcgetattr(mComHandle, &options)) + { + Close(); + std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl; + return false; + } + + options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS); + options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD); + options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | + ISIG | IEXTEN); //|ECHOPRT + options.c_oflag &= (tcflag_t) ~(OPOST); + options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK); + + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 0; + + switch (version) + { + case 0: + cfsetispeed(&options, B115200); + break; + case 3: + cfsetispeed(&options, B115200); + break; + case 6: + cfsetispeed(&options, B230400); + break; + case 8: + cfsetispeed(&options, B115200); + break; + case 14: + cfsetispeed(&options, B115200); + break; + default: + cfsetispeed(&options, B115200); + } + + if (tcsetattr(mComHandle, TCSANOW, &options) < 0) + { + std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl; + Close(); + return false; + } + + tcflush(mComHandle, TCIFLUSH); + + mRxThreadExitFlag = false; + mRxThread = new std::thread(mRxThreadProc, this); + mIsCmdOpened = true; + + return true; +} +bool CmdInterfaceLinux::Close() +{ + if (mIsCmdOpened == false) + { + return true; + } + + mRxThreadExitFlag = true; + + if (mComHandle != -1) + { + close(mComHandle); + mComHandle = -1; + } + + if ((mRxThread != nullptr) && mRxThread->joinable()) + { + mRxThread->join(); + delete mRxThread; + mRxThread = NULL; + } + + mIsCmdOpened = false; + + return true; +} +bool CmdInterfaceLinux::GetCmdDevices(std::vector> &device_list) +{ + struct udev *udev; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + struct udev_device *dev; + + udev = udev_new(); + if (!udev) + { + return false; + } + enumerate = udev_enumerate_new(udev); + udev_enumerate_add_match_subsystem(enumerate, "tty"); + udev_enumerate_scan_devices(enumerate); + devices = udev_enumerate_get_list_entry(enumerate); + udev_list_entry_foreach(dev_list_entry, devices) + { + const char *path; + + path = udev_list_entry_get_name(dev_list_entry); + dev = udev_device_new_from_syspath(udev, path); + std::string dev_path = std::string(udev_device_get_devnode(dev)); + dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device"); + if (dev) + { + std::pair p; + p.first = dev_path; + p.second = udev_device_get_sysattr_value(dev, "product"); + device_list.push_back(p); + udev_device_unref(dev); + } + else + { + continue; + } + } + udev_enumerate_unref(enumerate); + udev_unref(udev); + return true; +} +bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len) +{ + static timespec timeout = {0, (long)(100 * 1e6)}; + int32_t len = -1; + + if (IsOpened()) + { + fd_set read_fds; + FD_ZERO(&read_fds); + FD_SET(mComHandle, &read_fds); + int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL); + if (r < 0) + { + // Select was interrupted + if (errno == EINTR) + { + return false; + } + } + else if (r == 0) // timeout + { + return false; + } + + if (FD_ISSET(mComHandle, &read_fds)) + { + len = (int32_t)read(mComHandle, rx_buf, rx_buf_len); + if ((len != -1) && rx_len) + { + *rx_len = len; + } + } + } + return len == -1 ? false : true; +} +bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len) +{ + int32_t len = -1; + + if (IsOpened()) + { + len = (int32_t)write(mComHandle, tx_buf, tx_buf_len); + if ((len != -1) && tx_len) + { + *tx_len = len; + } + } + return len == -1 ? false : true; +} +void CmdInterfaceLinux::mRxThreadProc(void *param) +{ + CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param; + char *rx_buf = new char[MAX_ACK_BUF_LEN + 1]; + uchar failedNum = 0; + while (!cmd_if->mRxThreadExitFlag.load()) + { + uint32_t readed = 0; + bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed); + if (res && readed) + { + cmd_if->mRxCount += readed; + if (cmd_if->mReadCallback != nullptr) + { + cmd_if->mReadCallback(rx_buf, readed); + } + failedNum = 0; + } + else + { + if(++failedNum == 255) + break; + } + } + cmd_if->mRxThreadExitFlag = true; + delete[] rx_buf; +} + +LiPkg::LiPkg() : mTimestamp(0), + mSpeed(0), + mErrorTimes(0), + mIsFrameReady(false), + mIsPkgReady(false) +{ +} +double LiPkg::GetSpeed(void) +{ + return mSpeed / 360.0; +} +bool LiPkg::Parse(const uint8_t *data, long len) +{ + for (int i = 0; i < len; i++) + { + mDataTmp.push_back(*(data + i)); + } + + if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) + return false; + + if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100) + { + mErrorTimes++; + mDataTmp.clear(); + return false; + } + + uint16_t start = 0; + + while (start < mDataTmp.size() - 2) + { + start = 0; + while (start < mDataTmp.size() - 2) + { + if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN)) + { + break; + } + + if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5)))) + { + break; + } + start++; + } + + if (start != 0) + { + mErrorTimes++; + for (int i = 0; i < start; i++) + { + mDataTmp.erase(mDataTmp.begin()); + } + } + + if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) + return false; + + LiDARFrameTypeDef *pkg = (LiDARFrameTypeDef *)mDataTmp.data(); + uint8_t crc = 0; + + for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++) + { + crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff]; + } + + if (crc == pkg->crc8) + { + double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360; + if (diff > (double)pkg->speed * POINT_PER_PACK / 2300 * 3 / 2) + { + mErrorTimes++; + } + else + { + mSpeed = pkg->speed; + mTimestamp = pkg->timestamp; + uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000; + float step = diff / (POINT_PER_PACK - 1) / 100.0; + float start = (double)pkg->start_angle / 100.0; + float end = (double)(pkg->end_angle % 36000) / 100.0; + PointData data; + for (int i = 0; i < POINT_PER_PACK; i++) + { + data.distance = pkg->point[i].distance; + data.angle = start + i * step; + if (data.angle >= 360.0) + { + data.angle -= 360.0; + } + data.confidence = pkg->point[i].confidence; + mOnePkg[i] = data; + mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence)); + } + // prevent angle invert + mOnePkg.back().angle = end; + + mIsPkgReady = true; + } + + for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++) + { + mDataTmp.erase(mDataTmp.begin()); + } + + if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) + { + break; + } + } + else + { + mErrorTimes++; + /*only remove header,not all frame,because lidar data may contain head*/ + for (int i = 0; i < 2; i++) + { + mDataTmp.erase(mDataTmp.begin()); + } + } + } + + return true; +} +bool LiPkg::AssemblePacket() +{ + float last_angle = 0; + std::vector tmp; + int count = 0; + for (auto n : mFrameTemp) + { + /*wait for enough data, need enough data to show a circle*/ + if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */ + { + mFrameData.len = tmp.size(); + Transform(tmp); /*transform raw data to stantard data */ + + if (tmp.size() == 0) + { + mFrameTemp.clear(); + mIsFrameReady = false; + return false; + } + +#ifdef USE_SLBI + Slbi sb(mSpeed); + sb.FindBarcode(tmp); +#endif + +#ifdef USE_SLBF + Slbf sb(mSpeed); + tmp = sb.NearFilter(tmp); +#endif + + std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) + { return a.angle < b.angle; }); + mFrameData.angle_min = tmp[0].angle; + mFrameData.angle_max = tmp.back().angle; + mFrameData.distance.clear(); + mFrameData.intensities.clear(); + mFrameData.distance.resize(mFrameData.len); + mFrameData.intensities.resize(mFrameData.len); + float step = (mFrameData.angle_max - mFrameData.angle_min) / mFrameData.len; + float angle_acc = mFrameData.angle_min; + int tmp_count = 0; + /* interpolation method */ + for (uint32_t i = 0; i < mFrameData.len; i++) + { + if (angle_acc >= tmp[tmp_count].angle) + { + mFrameData.distance[i] = tmp[tmp_count].distance; + mFrameData.intensities[i] = tmp[tmp_count].confidence; + tmp_count++; + if (tmp_count == tmp.size()) + { + break; + } + } + else + { + mFrameData.distance[i] = 0; + mFrameData.intensities[i] = 0; + } + angle_acc += step; + } + std::vector tmp2; + for (uint32_t i = count; i < mFrameTemp.size(); i++) + { + tmp2.push_back(mFrameTemp[i]); + } + mFrameTemp.clear(); + mFrameTemp = tmp2; + mIsFrameReady = true; + return true; + } + else + { + tmp.push_back(n); /* getting data */ + } + + count++; + last_angle = n.angle; + } + + return false; +} +const std::array &LiPkg::GetPkgData(void) +{ + mIsPkgReady = false; + return mOnePkg; +} + +std::vector LiPkg::scan_to_pointcloud(FrameData mFrameData) +{ + u_int32_t len = mFrameData.len; + float angle_min = ANGLE_TO_RADIAN(mFrameData.angle_min); + float angle_max = ANGLE_TO_RADIAN(mFrameData.angle_max); + cv::Mat pointcloud2(len, 3, CV_32F); + cv::Mat intensities(1, len, CV_32F); + cv::Mat pointdis(1, len, CV_32F); + float a_increment = (angle_max - angle_min) / (float)(len - 1); + for (int i = 0; i < len; i++) + { + float dis = mFrameData.distance[i] / 1000.f; + intensities.at(0, i) = mFrameData.intensities[i]; + pointdis.at(0, i) = dis; + + float x_temp = cos(angle_min + i * a_increment) * dis; + float y_temp = sin(angle_min + i * a_increment) * dis; + pointcloud2.at(i, 0) = x_temp; + pointcloud2.at(i, 1) = y_temp; + pointcloud2.at(i, 2) = 0.0; + } + + return std::vector{pointcloud2, intensities, pointdis}; +} + +/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ + +SlTransform::SlTransform(LDVersion version, bool to_right_hand) +{ + switch (version) + { + case LDVersion::LD_ZERO: + case LDVersion::LD_NINE: + offset_x = 8.1; + offset_y = -22.5156; + break; + + case LDVersion::LD_THREE: + case LDVersion::LD_EIGHT: + offset_x = 5.9; + offset_y = -20.14; + break; + + case LDVersion::LD_FOURTEENTH: + offset_x = 5.9; + offset_y = -20.14; + break; + + default: + break; + } +} +Points2D SlTransform::Transform(const Points2D &data) +{ + Points2D tmp2; + for (auto n : data) + { + /*Filter out invalid data*/ + if (n.distance == 0) + { + continue; + } + /*transfer the origin to the center of lidar circle*/ + /*The default direction of radar rotation is clockwise*/ + /*transfer to the right-hand coordinate system*/ + float right_hand = (360.f - n.angle); + double x = n.distance + offset_x; + double y = n.distance * 0.11923 + offset_y; + double d = sqrt(x * x + y * y); + double shift = atan(y / x) * 180.f / 3.14159; + /*Choose whether to use the right-hand system according to the flag*/ + double angle; + if (to_right_hand) + angle = right_hand + shift; + else + angle = n.angle - shift; + + if (angle > 360) + { + angle -= 360; + } + if (angle < 0) + { + angle += 360; + } + + tmp2.push_back(PointData(angle, n.distance, n.confidence, x, y)); + } + + return tmp2; +} +SlTransform::~SlTransform() +{ +} +void LD14_LiPkg::Transform(std::vector &tmp) +{ + SlTransform trans(LDVersion::LD_FOURTEENTH); + tmp = trans.Transform(tmp); + // std::cout << "Transform LD_FOURTEENTH !!" << std::endl; +} \ No newline at end of file diff --git a/融合/lidar.h b/融合/lidar.h new file mode 100644 index 0000000..49b1d25 --- /dev/null +++ b/融合/lidar.h @@ -0,0 +1,174 @@ +/** +* @file lidar.h +* @author LD Robot +* @version V01 + +* @brief +* @note +* @attention COPYRIGHT LDROBOT +**/ + +#ifndef __LIPKG_H +#define __LIPKG_H +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct PointData +{ + //�������ʾ��ʽ + float angle; + uint16_t distance; + uint8_t confidence; + //ֱ�������ʾ��ʽ + double x; + double y; + PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0) + { + this->angle = angle; + this->distance = distance; + this->confidence = confidence; + this->x = x; + this->y = y; + } + PointData() {} + friend std::ostream& operator<<(std::ostream& os, const PointData& data) + { + os << data.angle << " " << data.distance << " " << (int)data.confidence << " " << data.x << " " << data.y; + return os; + } +}; +typedef std::vector Points2D; + + +enum +{ + PKG_HEADER = 0x54, + PKG_VER_LEN = 0x2C, + POINT_PER_PACK = 12, +}; +typedef struct __attribute__((packed)) +{ + uint16_t distance; + uint8_t confidence; +}LidarPointStructDef; +typedef struct __attribute__((packed)) +{ + uint8_t header; + uint8_t ver_len; + uint16_t speed; + uint16_t start_angle; + LidarPointStructDef point[POINT_PER_PACK]; + uint16_t end_angle; + uint16_t timestamp; + uint8_t crc8; +}LiDARFrameTypeDef; +struct FrameData +{ + ros::Time timestamp; + float angle_min; + float angle_max; + uint32_t len; + std::vector distance; + std::vector intensities; +}; +class LiPkg +{ +public: + LiPkg(); + double GetSpeed(void); /*Lidar spin speed (Hz)*/ + uint16_t GetTimestamp(void) { return mTimestamp; } /*time stamp of the packet */ + bool IsPkgReady(void) { return mIsPkgReady; }/*a packet is ready */ + bool IsFrameReady(void) { return mIsFrameReady; }/*Lidar data frame is ready*/ + long GetErrorTimes(void) { return mErrorTimes; }/*the number of errors in parser process of lidar data frame*/ + const std::array& GetPkgData(void);/*original data package*/ + bool Parse(const uint8_t* data, long len);/*parse single packet*/ + virtual void Transform(std::vector& tmp) = 0;/*transform raw data to stantard data */ + bool AssemblePacket();/*combine stantard data into data frames and calibrate*/ + const FrameData& GetFrameData(void) { mIsFrameReady = false; return mFrameData; } + std::vector scan_to_pointcloud(FrameData mFrameData); +private: + uint16_t mTimestamp; + double mSpeed; + std::vector mDataTmp; + long mErrorTimes; + std::arraymOnePkg; + std::vector mFrameTemp; + FrameData mFrameData; + bool mIsPkgReady; + bool mIsFrameReady; +}; +class LD14_LiPkg : public LiPkg +{ +public: + virtual void Transform(std::vector& tmp); +}; + +enum class LDVersion +{ + LD_ZERO, /*Zero generation lidar*/ + LD_THREE, /*Third generation lidar*/ + LD_EIGHT, /*Eight generation radar*/ + LD_NINE, /*Nine generation radar*/ + LD_FOURTEENTH /*Fourteenth generation radar*/ +}; + +class SlTransform +{ +private: + bool to_right_hand = true; + double offset_x; + double offset_y; + +public: + SlTransform(LDVersion version, bool to_right_hand = false); + Points2D Transform(const Points2D& data); + ~SlTransform(); +}; + + + +class CmdInterfaceLinux +{ +public: + CmdInterfaceLinux(int32_t ver); + ~CmdInterfaceLinux(); + + bool Open(std::string& port_name); + bool Close(); + bool ReadFromIO(uint8_t* rx_buf, uint32_t rx_buf_len, uint32_t* rx_len); + bool WriteToIo(const uint8_t* tx_buf, uint32_t tx_buf_len, uint32_t* tx_len); + bool GetCmdDevices(std::vector >& device_list); + void SetReadCallback(std::function callback) { mReadCallback = callback; } + bool IsOpened() { return mIsCmdOpened.load(); }; + bool IsExited() { return mRxThreadExitFlag.load(); }; + +private: + std::thread* mRxThread; + static void mRxThreadProc(void* param); + long long mRxCount; + int32_t version; + int32_t mComHandle; + std::atomic mIsCmdOpened, mRxThreadExitFlag; + std::function mReadCallback; +}; + +#endif +/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ diff --git a/融合/m28.rknn b/融合/m28.rknn new file mode 100644 index 0000000..fb3a6d4 Binary files /dev/null and b/融合/m28.rknn differ diff --git a/融合/main.cpp b/融合/main.cpp new file mode 100644 index 0000000..5202a71 --- /dev/null +++ b/融合/main.cpp @@ -0,0 +1,156 @@ +#include +#include +#include "lidar.h" +#include +#include +#include +#include "ranging.h" +#include "fusion.h" +#include +#include +#include +#include +/*#include +#include */ +#include "opencv2/imgcodecs/imgcodecs.hpp" +#include + +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) + +Ranging r(9, 640, 480); +std::queue> frameInfo; +std::queue laser_queue; +std::mutex g_mutex; + +void *ranging(void *args) // ranging线程 +{ + while (true) + { + std::cout<<" ************ enter ranging *********** "< result = r.get_range(); + g_mutex.lock(); + for (uchar i = 0; i < frameInfo.size(); i++) // 只保存当前最新的图片帧信息 + frameInfo.pop(); + frameInfo.push(result); + g_mutex.unlock(); + } +} + +int main(int argc, char **argv) +{ + uint8_t laser_queue_size = 2; // 雷达帧队列大小参数 + LD14_LiPkg *pkg = new LD14_LiPkg; + CmdInterfaceLinux cmd_port(9); + std::string port_name = "/dev/ttyUSB0"; + if (argc > 1) + port_name = argv[1]; +// 雷达读取回调函数，一直在执行雷达读取和处理数据 + cmd_port.SetReadCallback([&pkg](const char *byte, size_t len) { + if (pkg->Parse((uint8_t*)byte, len)) + { + pkg->AssemblePacket(); + } }); + if (!cmd_port.Open(port_name)) + return -1; + + ros::init(argc, argv, "publish"); + ros::NodeHandle nh; /* create a ROS Node */ + ros::Publisher uplidar_pub = nh.advertise("up_scan", 1); + //ros::Publisher downlidar_pub = nh.advertise("down_scan", 1); + sensor_msgs::LaserScan upscan; + // sensor_msgs::LaserScan downscan; + + + /*image_transport::ImageTransport it(nh); + image_transport::Publisher img_pub = it.advertise("camera/image", 1);*/ + pthread_t tids[1]; // 执行ranging线程 + int ret = pthread_create(&tids[0], NULL, ranging, NULL); + if (ret != 0) + { + std::cout << "Multithreading error !" << std::endl; + return -2; + } + + LCFusion lcFusion(&r); + namedWindow("camera"); + + while (ros::ok() && !cmd_port.IsExited()) + { + int64 t = getTickCount(); + if (pkg->IsFrameReady()) // 若一帧雷达处理好，执行后续操作 + { + FrameData laserData_queue = pkg->GetFrameData(); // 得到雷达数据 + laserData_queue.timestamp = ros::Time::now(); + laser_queue.push(laserData_queue); + while(laser_queue.size() > laser_queue_size) // 只保存两帧雷达数据 + { + laser_queue.pop(); + } + } + else + continue; + if (laser_queue.size() == laser_queue_size) // 若队列有两帧数据，执行后续操作 + { + FrameData laserData = laser_queue.front(); + laser_queue.pop(); + upscan.header.stamp = laserData.timestamp; + // upscan.header.stamp = ros::Time::now(); + // downscan.header.stamp = upscan.header.stamp; + + // uint16_t times_stamp = pkg->GetTimestamp(); + // FrameData laserData = pkg->GetFrameData(); + char state = -1; + + float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); // 角度弧度转换 + float angle_max = ANGLE_TO_RADIAN(laserData.angle_max); + uint32_t len = laserData.len; + float angle_inc = (angle_max - angle_min) / (len - 1.); // 角度增量 + lcFusion.filter_laser(laserData, angle_min, angle_inc); // 视角同步，处理视角内雷达数据 + if (!frameInfo.empty()) + { + g_mutex.lock(); + std::vector camInfo = frameInfo.front(); // 读取图像帧数据 + frameInfo.pop(); + g_mutex.unlock(); + double fusion_old,fusion_now; + fusion_old = ros::Time::now().toSec(); + state = lcFusion.fusion(camInfo); // 融合 + fusion_now = ros::Time::now().toSec(); + cout << "data_fusion: " << fusion_now - fusion_old << endl; + if (state != -2) + { + lcFusion.set_laser_data(upscan); // 设置雷达发布数据 + uplidar_pub.publish(upscan); // 发布 + } + // 调试使用，将雷达点映射到图像中显示 + if (!lcFusion.projectedPoints.empty()) + { + for (int i = 0; i < lcFusion.projectedPoints.size(); i++) + { + Point2f p = lcFusion.projectedPoints[i]; + circle(camInfo[0], p, 3, Scalar(0, 255, 0), 1, 8); + } + } + /* sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", camInfo[0]).toImageMsg(); + img_pub.publish(msg);*/ + if (!camInfo[0].empty()) + { + cv::Mat pic; + cv::pyrDown(camInfo[0],pic); + imshow("camera", pic); + } + t = getTickCount() - t; + char fps[50]; + std::cout << "fps: " << int(1 / (t / getTickFrequency())) << std::endl; + if (waitKey(1) == 81) + break; + } + //lcFusion.set_laser_data(downscan, laserData); + //downlidar_pub.publish(downscan); + } + } + std::cout << "ROS or lidar error !" << std::endl; + delete pkg; + cmd_port.Close(); + return 0; +} diff --git a/融合/main.out b/融合/main.out new file mode 100644 index 0000000..05c63c7 Binary files /dev/null and b/融合/main.out differ diff --git a/融合/ranging.cpp b/融合/ranging.cpp new file mode 100644 index 0000000..749b393 --- /dev/null +++ b/融合/ranging.cpp @@ -0,0 +1,444 @@ +#include +#include +#include +#include +#include "ranging.h" +#include +#include + +#include "rknn_sdk.h" +#include +#include +#include +#include "opencv2/core.hpp" +#include "opencv2/imgcodecs.hpp" +#include "opencv2/imgproc.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; + + +void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数 +{ + remap(oriImgL, oriImgL, mapX1, mapX2, INTER_LINEAR); + remap(oriImgR, oriImgR, mapY1, mapY2, INTER_LINEAR); +} + +std::vector Ranging::pic2cam(int u, int v) //像素坐标转相机坐标 +{ + //(u,v)->(x,y)"(loc[0],loc[1])" + std::vector loc; + loc.push_back((u - cam_matrix_right.at(0, 2)) * q.at(2, 3) / cam_matrix_right.at(0, 0)); + loc.push_back((v - cam_matrix_right.at(1, 2)) * q.at(2, 3) / cam_matrix_right.at(1, 1)); + return loc; +} + +std::vector Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配 +{ + int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3]; + // cv::Mat right_image_,left_image_; + // cv::pyrDown(right_image, right_image_); + // cv::pyrDown(left_image, left_image_); + Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框 + Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域 + // Mat tpl = right_image_.rowRange(max(int(0.5*y1 - 2), 0), min(int(0.5*y2 + 2), 479)).colRange(int(0.5*x1), int(0.5*x2)); + // Mat target = left_image_.rowRange(max(int(0.5*y1 - 10), 0), min(int(0.5*y2 + 10), 239)).colRange(0, 319); + int th = tpl.rows, tw = tpl.cols; + Mat result; + // methods = TM_CCOEFF_NORMED, TM_SQDIFF_NORMED, TM_CCORR_NORMED] + matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关 + double minVal, maxVal; + Point minLoc, maxLoc; + minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标 + Point tl = maxLoc, br; + + br.x = min(maxLoc.x + tw, 639); //转为像素坐标系 + br.y = min(maxLoc.y + th, 479); //转为像素坐标系 + ////展示匹配结果 + // br.x = min(maxLoc.x + tw, 319); + // br.y = min(maxLoc.y + th, 239); + //rectangle(target, tl, br, (0, 255, 0), 3); + //imshow("match-", target); + //waitKey(2); + std::vector maxloc; + maxloc.push_back(maxLoc.x); + maxloc.push_back(maxLoc.y); + return maxloc; +} + +void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k) +{ + //保证摄像头与地面平行 + int x1 = bboxs.at(k, 0); + int x2 = bboxs.at(k, 2); + int y1 = bboxs.at(k, 1); + int y2 = bboxs.at(k, 3); + float Conf = bboxs.at(k, 4); + int cls = bboxs.at(k, 5); + float Y_B, Y_H; + Mat edge, grayImage; + vector idx; + Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围 + //Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); + Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1); + cvtColor(tpl, grayImage, COLOR_BGR2GRAY); + GaussianBlur(grayImage,grayImage,Size(5,5),0); + Canny(grayImage, edge, 120, 180, 3); //提取边缘，获取与地面接触点 + //cv::imshow("1",edge); + //cv::waitKey(1); + float cluster[650]; + for (int i = 0;i<650;i++) + { + cluster[i] = 0; + } + int y_b, y_h; + int j = 0; + for (int i = 0; i < x2-x1; i++) + { + //Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2); + Mat temp = edge.col(i); //取第i列 + // std::cout << "temp: " < point_b = pic2cam(x1 + i, 240); //转为相机坐标系 + std::vector point_H = pic2cam(320, 240); + float alfa = atan((point_b[0] - point_H[0]) / q.at(2, 3)); + if (idx.size() < 1) + { + Z.at(0, i) = 0; + Z.at(1, i) = alfa; + continue; + } + int y_b = idx[idx.size() - 1].y + y1; // + y_b = int(y_b + y_b*0.03); + int y_h = 240; + point_b = pic2cam(x1 + i, y_b); //转为相机坐标系 + point_H = pic2cam(320, y_h); + Y_B = point_b[1]; + Y_H = point_H[1]; + // std::cout << "y_b: " << y_b << std::endl; + float H_c = 60; //摄像头离地高度，单位mm + float theta = 0; //摄像头与地面夹角，弧度 + float d = (1/cos(theta)* cos(theta)) * q.at(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta); + alfa = atan((point_b[0] - point_H[0]) / q.at(2, 3)); + //cout << "d: " << d << endl; + if (d > 700) + {d = 0;} + Z.at(0, i) = d/cos(alfa); + Z.at(1, i) = alfa; + } + /*if (i > 0) + { + if (abs(Z.at(0, i) - Z.at(0, i - 1)) < 40) //聚类 + { + cluster[j] = cluster[j] + 1; + } + else + { + j = j + 1; + cluster[j] = 1; + //std::cout<<"j : "<< j<< std::endl; + } + } + } + //int max_loc = distance(cluster,max_element(cluster,cluster + sizeof(cluster)/sizeof(cluster[0]))); //只保留数量最多的一类，其余置0 + //std::cout<<" max_loc : "<< max_loc<(0, t) = 0; + } + } + temp = temp + cluster[i]; + }*/ + // std::cout << "z : " <Z = Z.clone(); +} + +void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info) +{ + // ֱ��ͼ���⻯ + Mat imgGrayL, imgGrayR; + cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY); + cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY); + Mat imgR_weight = imgR.clone(); + Mat infoRow; + for (uchar i = 0; i < detBoxes.rows; i++) + { + int x1 = detBoxes.at(i, 0); + int y1 = detBoxes.at(i, 1); + int x2 = detBoxes.at(i, 2); + int y2 = detBoxes.at(i, 3); + //std::cout<<"x1: "<(i, 4); + int cls = detBoxes.at(i, 5); + + if (cls == 6 || cls == 10 || cls == 11) //体重秤，地毯和毛巾采用单目测距方法 + { + horizon_estimate(imgR_weight, detBoxes, i); + } + if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体 + { + + //char cm[15]; + //sprintf(cm, "cannot match !"); + + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + rectangle(imgR, Point(int(x1), int(y1)), + Point(int(x2), int(y2)), Scalar(0, 0, 255)); + continue; + } + if (cls!=0 && cls!=1 && cls!=2 && cls!= 6 && cls!= 7 && cls!= 10 && cls!=11 && cls!=14 && cls!=15) //大物体不进行测距 + { + if (x1 < 30 || x2 < 80 || x1>580 || x2 > 610) //删除边缘的目标框 + { + detBoxes.at(i, 5) = -1; + cls = detBoxes.at(i, 5); + } + //char cm[15]; + //sprintf(cm, "cannot match !"); + + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + rectangle(imgR, Point(int(x1), int(y1)), + Point(int(x2), int(y2)), Scalar(0, 0, 255)); + continue; + } + + rectangle(imgR, Point(int(x1), int(y1)), + Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框 + + int coordinates[4] = {x1, y1, x2, y2}; + std::vector disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配 + float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差 + float disp_pixel_y = disp_pixel[1] - y1; // + disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到 + + //Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz; + Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息 + Mat threed_pixel_xyz, threedImage; + reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d + threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方， + std::vector channels; + split(threed_pixel_xyz.clone(), channels); + threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离 + threed_pixel_xyz.forEach([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d + + int mid_pixel = int((x1 + x2) / 2); + std::vector mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标 + std::vector loc_tar = pic2cam(mid_pixel, imgGrayR.rows); + float alfa = atan((loc_tar[0] - mid[0]) / q.at(2, 3)); + + + if (disp_pixel_x > 240) // 距离太近，视差过大 + { + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + continue; + } + else + { + float median = threed_pixel_xyz.at((int)(y1 + y2) / 2, (int)(x1 + x2) / 2); + + std::vector ltPoint = pic2cam(x1, y1); + std::vector rbPoint = pic2cam(x2, y2); + float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高 + float f = q.at(2, 3); + float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出 + //float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1; + float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1; + float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f); + //float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2; + float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2; + float w1 = sqrt(pow((threedImage.at(y2, x1)[0] - threedImage.at(y2, x2)[0]), 2) + + pow((threedImage.at(y2, x1)[1] - threedImage.at(y2, x2)[1]), 2) + + pow((threedImage.at(y2, x1)[2] - threedImage.at(y2, x2)[2]), 2)); + + w1 = w1 / 10; + h1 = (h1 + h2) / 10; + median /= 10; + if (median > 120) //过远测距误差较大 + { + //char tf[9]; + //sprintf(tf, "Too far!"); + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + continue; + } + // ��ͼ���ϻ�����Ϣ + char dc[50], wh[50]; + std::string cname = className[cls + 1]; + sprintf(dc, "dis:%.2fcm %d", median, cls); + sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa); + putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2); + putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5); + + + //返回数据 + infoRow = (Mat_(1, 4) << median, w1, h1, alfa); + infoRow.copyTo(info.row(i)); + }; + } +} + +Ranging::Ranging(int index, int imgw, int imgh) : //初始化 + mapX1(imgh, imgw, CV_64F), //初始化矩阵 ，用于计算无畸变和修正转换映射。 + mapX2(imgh, imgw, CV_64F), + mapY1(imgh, imgw, CV_64F), + mapY2(imgh, imgw, CV_64F), + q(4, 4, CV_64F), + imgw(imgw), + imgh(imgh) +{ + // Z = Mat::zeros(2, 1, CV_32FC1); + if (!vcapture.open(index)) + { + std::cout << "Open camera failed !" << std::endl; + exit(EXIT_FAILURE); + } + else + { + //vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG); + vcapture.set(CAP_PROP_FPS, 30); + vcapture.set(CAP_PROP_FRAME_WIDTH, imgw * 2); + vcapture.set(CAP_PROP_FRAME_HEIGHT, imgh); + vcapture.set(CAP_PROP_BUFFERSIZE, 1); + + auto imgSize = Size(imgw, imgh); + Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F); + + stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r, + imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正 + + initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射 + initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射 + + RKNN_Create(&hdx, modelPath); // 初始化检测模型 + std::cout<< " ******************* CAMERA initialization ********************" << std::endl; + } +} + +// std::vector Ranging::pic2cam(int u, int v) //像素坐标转相机坐标 +// { +// std::vector loc; +// loc.push_back((u - cam_matrix_right.at(0, 2)) * q.at(2, 3) / cam_matrix_right.at(0, 0)); +// loc.push_back((v - cam_matrix_right.at(1, 2)) * q.at(2, 3) / cam_matrix_right.at(1, 1)); +// return loc; +// } + + + +std::vector Ranging::get_range() +{ + double rang_old, rang_now; + rang_old = ros::Time::now().toSec(); //测试运行时间 + Mat frame, lframe, rframe; + vcapture.read(frame); //获取视频帧 + + if (!frame.empty()) + { + int64 t = getTickCount(); + Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图 + Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图 + //imshow("lframe",lframe); + //waitKey(1); + rectifyImage(lframe, rframe); // + + DetRet *det_ret = NULL; // + InputData input_data; //定义输入数据 + int shape[4] = {1, 3, rframe.rows, rframe.cols}, det_num = 0; + memcpy(input_data.shape, shape, sizeof(shape)); + Mat Rframe = rframe.clone(); + + double detect_old, detect_now; + detect_old = ros::Time::now().toSec(); + input_data.data = Rframe.data; + //std::cout<<"Rframe.data.shape"<{rframe}; //没有检测框时，退出 + } + std::cout << "det_num: " << det_num << std::endl; + // detction box transfor to our format + Mat detBoxes(det_num, 6, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls) + for (int i = 0; i < det_num; i++) //存储目标检测内容 (x,y,x,y,conf,cls) + { + DetRet det_result = det_ret[i]; + // xyxy conf cls + float xmin = rframe.cols * det_result.fLeft; + float ymin = rframe.rows * det_result.fTop; + float xmax = rframe.cols * det_result.fRight; + float ymax = rframe.rows * det_result.fBottom; + + detBoxes.at(i, 0) = xmin; + detBoxes.at(i, 1) = ymin; + detBoxes.at(i, 2) = xmax; + detBoxes.at(i, 3) = ymax; + detBoxes.at(i, 4) = det_result.fConf; + // 实验测试，过滤过大的误检框 + float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + if (ratio > 0.7) + { + detBoxes.at(i, 5) = -1; + continue; + } + detBoxes.at(i, 5) = det_result.nLabel; + if (det_result.nLabel == 1 || det_result.nLabel == 2) //检测目标为体重秤和风扇底座时，将目标框拉高，使杆子的雷达点能与目标框对应 + { + detBoxes.at(i, 1) = 100; + } + } + Mat info(det_num, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α + if (det_num) + { + // double rang_old, rang_now; + // rang_old = ros::Time::now().toSec(); + getInfo(lframe, rframe, detBoxes, info); + // rang_now = ros::Time::now().toSec(); + // cout << "data_dis_time: " << rang_now-rang_old << endl; + } + t = getTickCount() - t; + char fps[50]; + sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency()))); + putText(rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5); + // rang_now = ros::Time::now().toSec(); + // cout << "data_rang_time" << rang_now - rang_old << endl; + return std::vector{rframe, detBoxes, info}; + } + return std::vector{rframe}; +} diff --git a/融合/ranging.h b/融合/ranging.h new file mode 100644 index 0000000..e71d149 --- /dev/null +++ b/融合/ranging.h @@ -0,0 +1,112 @@ +#pragma once + +#include +#include + +#include +#include +#include "rknn_sdk.h" +#include "fusion.h" + +using namespace cv; +class LCFusion; +class Ranging +{ +private: + friend class LCFusion; + VideoCapture vcapture; + rknn_handle hdx; + const char *modelPath = "m28.rknn"; + std::vector className = { + "Unknown", //未知 + "MaoGouWan", //猫碗/狗碗 + "BTYDiZuo", //吧台怅底座 + "FSDiZuo", //风扇底座 + "XiYiJi", //掗衣机 + "BingXiang", //冰箱 + "MaTong", //马桶 + "TiZhongCheng", //䜓重秀 + "DianXian", //电线 + "DianShiJi", //电视机 + "CanZhuo", //逐桌 + "DiTan", //地毯 + "MaBu", //抹垃 + "ChaJi", //茶几 + "DianShiGu", //电视柜 + "XieZi", //拖鞋/鞋子 + "WaZi", //袜子 + "YiGui", //衣柜 + "Chuang", //床 + "ShaFa", //沙发 + "YiZi", //怅子 + "Men", //闹 + "DLS", //倧理石 + "DiBan", //地板 + }; + + // 双目参数 + Mat cam_matrix_left = (Mat_(3, 3) << + 4.700950170847520e+02, 0, 0, + 0, 4.709244004725060e+02, 0, + 3.220628601465352e+02, 2.265265667516499e+02, 1); + + Mat cam_matrix_right = (Mat_(3, 3) << + 4.696016526986375e+02, 0, 0, + 0, 4.702371464881610e+02, 0, + 3.216994731643572e+02, 2.267449827655198e+02, 1); + Mat distortion_l = (Mat_(1, 5) << 0.088023418049400, -0.047968406803285, 0, + 0, 0); + + Mat distortion_r = (Mat_(1, 5) << 0.097018738959281, -0.081707209740704, 0, + 0, 0); + + Mat rotate = (Mat_(3, 3) << + 0.999895538560624, 1.514179660491009e-04, 0.014452994124399, + -1.935031776135467e-04, 0.999995745723263, 0.002910514026079, + -0.014452491933249, -0.002913006689884, 0.999891314028152); + Mat trans = (Mat_(3, 1) << + -59.978995009996730, 0.050886852679934, -0.088565378305175); +/* + + Mat cam_matrix_left = (Mat_(3, 3) << + 3.627346593660044e+02, 0, 0, + 0, 3.629168542317134e+02, 0, + 3.093231127433517e+02, 2.409353074183058e+02, 1); + + Mat cam_matrix_right = (Mat_(3, 3) << + 3.615909129180773e+02, 0, 0, + 0, 3.617772267770084e+02, 0, + 3.062363746645480e+02, 2.298948842159400e+02, 1); + Mat distortion_l = (Mat_(1, 5) << 0.117219440226075, -0.148594691012994, 0, + 0, 0); + Mat distortion_r = (Mat_(1, 5) << 0.116412295415583, -0.146817143572705, 0, + 0, 0); + Mat rotate = (Mat_(3, 3) << + 0.999962353120719, 4.648106081008926e-04, 0.008664657660520, + -4.493098874759400e-04, 0.999998295540464, -0.001790820145135, + -0.008665475284162, 0.001786859609987, 0.999960857569352); + Mat trans = (Mat_(3, 1) << + -60.097178758944190, -0.033859553542635, -0.423965574285253); +*/ +/* + Mat cam_matrix_left = (Mat_(3, 3) << 3.629995979572968e+02, 0, 0, 0, 3.605811984929196e+02, 0, 3.056618479641253e+02, 2.425978482096805e+02, 1); + Mat cam_matrix_right = (Mat_(3, 3) << 3.642383320232306e+02, 0, 0, 0, 3.619176322068948e+02, 0, 3.048699917391443e+02, 2.355042537698953e+02, 1); + Mat distortion_l = (Mat_(1, 5) << 0.114519742751324, -0.147768779338520, 0.001196745401736, -0.00274469994995, 0.008589264227682); + Mat distortion_r = (Mat_(1, 5) << 0.107862414325132, -0.112272107836791, 0.002046119288761, -8.703752362970271e-04, -0.050706452190122); + Mat rotate = (Mat_(3, 3) << 0.999984364022271, 6.994668898371339e-04, 0.005548194034452, -6.988446369697146e-04, 0.999999749299562, -1.140920137281147e-04, -0.005548272447104, 1.102129041420978e-04, 0.999984602144437); + Mat trans = (Mat_(3, 1) << -60.134851044411256, 0.007279986569326, -0.065092184396760); +*/ + + Mat mapX1, mapX2, mapY1, mapY2, q, Z; + + int imgw, imgh; + +public: + Ranging(int index, int imgw, int imgh); + void rectifyImage(Mat &oriImgL, Mat &oriImgR); + void getInfo(Mat &imgl, Mat &imgr, Mat &detBoxes, Mat &info); + std::vector pic2cam(int u, int v); + std::vector muban(Mat &left_image, Mat &right_image, const int *coordinates); + std::vector get_range(); + void horizon_estimate(Mat& img, Mat& bboxs,int k); +}; diff --git a/融合/ranging.out b/融合/ranging.out new file mode 100644 index 0000000..4f2fb0b Binary files /dev/null and b/融合/ranging.out differ diff --git a/融合/rknn_sdk.h b/融合/rknn_sdk.h new file mode 100644 index 0000000..cda1b0f --- /dev/null +++ b/融合/rknn_sdk.h @@ -0,0 +1,110 @@ +#ifndef __RKNN_SDK_H__ +#define __RKNN_SDK_H__ + +#include + +// 返回错误码 +enum XErrorCode { + RKNN_SUCCEED = 0, + RKNN_NOT_INIT = -1, //SDK未初始化 + RKNN_INVAILD_IMGSIZE = -2, //输入图像尺寸不匹配模型输入要求 + RKNN_INVAILD_PARAM = -3, //参数无效 +}; + +enum ObjCls { + OBJ_Unknown = -1, //未知 + OBJ_MaoGouWan = 0, //猫碗/狗碗 + OBJ_BTYDiZuo = 1, //吧台椅底座 + OBJ_FSDiZuo = 2, //风扇底座 + OBJ_XiYiJi = 3, //洗衣机 + OBJ_BingXiang = 4, //冰箱 + OBJ_MaTong = 5, //马桶 + OBJ_TiZhongCheng = 6, //体重秤 + OBJ_DianXian = 7, //电线 + OBJ_DianShiJi = 8, //电视机 + OBJ_CanZhuo = 9, //餐桌 + OBJ_DiTan = 10, //地毯 + OBJ_MaBu = 11, //抹布 + OBJ_ChaJi = 12, //茶几 + OBJ_DianShiGui = 13, //电视柜 + OBJ_XieZi = 14, //拖鞋/鞋子 + OBJ_WaZi = 15, //袜子 + OBJ_YiGui = 16, //衣柜 + OBJ_Chuang = 17, //床 + OBJ_ShaFa = 18, //沙发 + OBJ_YiZi = 19, //椅子 + OBJ_Men = 20, //门 + OBJ_DLS = 21, //da li shi + OBJ_DiBan = 22 //di ban + //OBJ_Xiang = 21 //宠物粪便 +}; + + +// 图像信息与buffer +typedef struct _InputData { + int shape[4]; // Num, Channel, Height, Width + void* data; // img.data +} InputData; + +// 检测结果 +typedef struct _DetRet +{ + int nID; //图像ID + int nLabel; //物体类别 ObjCls nLabel; //物体类别 + float fConf; //置信度 + float fLeft; //检测框左上角比例坐标x_min -> int(x_min*width) 获取像素坐标 + float fTop; //检测框左上角比例坐标y_min -> int(y_min*height) 获取像素坐标 + float fRight; //检测框右下角比例坐标x_max -> int(x_max*width) 获取像素坐标 + float fBottom; //检测框右下角比例坐标y_max -> int(y_max*height) 获取像素坐标 +} DetRet; + + + +#define RKNNSDK_API(retType) retType + +/// +#ifdef __cplusplus +extern "C" { +#endif + +typedef uint64_t rknn_handle; + + +/* RKNN_Create + SDK加载模型，仅初始化一次; + input: + rknn_handle hd 句柄 + const char* model_path 模型路径 + return: + int 错误码 +*/ +RKNNSDK_API(int) RKNN_Create(rknn_handle* hd, const char* model_path); + +/* RKNN_ObjDet + 输入图像，获取检测结果和检测目标数量; + input: + rknn_handle hd 句柄 + const InputData* input_data 输入图像数据 + DetRet** det_ret 获取检测结果 + int* det_num 获取检测目标数量 + return: + int 错误码 +*/ +RKNNSDK_API(int) RKNN_ObjDet(rknn_handle hd, const InputData* input_data, DetRet** det_ret, int* det_num); + +/* RKNN_Release + SDK资源释放，仅在程序退出前调用; + input: + rknn_handle hd 句柄 + return: + int 错误码 +*/ +RKNNSDK_API(int) RKNN_Release(rknn_handle hd); + +#ifdef __cplusplus +} +#endif + + + +#endif //__RKNN_SDK_H__ diff --git a/融合/zhixing-1 b/融合/zhixing-1 new file mode 100644 index 0000000..cb03575 --- /dev/null +++ b/融合/zhixing-1 @@ -0,0 +1 @@ +g++ -std=c++11 ./*.cpp -o main.out -I /usr/local/include -I /usr/local/include/opencv4 -I /usr/include -I /opt/ros/melodic/include -I /home/firefly/Desktop/object_detect_sdk_1804/install/include -L /usr/local/lib -l opencv_core -l opencv_imgproc -l opencv_imgcodecs -l opencv_highgui -l opencv_dnn -l opencv_videoio -l opencv_calib3d -L /usr/lib/obj_sdk -l rknn_sdk -l rknnrt -l roscpp -l roslib -l rosconsole -l roscpp_serialization -L /opt/ros/melodic/lib -l rostime -l boost_system -l pthread -l udev \ No newline at end of file diff --git a/融合/更新说明.txt b/融合/更新说明.txt new file mode 100644 index 0000000..cd3fe68 --- /dev/null +++ b/融合/更新说明.txt @@ -0,0 +1,12 @@ +V 2.0 + +1. 添加了基于地平线的测距方法，稳定性待测试。 +2. 解决断连bug。原因是融合的判空与双目判空条件不一致。 + +V 2.1 +1. 立体矫正出错，ros的opencv会导致矫正出错，更改为opencv4.x即可。 +2. 解决了摄像头画面远程传输问题 + + +问题描述： +1. 存在摄像头断连的情况，设备号由video9变更为video10引起。 \ No newline at end of file diff --git a/运行方法.txt b/运行方法.txt new file mode 100644 index 0000000..77e062b --- /dev/null +++ b/运行方法.txt @@ -0,0 +1,24 @@ + +//融合代码 +./main.out /dev/ttyUSB0 + +//ttyUSB0为雷达默认设备号，如果有变化，需要修改，摄像头设备号需要源码修改。 + + + +//cartographer: +//运行cartographer +进入目录：cd ~/carto_ws +环境变量：source devel_isolated/setup.bash +运行：roslaunch cartographer_ros wyk.launch + +//后处理： +运行系统：Ubuntu + +运行流程： +1. 运行map.out +2. 当小车建图结束时，点击map可视化窗口，并按ESC建，程序会自动推出，并将接收到的图像保存为map.png +3. 将map.png放在main.py文件同目录下 +4. 运行main.py文件，即可得到经过后处理的语义地图 + +注：