commit 0cef943b16e660f24024bde795b42c7b54e1139e Author: Vladyslav Usenko Date: Sun Apr 14 21:07:42 2019 +0200 Initial commit diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..dc0134c --- /dev/null +++ b/.clang-format @@ -0,0 +1,6 @@ +--- +Language: Cpp +BasedOnStyle: Google +IndentWidth: 2 +... + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..51704a2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +cmake-build* +.idea +CMakeLists.txt.user +build +thirdparty/build-* +scripts/eval/eval_* +scripts/eval_full/eval_* diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..d38d87c --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,76 @@ +image: vladyslavusenko/b_image:latest + +compile: + stage: build + variables: + CXX_MARCH: 'corei7-avx' + before_script: + - mkdir -p ccache + - export CCACHE_BASEDIR=${PWD} + - export CCACHE_DIR=${PWD}/ccache + cache: + paths: + - ccache/ + script: + - ./scripts/update_submodules.sh + - mkdir build + - cd build + - cmake .. -DCMAKE_BUILD_TYPE=Release -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=/usr/ + - make -j3 + - make package + - cd test/ + - ctest -V + - cd ../../ + - mkdir deb_bionic + - cp build/*.deb deb_bionic/ + artifacts: + paths: + - deb_bionic/*.deb + expire_in: 1 week + +compile_16_04: + stage: build + before_script: + - mkdir -p ccache + - export CCACHE_BASEDIR=${PWD} + - export CCACHE_DIR=${PWD}/ccache + cache: + paths: + - ccache/ + script: + - ./scripts/update_submodules.sh + - mkdir build + - cd build + - cmake .. -DCMAKE_BUILD_TYPE=Release -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=/usr/ + - make -j3 + - make package + - cd test/ + - ctest -V + - cd ../../ + - mkdir deb_xenial + - cp build/*.deb deb_xenial/ + artifacts: + paths: + - deb_xenial/*.deb + expire_in: 1 week + only: + - master + image: vladyslavusenko/b_image_xenial:latest + +deploy: + stage: deploy + before_script: + - eval $(ssh-agent -s) + - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null + - mkdir -p ~/.ssh + - chmod 700 ~/.ssh + - echo "$SSH_KNOWN_HOSTS" > ~/.ssh/known_hosts + - chmod 644 ~/.ssh/known_hosts + variables: + GIT_STRATEGY: none + only: + - master + script: + - scp deb_xenial/*.deb $REPOSITORY_URL/xenial/ + - scp deb_bionic/*.deb $REPOSITORY_URL/bionic/ + diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..b1d0363 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,21 @@ +[submodule "thirdparty/CLI11"] + path = thirdparty/CLI11 + url = https://github.com/CLIUtils/CLI11.git +[submodule "thirdparty/opengv"] + path = thirdparty/opengv + url = https://github.com/laurentkneip/opengv.git +[submodule "thirdparty/Pangolin"] + path = thirdparty/Pangolin + url = https://github.com/stevenlovegrove/Pangolin.git +[submodule "thirdparty/ros_comm"] + path = thirdparty/ros/ros_comm + url = https://github.com/ros/ros_comm.git +[submodule "thirdparty/roscpp_core"] + path = thirdparty/ros/roscpp_core + url = https://github.com/ros/roscpp_core.git +[submodule "thirdparty/console_bridge"] + path = thirdparty/ros/console_bridge + url = https://github.com/ros/console_bridge.git +[submodule "thirdparty/basalt-headers"] + path = thirdparty/basalt-headers + url = https://gitlab.com/VladyslavUsenko/basalt-headers.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5bbdc53 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,210 @@ +cmake_minimum_required(VERSION 3.8) +project(basalt) + +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/" ${CMAKE_MODULE_PATH}) + +set(EIGEN_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/basalt-headers/thirdparty/eigen") + +string(TOLOWER "${PROJECT_NAME}" PROJECT_NAME_LOWERCASE) +find_program(DPKG_PROGRAM dpkg DOC "dpkg program of Debian-based systems") +if(DPKG_PROGRAM) + execute_process( + COMMAND ${DPKG_PROGRAM} --print-architecture + OUTPUT_VARIABLE CPACK_DEBIAN_PACKAGE_ARCHITECTURE + OUTPUT_STRIP_TRAILING_WHITESPACE + ) +endif(DPKG_PROGRAM) + + +find_program(LSB_RELEASE_PROGRAM lsb_release DOC "lsb_release program of Debian-based systems") +if(LSB_RELEASE_PROGRAM) +execute_process(COMMAND ${LSB_RELEASE_PROGRAM} -rs + OUTPUT_VARIABLE LSB_RELEASE_ID_SHORT + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +if(${LSB_RELEASE_ID_SHORT} EQUAL "18.04") + set(DEBIAN_DEPENDS "libtbb2, liblz4-1, libbz2-1.0, libboost-filesystem1.65.1, libboost-date-time1.65.1, libboost-program-options1.65.1, libboost-regex1.65.1, libopencv-dev, libglew2.0, libjpeg8, libpng16-16") +elseif(${LSB_RELEASE_ID_SHORT} EQUAL "16.04") + set(DEBIAN_DEPENDS "libtbb2, liblz4-1, libbz2-1.0, libboost-filesystem1.58.0, libboost-date-time1.58.0, libboost-program-options1.58.0, libboost-regex1.58.0, libopencv-dev, libglew1.13, libjpeg8, libpng12-0") +endif(${LSB_RELEASE_ID_SHORT} EQUAL "18.04") + +endif(LSB_RELEASE_PROGRAM) + +string(TIMESTAMP PROJECT_VERSION_REVISION "%Y%m%d%H%M") + +SET(CPACK_GENERATOR "DEB") +SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Vladyslav Usenko ") +SET(CPACK_PACKAGE_VERSION_MAJOR "0") +SET(CPACK_PACKAGE_VERSION_MINOR "1") +SET(CPACK_PACKAGE_VERSION_PATCH "0-${PROJECT_VERSION_REVISION}~${LSB_RELEASE_ID_SHORT}") +SET(CPACK_DEBIAN_PACKAGE_DEPENDS ${DEBIAN_DEPENDS}) +SET(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME_LOWERCASE}_${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}_${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}") +SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +include(CPack) + + +# Configure CCache if available +find_program(CCACHE_PROGRAM ccache) +if(CCACHE_PROGRAM) + message(STATUS "Found ccache: ${CCACHE_PROGRAM}") + set(CMAKE_C_COMPILER_LAUNCHER ${CCACHE_PROGRAM}) + set(CMAKE_CXX_COMPILER_LAUNCHER ${CCACHE_PROGRAM}) +endif(CCACHE_PROGRAM) + +IF( NOT CMAKE_BUILD_TYPE ) + SET( CMAKE_BUILD_TYPE Release) +ENDIF() + +IF(NOT CXX_MARCH) + SET(CXX_MARCH native) +ENDIF() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Flags used for CHECK_CXX_SOURCE_COMPILES +set(CMAKE_REQUIRED_FLAGS "-Wno-unused-variable -Wno-unused-value") + + +IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") + SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -ftree-vectorize -march=${CXX_MARCH}") + SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -ftree-vectorize -march=${CXX_MARCH}") + SET(CMAKE_CXX_FLAGS "-Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics -fopenmp") +ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang") + SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") + SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -ftree-vectorize -march=${CXX_MARCH}") + SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -ftree-vectorize -march=${CXX_MARCH}") + SET(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-deprecated-register -Wno-deprecated-declarations -Wno-sign-compare -Wno-exceptions -Wno-missing-field-initializers -Wno-unused-parameter -Wno-unused-private-field -Qunused-arguments -fcolor-diagnostics -nostdinc++") + include_directories(/usr/local/opt/llvm/include/c++/v1) + link_directories(/usr/local/opt/llvm/lib) + SET(STD_CXX_FS c++fs) +ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") + SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") + SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -ftree-vectorize -march=${CXX_MARCH}") + SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -ftree-vectorize -march=${CXX_MARCH}") + SET(CMAKE_CXX_FLAGS "-Wall -Werror -Wextra -std=c++11 -Wno-misleading-indentation -Wno-sign-compare -Wno-maybe-uninitialized -Wno-int-in-bool-context -Wno-implicit-fallthrough -Wno-unused-parameter -Wno-deprecated-declarations -ftemplate-backtrace-limit=0 -fopenmp") + SET(STD_CXX_FS stdc++fs) +ENDIF() + +set(EIGEN_INCLUDE_DIR_HINTS ${EIGEN_ROOT}) +find_package(Eigen3 3.3.7 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) +message(STATUS "Found Eigen headers in: ${EIGEN3_INCLUDE_DIR}") + +find_package(TBB REQUIRED) +include_directories(${TBB_INCLUDE_DIR}) + +find_package(OpenCV REQUIRED core imgproc calib3d) +include_directories(${OpenCV_INCLUDE_DIR}) +message(STATUS "Found OpenCV headers in: ${OpenCV_INCLUDE_DIR}") +message(STATUS "Found OpenCV_LIBS: ${OpenCV_LIBS}") + +add_subdirectory(thirdparty/ros) +add_subdirectory(thirdparty/apriltag) +add_subdirectory(thirdparty/DBoW3) + + +set(BUILD_SHARED_LIBS OFF CACHE BOOL "Enable BUILD_SHARED_LIBS") +set(BUILD_TESTS OFF CACHE BOOL "Enable BUILD_TESTS") + +set(BUILD_PANGOLIN_LIBOPENEXR OFF CACHE BOOL "Enable BUILD_PANGOLIN_LIBOPENEXR") +set(BUILD_PANGOLIN_PYTHON OFF CACHE BOOL "Enable BUILD_PANGOLIN_PYTHON") +set(BUILD_EXAMPLES OFF CACHE BOOL "Enable BUILD_EXAMPLES") +add_subdirectory(thirdparty/opengv EXCLUDE_FROM_ALL) + +# Hack to disable CPack in Pangolin. +macro(include) + if(NOT ${ARGV0} STREQUAL "CPack") + _include(${ARGN}) + endif() +endmacro() +add_subdirectory(thirdparty/Pangolin EXCLUDE_FROM_ALL) + +include_directories(thirdparty/basalt-headers/thirdparty/Sophus) +include_directories(thirdparty/basalt-headers/thirdparty/cereal/include) +include_directories(thirdparty/basalt-headers/include) +include_directories(thirdparty/CLI11/include) +include_directories(thirdparty/fast/include) +include_directories(thirdparty/DBoW3/src/) + +include_directories(include) + + +add_library(basalt SHARED + src/io/dataset_io.cpp + src/io/marg_data_io.cpp + src/calibration/cam_calib.cpp + src/calibration/cam_imu_calib.cpp + src/calibration/calibraiton_helper.cpp + src/calibration/vignette.cpp + src/utils/vio_config.cpp + src/optical_flow/optical_flow.cpp + src/vi_estimator/keypoint_vio.cpp + src/vi_estimator/keypoint_vio_linearize.cpp + src/vi_estimator/vio_estimator.cpp + src/vi_estimator/ba_base.cpp + src/vi_estimator/nfr_mapper.cpp + src/utils/keypoints.cpp) + + +target_link_libraries(basalt rosbag pangolin apriltag ${OPENGV_LIBS} ${TBB_LIBRARIES} ${OpenCV_LIBS} opengv ${STD_CXX_FS} DBoW3) + + +add_executable(basalt_calibrate src/calibrate.cpp) +target_link_libraries(basalt_calibrate ${Pangolin_LIBRARIES} apriltag ${OPENGV_LIBS} ${TBB_LIBRARIES} ${rosbag_LIBRARIES} basalt) + +add_executable(basalt_calibrate_imu src/calibrate_imu.cpp) +target_link_libraries(basalt_calibrate_imu ${Pangolin_LIBRARIES} apriltag ${OPENGV_LIBS} ${TBB_LIBRARIES} ${rosbag_LIBRARIES} basalt) + + +add_executable(basalt_vio_sim src/vio_sim.cpp ) +target_link_libraries(basalt_vio_sim ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} ${TBB_LIBRARIES} basalt) + +add_executable(basalt_mapper_sim src/mapper_sim.cpp ) +target_link_libraries(basalt_mapper_sim ${Pangolin_LIBRARIES} opengv ${TBB_LIBRARIES} basalt) + +add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp) +target_link_libraries(basalt_mapper_sim_naive ${Pangolin_LIBRARIES} opengv ${TBB_LIBRARIES} basalt) + +add_executable(basalt_mapper src/mapper.cpp) +target_link_libraries(basalt_mapper ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} ${TBB_LIBRARIES} basalt) + + +add_executable(basalt_opt_flow src/opt_flow.cpp) +target_link_libraries(basalt_opt_flow ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} basalt) + +add_executable(basalt_vio src/vio.cpp) +target_link_libraries(basalt_vio ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} basalt) + + + +install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper_sim_naive basalt_mapper basalt_opt_flow basalt_vio basalt + EXPORT BasaltTargets + RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin + LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib + ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib +) + +file(GLOB CONFIG_FILES "${CMAKE_CURRENT_SOURCE_DIR}/data/*.json") +INSTALL(FILES ${CONFIG_FILES} ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/DBoW3/vocab/orbvoc.dbow3 + DESTINATION ${CMAKE_INSTALL_PREFIX}/etc/basalt +) + + +# Replace install() to do-nothing macro. +macro(install) +endmacro() +# Include subproject (or any other CMake code) with "disabled" install(). +add_subdirectory(thirdparty/basalt-headers/test) +add_subdirectory(test) +# Restore original install() behavior. +macro(install) + _install(${ARGN}) +endmacro() + + + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..03fd6f3 --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..8dc7987 --- /dev/null +++ b/README.md @@ -0,0 +1,62 @@ +## Basalt +For more information see https://vision.in.tum.de/research/vslam/basalt + +![teaser](doc/img/teaser.png) + +This project contains tools for: +* Camera, IMU and motion capture calibration. +* Visual-inertial odometry and mapping. +* Simulated environment to test different components of the system. + + +## Related Publications +Visual-Inertial Odometry and Mapping: +* **Visual-Inertial Mapping with Non-Linear Factor Recovery**, V. Usenko, N. Demmel, D. Schubert, J. Stückler, D. Cremers, In [[arXiv:]](https://arxiv.org/abs/). + +Calibration (explains implemented camera models): +* **The Double Sphere Camera Model**, V. Usenko and N. Demmel and D. Cremers, In 2018 International Conference on 3D Vision (3DV), [[DOI:10.1109/3DV.2018.00069]](https://doi.org/10.1109/3DV.2018.00069), [[arXiv:1807.08957]](https://arxiv.org/abs/1807.08957). + +Calibration (demonstrates how these tools can be used for dataset calibration): +* **The TUM VI Benchmark for Evaluating Visual-Inertial Odometry**, D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, D. Cremers, In 2018 International Conference on Intelligent Robots and Systems (IROS), [[DOI:10.1109/IROS.2018.8593419]](https://doi.org/10.1109/IROS.2018.8593419), [[arXiv:1804.06120]](https://arxiv.org/abs/1804.06120). + + +## Installation +### APT installation for Ubuntu 16.04 and 18.04 (Fast) +Set up keys +``` +sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 0D97B6C9 +``` +Add the repository to the sources list. On **Ubuntu 18.04** run: +``` +sudo sh -c 'echo "deb [arch=amd64] http://packages.usenko.eu/ubuntu bionic main" > /etc/apt/sources.list.d/basalt.list' +``` +On **Ubuntu 16.04** run: +``` +sudo sh -c 'echo "deb [arch=amd64] http://packages.usenko.eu/ubuntu xenial main" > /etc/apt/sources.list.d/basalt.list' +``` +Update the Ubuntu package index and install Basalt: +``` +sudo apt-get update +sudo apt-get install basalt +``` +### Source installation for Ubuntu 18.04 and MacOS 10.14 Mojave +Clone the source code for the project +``` +git clone --recursive https://gitlab.com/VladyslavUsenko/basalt.git +cd basalt +./scripts/install_deps.sh +mkdir build +cd build +cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo +make -j8 +``` +NOTE: It is possible to compile the code on Ubuntu 16.04, but you need to install cmake-3.10 and gcc-7. See corresponding [Dockerfile](docker/b_image_xenial/Dockerfile) as an example. + +## Usage +* [Camera, IMU and Mocap calibration.](doc/Calibration.md) +* [Visual-inertial odometry and mapping.](doc/VioMapping.md) +* [Simulation tools to test different components of the system.](doc/Simulation.md) + +## Licence +The code for this practical course is provided under a BSD 3-clause license. See the LICENSE file for details. +Note also the different licenses of thirdparty submodules. diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake new file mode 100644 index 0000000..9c44084 --- /dev/null +++ b/cmake_modules/FindEigen3.cmake @@ -0,0 +1,83 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + HINTS ${EIGEN_INCLUDE_DIR_HINTS} + PATHS + /usr/local/include + /opt/local/include + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/cmake_modules/FindOpenGV.cmake b/cmake_modules/FindOpenGV.cmake new file mode 100644 index 0000000..ad244c6 --- /dev/null +++ b/cmake_modules/FindOpenGV.cmake @@ -0,0 +1,85 @@ + +# This is FindOPENGV.cmake +# CMake module to locate the OPENGV package +# +# The following cache variables may be set before calling this script: +# +# OPENGV_DIR (or OPENGV_ROOT): (Optional) The install prefix OR source tree of opengv (e.g. /usr/local or src/opengv) +# OPENGV_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory +# within it (e.g build-debug). Without this defined, this script tries to +# intelligently find the build directory based on the project's build directory name +# or based on the build type (Debug/Release/etc). +# +# The following variables will be defined: +# +# OPENGV_FOUND : TRUE if the package has been successfully found +# OPENGV_INCLUDE_DIR : paths to OPENGV's INCLUDE directories +# OPENGV_LIBS : paths to OPENGV's libraries +# +# NOTES on compiling against an uninstalled OPENGV build tree: +# - A OPENGV source tree will be automatically searched for in the directory +# 'opengv' next to your project directory, after searching +# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. +# - The build directory will be searched first with the same name as your +# project's build directory, e.g. if you build from 'MyProject/build-optimized', +# 'opengv/build-optimized' will be searched first. Next, a build directory for +# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is +# 'Release', then 'opengv/build-release' will be searched next. Finally, plain +# 'opengv/build' will be searched. +# - You can control the opengv build directory name directly by defining the CMake +# cache variable 'OPENGV_BUILD_NAME', then only 'opengv/${OPENGV_BUILD_NAME} will +# be searched. +# - Use the standard CMAKE_PREFIX_PATH, or OPENGV_DIR, to find a specific opengv +# directory. + +# Get path suffixes to help look for opengv +if(OPENGV_BUILD_NAME) + set(opengv_build_names "${OPENGV_BUILD_NAME}/opengv") +else() + # lowercase build type + string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) + # build suffix of this project + get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) + + set(opengv_build_names "${my_build_name}/opengv" "build-${build_type_suffix}/opengv" "build/opengv" "build/lib") +endif() + +# Use OPENGV_ROOT or OPENGV_DIR equivalently +if(OPENGV_ROOT AND NOT OPENGV_DIR) + set(OPENGV_DIR "${OPENGV_ROOT}") +endif() + +if(OPENGV_DIR) + # Find include dirs + find_path(OPENGV_INCLUDE_DIR opengv/types.hpp + PATHS "${OPENGV_DIR}/include" "${OPENGV_DIR}" NO_DEFAULT_PATH + DOC "OPENGV include directories") + + # Find libraries + find_library(OPENGV_LIBS NAMES opengv + HINTS "${OPENGV_DIR}/lib" "${OPENGV_DIR}" NO_DEFAULT_PATH + PATH_SUFFIXES ${opengv_build_names} + DOC "OPENGV libraries") +else() + # Find include dirs + set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../opengv" /usr/local/include /usr/include) + find_path(OPENGV_INCLUDE_DIR opengv/types.hpp + PATHS ${extra_include_paths} + DOC "OPENGV include directories") + if(NOT OPENGV_INCLUDE_DIR) + message(STATUS "Searched for opengv headers in default paths plus ${extra_include_paths}") + endif() + + # Find libraries + find_library(OPENGV_LIBS NAMES opengv + HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../opengv" /usr/local/lib /usr/lib + PATH_SUFFIXES ${opengv_build_names} + DOC "OPENGV libraries") +endif() + +# handle the QUIETLY and REQUIRED arguments and set OPENGV_FOUND to TRUE +# if all listed variables are TRUE +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OPENGV DEFAULT_MSG + OPENGV_LIBS OPENGV_INCLUDE_DIR) + diff --git a/cmake_modules/FindTBB.cmake b/cmake_modules/FindTBB.cmake new file mode 100644 index 0000000..f9e3e0f --- /dev/null +++ b/cmake_modules/FindTBB.cmake @@ -0,0 +1,283 @@ +# Locate Intel Threading Building Blocks include paths and libraries +# FindTBB.cmake can be found at https://code.google.com/p/findtbb/ +# Written by Hannes Hofmann +# Improvements by Gino van den Bergen , +# Florian Uhlig , +# Jiri Marsik + +# The MIT License +# +# Copyright (c) 2011 Hannes Hofmann +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler. +# e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21" +# TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found +# in the TBB installation directory (TBB_INSTALL_DIR). +# +# GvdB: Mac OS X distribution places libraries directly in lib directory. +# +# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER. +# TBB_ARCHITECTURE [ ia32 | em64t | itanium ] +# which architecture to use +# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9 +# which compiler to use (detected automatically on Windows) + +# This module respects +# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR} + +# This module defines +# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc. +# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc +# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug +# TBB_INSTALL_DIR, the base TBB install directory +# TBB_LIBRARIES, the libraries to link against to use TBB. +# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols. +# TBB_FOUND, If false, don't try to use TBB. +# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h + + +if (WIN32) + # has em64t/vc8 em64t/vc9 + # has ia32/vc7.1 ia32/vc8 ia32/vc9 + set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB" "C:/Program Files (x86)/Intel/TBB") + set(_TBB_LIB_NAME "tbb") + set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") + set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") + set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") + if (MSVC71) + set (_TBB_COMPILER "vc7.1") + endif(MSVC71) + if (MSVC80) + set(_TBB_COMPILER "vc8") + endif(MSVC80) + if (MSVC90) + set(_TBB_COMPILER "vc9") + endif(MSVC90) + if(MSVC10) + set(_TBB_COMPILER "vc10") + endif(MSVC10) + # Todo: add other Windows compilers such as ICL. + set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) +endif (WIN32) + +if (UNIX) + if (APPLE) + # MAC + set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions") + # libs: libtbb.dylib, libtbbmalloc.dylib, *_debug + set(_TBB_LIB_NAME "tbb") + set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") + set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") + set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") + # default flavor on apple: ia32/cc4.0.1_os10.4.9 + # Jiri: There is no reason to presume there is only one flavor and + # that user's setting of variables should be ignored. + if(NOT TBB_COMPILER) + set(_TBB_COMPILER "cc4.0.1_os10.4.9") + elseif (NOT TBB_COMPILER) + set(_TBB_COMPILER ${TBB_COMPILER}) + endif(NOT TBB_COMPILER) + if(NOT TBB_ARCHITECTURE) + set(_TBB_ARCHITECTURE "ia32") + elseif(NOT TBB_ARCHITECTURE) + set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) + endif(NOT TBB_ARCHITECTURE) + else (APPLE) + # LINUX + set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include") + set(_TBB_LIB_NAME "tbb") + set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") + set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") + set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") + # has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21 + # has ia32/* + # has itanium/* + set(_TBB_COMPILER ${TBB_COMPILER}) + set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) + endif (APPLE) +endif (UNIX) + +if (CMAKE_SYSTEM MATCHES "SunOS.*") +# SUN +# not yet supported +# has em64t/cc3.4.3_kernel5.10 +# has ia32/* +endif (CMAKE_SYSTEM MATCHES "SunOS.*") + + +#-- Clear the public variables +set (TBB_FOUND "NO") + + +#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR} +# first: use CMake variable TBB_INSTALL_DIR +if (TBB_INSTALL_DIR) + set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR}) +endif (TBB_INSTALL_DIR) +# second: use environment variable +if (NOT _TBB_INSTALL_DIR) + if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR}) + endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "") + # Intel recommends setting TBB21_INSTALL_DIR + if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR}) + endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "") + if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR}) + endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "") + if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR}) + endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "") +endif (NOT _TBB_INSTALL_DIR) +# third: try to find path automatically +if (NOT _TBB_INSTALL_DIR) + if (_TBB_DEFAULT_INSTALL_DIR) + set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR}) + endif (_TBB_DEFAULT_INSTALL_DIR) +endif (NOT _TBB_INSTALL_DIR) +# sanity check +if (NOT _TBB_INSTALL_DIR) + message ("ERROR: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}") +else (NOT _TBB_INSTALL_DIR) +# finally: set the cached CMake variable TBB_INSTALL_DIR +if (NOT TBB_INSTALL_DIR) + set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory") + mark_as_advanced(TBB_INSTALL_DIR) +endif (NOT TBB_INSTALL_DIR) + + +#-- A macro to rewrite the paths of the library. This is necessary, because +# find_library() always found the em64t/vc9 version of the TBB libs +macro(TBB_CORRECT_LIB_DIR var_name) +# if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t") + string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}}) +# endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t") + string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}}) + string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) + string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) + string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) + string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) +endmacro(TBB_CORRECT_LIB_DIR var_content) + + +#-- Look for include directory and set ${TBB_INCLUDE_DIR} +set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include) +# Jiri: tbbvars now sets the CPATH environment variable to the directory +# containing the headers. +find_path(TBB_INCLUDE_DIR + tbb/task_scheduler_init.h + PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH +) +mark_as_advanced(TBB_INCLUDE_DIR) + + +#-- Look for libraries +# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh] +if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "") + set (_TBB_LIBRARY_DIR + ${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM} + ${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib + ) +endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "") +# Jiri: This block isn't mutually exclusive with the previous one +# (hence no else), instead I test if the user really specified +# the variables in question. +if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL "")) + # HH: deprecated + message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).") + # Jiri: It doesn't hurt to look in more places, so I store the hints from + # ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER + # variables and search them both. + set (_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}) +endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL "")) + +# GvdB: Mac OS X distribution places libraries directly in lib directory. +list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib) + +# Jiri: No reason not to check the default paths. From recent versions, +# tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH +# variables, which now point to the directories of the lib files. +# It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS +# argument instead of the implicit PATHS as it isn't hard-coded +# but computed by system introspection. Searching the LIBRARY_PATH +# and LD_LIBRARY_PATH environment variables is now even more important +# that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates +# the use of TBB built from sources. +find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) +find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) + +#Extract path from TBB_LIBRARY name +get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH) + +#TBB_CORRECT_LIB_DIR(TBB_LIBRARY) +#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY) +mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY) + +#-- Look for debug libraries +# Jiri: Changed the same way as for the release libraries. +find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) +find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) + +# Jiri: Self-built TBB stores the debug libraries in a separate directory. +# Extract path from TBB_LIBRARY_DEBUG name +get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH) + +#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG) +#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG) +mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG) + + +if (TBB_INCLUDE_DIR) + if (TBB_LIBRARY) + set (TBB_FOUND "YES") + set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES}) + set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES}) + set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE) + set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE) + # Jiri: Self-built TBB stores the debug libraries in a separate directory. + set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE) + mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES) + message(STATUS "Found Intel TBB") + endif (TBB_LIBRARY) +endif (TBB_INCLUDE_DIR) + +if (NOT TBB_FOUND) + message("ERROR: Intel TBB NOT found!") + message(STATUS "Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}") + # do only throw fatal, if this pkg is REQUIRED + if (TBB_FIND_REQUIRED) + message(FATAL_ERROR "Could NOT find TBB library.") + endif (TBB_FIND_REQUIRED) +endif (NOT TBB_FOUND) + +endif (NOT _TBB_INSTALL_DIR) + +if (TBB_FOUND) + set(TBB_INTERFACE_VERSION 0) + FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS) + STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}") + set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}") +endif (TBB_FOUND) diff --git a/data/euroc_config.json b/data/euroc_config.json new file mode 100644 index 0000000..22949a6 --- /dev/null +++ b/data/euroc_config.json @@ -0,0 +1,30 @@ +{ + "value0": { + "config.optical_flow_type": "frame_to_frame", + "config.optical_flow_detection_grid_size": 50, + "config.optical_flow_max_recovered_dist2": 0.04, + "config.optical_flow_pattern": 51, + "config.optical_flow_max_iterations": 5, + "config.optical_flow_epipolar_error": 0.005, + "config.optical_flow_levels": 3, + "config.vio_max_states": 3, + "config.vio_max_kfs": 7, + "config.vio_min_frames_after_kf": 5, + "config.vio_new_kf_keypoints_thresh": 0.7, + "config.vio_max_iterations": 5, + "config.vio_debug": false, + "config.vio_obs_std_dev": 0.5, + "config.vio_obs_huber_thresh": 1.0, + + "config.mapper_obs_std_dev": 0.25, + "config.mapper_obs_huber_thresh": 1.5, + "config.mapper_detection_num_points": 800, + "config.mapper_num_frames_to_match": 30, + "config.mapper_frames_to_match_threshold": 0.3, + "config.mapper_min_matches": 20, + "config.mapper_ransac_threshold": 5e-5, + "config.mapper_min_track_length": 5, + "config.mapper_max_hamming_distance": 70, + "config.mapper_second_best_test_ratio": 1.2 + } +} diff --git a/data/euroc_ds_calib.json b/data/euroc_ds_calib.json new file mode 100644 index 0000000..83e1872 --- /dev/null +++ b/data/euroc_ds_calib.json @@ -0,0 +1,213 @@ +{ + "value0": { + "T_imu_cam": [ + { + "px": -0.016774788924641534, + "py": -0.068938940687127, + "pz": 0.005139123188382424, + "qx": -0.007239825785317818, + "qy": 0.007541278561558601, + "qz": 0.7017845426564943, + "qw": 0.7123125505904486 + }, + { + "px": -0.01507436282032619, + "py": 0.0412627204046637, + "pz": 0.00316287258752953, + "qx": -0.0023360576185881625, + "qy": 0.013000769689092388, + "qz": 0.7024677108343111, + "qw": 0.7115930283929829 + } + ], + "intrinsics": [ + { + "camera_type": "ds", + "intrinsics": { + "fx": 349.7560023050409, + "fy": 348.72454229977037, + "cx": 365.89440762590149, + "cy": 249.32995565708704, + "xi": -0.2409573942178872, + "alpha": 0.566996899163044 + } + }, + { + "camera_type": "ds", + "intrinsics": { + "fx": 361.6713883800533, + "fy": 360.5856493689301, + "cx": 379.40818394080869, + "cy": 255.9772968522045, + "xi": -0.21300835384809328, + "alpha": 0.5767008625037023 + } + } + ], + "resolution": [ + [ + 752, + 480 + ], + [ + 752, + 480 + ] + ], + "vignette": [ + { + "value0": 0, + "value1": 50000000000, + "value2": [ + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 0.977014799737916 + ], + [ + 0.963099657065011 + ], + [ + 0.922192868484681 + ], + [ + 0.854947649264748 + ], + [ + 0.775979681731632 + ], + [ + 0.722888431826212 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ] + ] + }, + { + "value0": 0, + "value1": 50000000000, + "value2": [ + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 0.977014799737916 + ], + [ + 0.963099657065011 + ], + [ + 0.922192868484681 + ], + [ + 0.854947649264748 + ], + [ + 0.775979681731632 + ], + [ + 0.722888431826212 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ] + ] + } + ], + "calib_accel_bias": [ + 0.001725405479279035, + 0.1500005286487319, + 0.06708820471592454, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "calib_gyro_bias": [ + -0.002186848441668376, + 0.024427823167917037, + 0.07668367023977922, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "imu_update_rate": 200.0, + "accel_noise_std": 0.016, + "gyro_noise_std": 0.000282, + "accel_bias_std": 0.001, + "gyro_bias_std": 0.0001, + "T_mocap_world": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "T_imu_marker": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "mocap_time_offset_ns": 0, + "mocap_to_imu_offset_ns": 140763258159875, + "cam_time_offset_ns": 0 + } +} diff --git a/data/euroc_eucm_calib.json b/data/euroc_eucm_calib.json new file mode 100644 index 0000000..9973a89 --- /dev/null +++ b/data/euroc_eucm_calib.json @@ -0,0 +1,213 @@ +{ + "value0": { + "T_imu_cam": [ + { + "px": -0.016774788924641534, + "py": -0.068938940687127, + "pz": 0.005139123188382424, + "qx": -0.007239825785317818, + "qy": 0.007541278561558601, + "qz": 0.7017845426564943, + "qw": 0.7123125505904486 + }, + { + "px": -0.01507436282032619, + "py": 0.0412627204046637, + "pz": 0.00316287258752953, + "qx": -0.0023360576185881625, + "qy": 0.013000769689092388, + "qz": 0.7024677108343111, + "qw": 0.7115930283929829 + } + ], + "intrinsics": [ + { + "camera_type": "eucm", + "intrinsics": { + "fx": 460.76484651566468, + "fy": 459.4051018049483, + "cx": 365.8937161309615, + "cy": 249.33499869752445, + "alpha": 0.5903365915227143, + "beta": 1.127468196965374 + } + }, + { + "camera_type": "eucm", + "intrinsics": { + "fx": 459.55216904505178, + "fy": 458.17181312352059, + "cx": 379.4066773637502, + "cy": 255.98301446219285, + "alpha": 0.6049889282227827, + "beta": 1.0907289821146678 + } + } + ], + "resolution": [ + [ + 752, + 480 + ], + [ + 752, + 480 + ] + ], + "vignette": [ + { + "value0": 0, + "value1": 50000000000, + "value2": [ + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 0.977014799737916 + ], + [ + 0.963099657065011 + ], + [ + 0.922192868484681 + ], + [ + 0.854947649264748 + ], + [ + 0.775979681731632 + ], + [ + 0.722888431826212 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ] + ] + }, + { + "value0": 0, + "value1": 50000000000, + "value2": [ + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 1.0 + ], + [ + 0.977014799737916 + ], + [ + 0.963099657065011 + ], + [ + 0.922192868484681 + ], + [ + 0.854947649264748 + ], + [ + 0.775979681731632 + ], + [ + 0.722888431826212 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ], + [ + 0.0686113338332548 + ] + ] + } + ], + "calib_accel_bias": [ + 0.001725405479279035, + 0.1500005286487319, + 0.06708820471592454, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "calib_gyro_bias": [ + -0.002186848441668376, + 0.024427823167917037, + 0.07668367023977922, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "imu_update_rate": 200.0, + "accel_noise_std": 0.016, + "gyro_noise_std": 0.000282, + "accel_bias_std": 0.001, + "gyro_bias_std": 0.0001, + "T_mocap_world": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "T_imu_marker": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "mocap_time_offset_ns": 0, + "mocap_to_imu_offset_ns": 140763258159875, + "cam_time_offset_ns": 0 + } +} diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json new file mode 100644 index 0000000..5f189c4 --- /dev/null +++ b/data/tumvi_512_config.json @@ -0,0 +1,30 @@ +{ + "value0": { + "config.optical_flow_type": "frame_to_frame", + "config.optical_flow_detection_grid_size": 40, + "config.optical_flow_max_recovered_dist2": 0.04, + "config.optical_flow_pattern": 51, + "config.optical_flow_max_iterations": 5, + "config.optical_flow_epipolar_error": 0.005, + "config.optical_flow_levels": 3, + "config.vio_max_states": 3, + "config.vio_max_kfs": 7, + "config.vio_min_frames_after_kf": 5, + "config.vio_new_kf_keypoints_thresh": 0.7, + "config.vio_max_iterations": 5, + "config.vio_debug": false, + "config.vio_obs_std_dev": 0.5, + "config.vio_obs_huber_thresh": 1.0, + + "config.mapper_obs_std_dev": 0.25, + "config.mapper_obs_huber_thresh": 1.5, + "config.mapper_detection_num_points": 800, + "config.mapper_num_frames_to_match": 30, + "config.mapper_frames_to_match_threshold": 0.3, + "config.mapper_min_matches": 20, + "config.mapper_ransac_threshold": 5e-5, + "config.mapper_min_track_length": 5, + "config.mapper_max_hamming_distance": 70, + "config.mapper_second_best_test_ratio": 1.2 + } +} diff --git a/data/tumvi_512_ds_calib.json b/data/tumvi_512_ds_calib.json new file mode 100644 index 0000000..1f5d4b8 --- /dev/null +++ b/data/tumvi_512_ds_calib.json @@ -0,0 +1,381 @@ +{ + "value0": { + "T_imu_cam": [ + { + "px": 0.04548094812071685, + "py": -0.07145370002838907, + "pz": -0.046315428444919249, + "qx": -0.013392900690257393, + "qy": -0.6945866755293793, + "qz": 0.7192437840259219, + "qw": 0.007639340823570553 + }, + { + "px": -0.05546984222234079, + "py": -0.06999334244486549, + "pz": -0.049092582974927929, + "qx": -0.01340980138125811, + "qy": -0.7115668842588793, + "qz": 0.7024477338114514, + "qw": 0.007741299385907546 + } + ], + "intrinsics": [ + { + "camera_type": "ds", + "intrinsics": { + "fx": 158.28600034966977, + "fy": 158.2743455478755, + "cx": 254.96116578191653, + "cy": 256.8894394501779, + "xi": -0.17213086034353243, + "alpha": 0.5931177593944744 + } + }, + { + "camera_type": "ds", + "intrinsics": { + "fx": 157.91830144176309, + "fy": 157.8901286125632, + "cx": 252.56547609702953, + "cy": 255.02489416194656, + "xi": -0.17114780716007858, + "alpha": 0.5925543396658507 + } + } + ], + "resolution": [ + [ + 512, + 512 + ], + [ + 512, + 512 + ] + ], + "calib_accel_bias": [ + -0.0011791549110128743, + -0.007714616665106584, + 0.00006733124206236036, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "calib_gyro_bias": [ + -0.00007403817898158015, + 0.0000024090695654577384, + -0.00001847724206614636, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "imu_update_rate": 200.0, + "accel_noise_std": 0.016, + "gyro_noise_std": 0.000282, + "accel_bias_std": 0.001, + "gyro_bias_std": 0.0001, + "T_mocap_world": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "T_imu_marker": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "mocap_time_offset_ns": 0, + "mocap_to_imu_offset_ns": 0, + "cam_time_offset_ns": 0, + "vignette": [ + { + "value0": 0, + "value1": 10000000000, + "value2": [ + [ + 0.9899372881888493 + ], + [ + 0.9931178795891834 + ], + [ + 0.9984407803536122 + ], + [ + 0.996964178110498 + ], + [ + 0.9956605833036875 + ], + [ + 0.9973489553595134 + ], + [ + 0.9985691891331739 + ], + [ + 1.0 + ], + [ + 0.9966866746094087 + ], + [ + 0.9958706368580655 + ], + [ + 0.9957807702286541 + ], + [ + 0.9927911027519979 + ], + [ + 0.9838733297580375 + ], + [ + 0.9719198145611155 + ], + [ + 0.9566058020047663 + ], + [ + 0.9378032130699772 + ], + [ + 0.9194004905192558 + ], + [ + 0.9024065312353705 + ], + [ + 0.8845584821495792 + ], + [ + 0.8624817202873822 + ], + [ + 0.8449163722596605 + ], + [ + 0.8239771250783265 + ], + [ + 0.802936456716557 + ], + [ + 0.7811632337206126 + ], + [ + 0.758539495055499 + ], + [ + 0.7341063700839284 + ], + [ + 0.7100276770866111 + ], + [ + 0.6874554816035845 + ], + [ + 0.6641863547515791 + ], + [ + 0.6412571843680321 + ], + [ + 0.6228323769487529 + ], + [ + 0.6030940673078892 + ], + [ + 0.5598212055840929 + ], + [ + 0.4377741720108287 + ], + [ + 0.24026566347390075 + ], + [ + 0.08222347033252997 + ], + [ + 0.03489917830551274 + ], + [ + 0.029153547460757366 + ], + [ + 0.030477276736751079 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ] + ] + }, + { + "value0": 0, + "value1": 10000000000, + "value2": [ + [ + 0.9982144512705363 + ], + [ + 0.9961165266369948 + ], + [ + 0.9892083034957775 + ], + [ + 0.9870647327777502 + ], + [ + 0.9863145153017516 + ], + [ + 0.9847218081828388 + ], + [ + 0.9828536767530198 + ], + [ + 0.9808724309018916 + ], + [ + 0.9797280962140972 + ], + [ + 0.9784762156394231 + ], + [ + 0.9771015659455632 + ], + [ + 0.9730709135912151 + ], + [ + 0.9650117828320545 + ], + [ + 0.9512319166574508 + ], + [ + 0.9346333455674182 + ], + [ + 0.9182278021221385 + ], + [ + 0.9025351121100457 + ], + [ + 0.8845788263796353 + ], + [ + 0.8668369304290567 + ], + [ + 0.8469326512989164 + ], + [ + 0.8280417962134851 + ], + [ + 0.8072181976634909 + ], + [ + 0.788496496511515 + ], + [ + 0.7691952807935 + ], + [ + 0.7459299901795546 + ], + [ + 0.7240742868053557 + ], + [ + 0.7009088004583106 + ], + [ + 0.6799474150660547 + ], + [ + 0.6586062723412073 + ], + [ + 0.6406388063878976 + ], + [ + 0.6239758367746359 + ], + [ + 0.6026729560290718 + ], + [ + 0.5455288113795859 + ], + [ + 0.3945979030583888 + ], + [ + 0.1981919324270445 + ], + [ + 0.06754460426558173 + ], + [ + 0.028910567836990938 + ], + [ + 0.02842995279817033 + ], + [ + 0.0299618927515648 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ] + ] + } + ] + } +} diff --git a/data/tumvi_512_eucm_calib.json b/data/tumvi_512_eucm_calib.json new file mode 100644 index 0000000..50010a8 --- /dev/null +++ b/data/tumvi_512_eucm_calib.json @@ -0,0 +1,381 @@ +{ + "value0": { + "T_imu_cam": [ + { + "px": 0.04548094812071685, + "py": -0.07145370002838907, + "pz": -0.046315428444919249, + "qx": -0.013392900690257393, + "qy": -0.6945866755293793, + "qz": 0.7192437840259219, + "qw": 0.007639340823570553 + }, + { + "px": -0.05546984222234079, + "py": -0.06999334244486549, + "pz": -0.049092582974927929, + "qx": -0.01340980138125811, + "qy": -0.7115668842588793, + "qz": 0.7024477338114514, + "qw": 0.007741299385907546 + } + ], + "intrinsics": [ + { + "camera_type": "eucm", + "intrinsics": { + "fx": 191.14799836282189, + "fy": 191.13150963902818, + "cx": 254.9585771534443, + "cy": 256.88154645599448, + "alpha": 0.6291060881178562, + "beta": 1.0418067381860868 + } + }, + { + "camera_type": "eucm", + "intrinsics": { + "fx": 190.47905769226575, + "fy": 190.44567561523216, + "cx": 252.55882115024333, + "cy": 255.02104780344699, + "alpha": 0.6281040684983363, + "beta": 1.041250259119081 + } + } + ], + "resolution": [ + [ + 512, + 512 + ], + [ + 512, + 512 + ] + ], + "calib_accel_bias": [ + -0.0011791549110128743, + -0.007714616665106584, + 0.00006733124206236036, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "calib_gyro_bias": [ + -0.00007403817898158015, + 0.0000024090695654577384, + -0.00001847724206614636, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "imu_update_rate": 200.0, + "accel_noise_std": 0.016, + "gyro_noise_std": 0.000282, + "accel_bias_std": 0.001, + "gyro_bias_std": 0.0001, + "T_mocap_world": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "T_imu_marker": { + "px": 0.0, + "py": 0.0, + "pz": 0.0, + "qx": 0.0, + "qy": 0.0, + "qz": 0.0, + "qw": 1.0 + }, + "mocap_time_offset_ns": 0, + "mocap_to_imu_offset_ns": 0, + "cam_time_offset_ns": 0, + "vignette": [ + { + "value0": 0, + "value1": 10000000000, + "value2": [ + [ + 0.9899372881888493 + ], + [ + 0.9931178795891834 + ], + [ + 0.9984407803536122 + ], + [ + 0.996964178110498 + ], + [ + 0.9956605833036875 + ], + [ + 0.9973489553595134 + ], + [ + 0.9985691891331739 + ], + [ + 1.0 + ], + [ + 0.9966866746094087 + ], + [ + 0.9958706368580655 + ], + [ + 0.9957807702286541 + ], + [ + 0.9927911027519979 + ], + [ + 0.9838733297580375 + ], + [ + 0.9719198145611155 + ], + [ + 0.9566058020047663 + ], + [ + 0.9378032130699772 + ], + [ + 0.9194004905192558 + ], + [ + 0.9024065312353705 + ], + [ + 0.8845584821495792 + ], + [ + 0.8624817202873822 + ], + [ + 0.8449163722596605 + ], + [ + 0.8239771250783265 + ], + [ + 0.802936456716557 + ], + [ + 0.7811632337206126 + ], + [ + 0.758539495055499 + ], + [ + 0.7341063700839284 + ], + [ + 0.7100276770866111 + ], + [ + 0.6874554816035845 + ], + [ + 0.6641863547515791 + ], + [ + 0.6412571843680321 + ], + [ + 0.6228323769487529 + ], + [ + 0.6030940673078892 + ], + [ + 0.5598212055840929 + ], + [ + 0.4377741720108287 + ], + [ + 0.24026566347390075 + ], + [ + 0.08222347033252997 + ], + [ + 0.03489917830551274 + ], + [ + 0.029153547460757366 + ], + [ + 0.030477276736751079 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ] + ] + }, + { + "value0": 0, + "value1": 10000000000, + "value2": [ + [ + 0.9982144512705363 + ], + [ + 0.9961165266369948 + ], + [ + 0.9892083034957775 + ], + [ + 0.9870647327777502 + ], + [ + 0.9863145153017516 + ], + [ + 0.9847218081828388 + ], + [ + 0.9828536767530198 + ], + [ + 0.9808724309018916 + ], + [ + 0.9797280962140972 + ], + [ + 0.9784762156394231 + ], + [ + 0.9771015659455632 + ], + [ + 0.9730709135912151 + ], + [ + 0.9650117828320545 + ], + [ + 0.9512319166574508 + ], + [ + 0.9346333455674182 + ], + [ + 0.9182278021221385 + ], + [ + 0.9025351121100457 + ], + [ + 0.8845788263796353 + ], + [ + 0.8668369304290567 + ], + [ + 0.8469326512989164 + ], + [ + 0.8280417962134851 + ], + [ + 0.8072181976634909 + ], + [ + 0.788496496511515 + ], + [ + 0.7691952807935 + ], + [ + 0.7459299901795546 + ], + [ + 0.7240742868053557 + ], + [ + 0.7009088004583106 + ], + [ + 0.6799474150660547 + ], + [ + 0.6586062723412073 + ], + [ + 0.6406388063878976 + ], + [ + 0.6239758367746359 + ], + [ + 0.6026729560290718 + ], + [ + 0.5455288113795859 + ], + [ + 0.3945979030583888 + ], + [ + 0.1981919324270445 + ], + [ + 0.06754460426558173 + ], + [ + 0.028910567836990938 + ], + [ + 0.02842995279817033 + ], + [ + 0.0299618927515648 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ], + [ + 0.01 + ] + ] + } + ] + } +} diff --git a/doc/Calibration.md b/doc/Calibration.md new file mode 100644 index 0000000..617e153 --- /dev/null +++ b/doc/Calibration.md @@ -0,0 +1,117 @@ +# Calibration + +Here, we explain how to use the calibration tools with [TUM-VI](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) and [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) datasets as an example. + + +## TUM-VI dataset +Download the datasets for camera and camera-IMU calibration: +``` +mkdir ~/tumvi_calib_data +cd ~/tumvi_calib_data +wget http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-calib-cam3_512_16.bag +wget http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-calib-imu1_512_16.bag +``` + +### Camera calibration +Run the camera calibration: +``` +basalt_calibrate --dataset-path ~/tumvi_calib_data/dataset-calib-cam3_512_16.bag --dataset-type bag --result-path ~/tumvi_calib_result/ --cam-types ds ds +``` +The command line options have the following meaning: +* `--dataset-path` path to the dataset. +* `--dataset-type` type of the datset. Currently only `bag` and `euroc` formats of the datasets are supported. +* `--result-path` path to the folder where the resulting calibration and intermediate results will be stored. +* `--cam-types` camera models for the image streams in the dataset. For more detais see [arXiv:1807.08957](https://arxiv.org/abs/1807.08957). + +After that, you should see the calibration GUI: +![tumvi_cam_calib](doc/img/tumvi_cam_calib.png) + +The buttons in the GUI are located in the order which you should follow to calibrate the camera: +* `load_dataset` loads the dataset. +* `detect_corners` starts corner detection in the backround thread. Since it is the most time consuming part of the calibration process, the detected corners are cached and loaded if you run the executable again pointing to the same result folder path. +* `init_cam_intr` computes an initial guess for camera intrinsics. +* `init_cam_poses` computes an initial guess for camera poses given the current intrinsics. +* `init_cam_extr` computes an initial transformation between the cameras. +* `init_opt` initializes optimization and shows the projected points given the current calibration and camera poses. +* `optimize` runs an iteration of the optimization and visualizes the result. You should press this button until the error printed in the console output stops decreasing and the optimization converges. +* `save_calib` saves the current calibration as `calibration.json` in the result folder. +* `compute_vign` **(Experimental)** computes a radially-symmetric vignetting for the cameras. For the algorithm to work, **the calibration pattern should be static (and camera moving around it) and have a constant lighting throughout the calibration sequence**. If you run `compute_vign` you should press `save_calib` afterwards. The png images with vignetting will also be stored in the result folder. + +You can also control the process using the following buttons: +* `show_frame` slider to switch between the frames in the sequence. +* `show_corners` toggles the visibility of the detected corners shown in red. +* `show_corners_rejected` toggles the visibility of rejected corners. +* `show_init_reproj` shows the initial reprojections computed by the `init_cam_poses` step. +* `show_opt` shows reprojected corners with the current estimate of the intrinsics and poses. +* `show_vign` toggles the visibility of the points used for vignetting estimation. The points are distributed across white areas of the pattern. +* `show_ids` toggles the ID visualization for every point. +* `huber_thresh` controls the threshold for the huber norm in pixels for the optimization. +* `opt_intr` controls if the optimization can change the intrinsics. For some datasets it might be helpful to disable this option for several first iterations of the optimization. + +### Camera + IMU + Mocap calibration +After calibrating the camera you can run the camera + IMU + Mocap calibration. The result path should point to the **same folder as before**: +``` +basalt_calibrate_imu --dataset-path ~/tumvi_calib_data/dataset-calib-imu1_512_16.bag --dataset-type bag --result-path ~/tumvi_calib_result/ --gyro-noise-std 0.000282 --accel-noise-std 0.016 --gyro-bias-std 0.0001 --accel-bias-std 0.001 +``` +The command line options for the IMU noise are continous-time and defined as in [Kalibr](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model): +* `--gyro-noise-std` gyroscope white noise. +* `--accel-noise-std` accelerometer white noise. +* `--gyro-bias-std` gyroscope random walk. +* `--accel-bias-std` accelerometer random walk. + +![tumvi_imu_calib](doc/img/tumvi_imu_calib.png) + +The buttons in the GUI are located in the order which you need to follow to calibrate the camera-IMU setup: +* `load_dataset`, `detect_corners`, `init_cam_poses` same as above. +* `init_cam_imu` initializes the rotation between camera and IMU by aligning rotation velocities of the camera to the gyro data. +* `init_opt` initializes the optimization. Shows reprojected corners in magenta and the estimated values from the spline as solid lines below. +* `optimize` runs an iteration of the optimization. You should press it several times until convergence before proceeding to next steps. +* `init_mocap` initializes the transformation from the Aprilgrid calibration pattern to the Mocap coordinate system. **Currently we assume that the orientation of the Mocap marker frame is approximately aligned with IMU axes**. +* `save_calib` save the current calibration as `calibration.json` in the result folder. +* `save_mocap_calib` save the current Mocap to IMU calibration as `mocap_calibration.json` in the result folder. + +You can also control the visualization using the following buttons: +* `show_frame` - `show_ids` the same as above. +* `show_spline` toggles the visibility of enabled measurements (accel, gyro, position, velocity) generated from the spline that we optimize. +* `show_data` toggles the visibility of raw data containted in the dataset. +* `show_accel` shows accelerometer data. +* `show_gyro` shows gyroscope data. +* `show_pos` shows the position data. +* `show_mocap` shows the mocap marker position transformed to the IMU frame. +* `show_mocap_rot_error` shows rotation between the spline and Mocap measurements. +* `show_mocap_rot_vel` shows the rotation velocity computed from the Mocap. + +The following options control the optimization process: +* `opt_intr` enables optimization of intrinsics. Usually should be disabled for the camera-IMU calibration. +* `opt_poses` enables optimization based camera pose initialization. Sometimes helps to better initialize the spline before running optimization with `opt_corners`. +* `opt_corners` enables optimization based on reprojection corner positions **(should be used by default)**. +* `opt_cam_time_offset` computes the time offset between camera and the IMU. This option should be used only for refinement when the optimization already converged. +* `opt_imu_scale` enables IMU axis scaling, rotation and mislignment calibration. This option should be used only for refinement when the optimization already converged. +* `opt_mocap` enables Mocap optimization. You should run it only after pressing `init_mocap`. +* `huber_thresh` controls the threshold for the huber norm in pixels for the optimization. + + +**NOTE:** In this case the we use pre-calibrated sequence, so most of refinements or Mocap to IMU calibration will not have any visible effect. If you want to test this functionality use the "raw" sequences, for example `http://vision.in.tum.de/tumvi/raw/dataset-calib-cam3.bag` and `http://vision.in.tum.de/tumvi/raw/dataset-calib-imu1.bag`. + +## EuRoC dataset +Download the datasets for camera and camera-IMU calibration: +``` +mkdir ~/euroc_calib_data +cd ~/euroc_calib_data +wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_datasets/cam_april/cam_april.bag +wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_datasets/imu_april/imu_april.bag +``` + +### Camera calibration +Run the camera calibration: +``` +basalt_calibrate --dataset-path ~/euroc_calib_data/cam_april.bag --dataset-type bag --result-path ~/euroc_calib_result/ --cam-types ds ds +``` +![euroc_cam_calib](doc/img/euroc_cam_calib.png) + +### Camera + IMU calibration +After calibrating the camera you can run the camera + IMU calibration. The result-path should point to the same folder as before: +``` +basalt_calibrate_imu --dataset-path ~/euroc_calib_data/imu_april.bag --dataset-type bag --result-path ~/euroc_calib_result/ --gyro-noise-std 0.000282 --accel-noise-std 0.016 --gyro-bias-std 0.0001 --accel-bias-std 0.001 +``` +![euroc_imu_calib](doc/img/euroc_imu_calib.png) diff --git a/doc/Simulation.md b/doc/Simulation.md new file mode 100644 index 0000000..fa63ad7 --- /dev/null +++ b/doc/Simulation.md @@ -0,0 +1,50 @@ +## Simulator + +For better evaluation of the system we use the simulated environment where the optical flow and IMU data is generated from the ground truth by adding noise. + +**Note:** The path to calibration and configuration files used here works for the APT installation. If you compile from source specify the appropriate path to the files in [data folder](data/). + + +### Visual-inertial odometry simulator +``` +basalt_vio_sim --cam-calib /usr/etc/basalt/euroc_ds_calib.json --marg-data sim_marg_data --show-gui 1 +``` + +The command line options have the following meaning: +* `--cam-calib` path to camera calibration file. Check [calibration instructions](doc/Calibration.md) to see how the calibration was generated. +* `--marg-data` folder where the data from keyframe marginalization will be stored. This data can be later used for visual-inertial mapping simulator. +* `--show-gui` enables or disables GUI. + +This opens the GUI and runs the sequence. +![SIM_VIO](doc/img/SIM_VIO.png) + +The buttons in the GUI have the following meaning: +* `show_obs` toggles the visibility the ground-truth landmarks in the image view. +* `show_noisy` toggles the visibility the noisy landmarks in the image view. +* `show_vio` toggles the visibility the landmarks estimated by VIO in the image view. +* `show_ids` toggles the IDs of the landmarks. +* `show_accel` shows noisy accelerometer measurements generated from ground truth spline. +* `show_gyro` shows noisy gyroscope measurements generated from ground truth spline. +* `show_gt_...` shows ground truth position, velocity and biases. +* `show_est_...` shows VIO estimates of the position, velocity and biases. +* `next_step` proceeds to next frame. +* `continue` plays the sequence. +* `align_button` performs SE(3) alignment with ground-truth trajectory and prints the RMS ATE to the console. + + +### Visual-inertial mapping simulator +``` +basalt_mapper_sim --cam-calib /usr/etc/basalt/euroc_ds_calib.json --marg-data sim_marg_data --show-gui 1 +``` +The command line arguments are the same as above. + +This opens the GUI where the map can be processed. +![SIM_MAPPER](doc/img/SIM_MAPPER.png) + +The system processes the marginalization data and extracts the non-linear factors from them. Roll-pitch and relative-pose factors are initially added to the system. One way to verify that they result in gravity-aligned map is the following +* `optimize` runs the optimization +* `rand_inc` applies a random increment to all frames of the system. If you run the `optimize` until convergence afterwards, and press `align_svd` the alignment transformation should only contain the rotation around Z axis. +* `rand_yaw` applies an increment in yaw to all poses. This should not change the error of the optimization once is have converged. +* `setup_points` triangulates the points and adds them to optimization. You should optimize the system again after adding the points. + +For comparison we also provide the `basalt_mapper_sim_naive` executable that has the same parameters. It runs a global bundle-adjustment on keyframe data and inserts pre-integrated IMU measurements between keyframes. This executable is included for comparison only. \ No newline at end of file diff --git a/doc/VioMapping.md b/doc/VioMapping.md new file mode 100644 index 0000000..3e0c958 --- /dev/null +++ b/doc/VioMapping.md @@ -0,0 +1,86 @@ +## EuRoC dataset + +We demonstrate the usage of the system with the `MH_05_difficult` sequence of the [EuRoC dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) as an example. + +**Note:** The path to calibration and configuration files used here works for the APT installation. If you compile from source specify the appropriate path to the files in [data folder](data/). + +Download the sequence from the dataset and extract it. +``` +mkdir euroc_data +cd euroc_data +wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_05_difficult/MH_05_difficult.zip +mkdir MH_05_difficult +cd MH_05_difficult +unzip ../MH_05_difficult.zip +cd ../ +``` + +### Visual-inertial odometry +To run the visual-inertial odometry execute the following command in `euroc_data` folder where you downloaded the dataset. +``` +basalt_vio --dataset-path MH_05_difficult/ --cam-calib /usr/etc/basalt/euroc_ds_calib.json --dataset-type euroc --config-path /usr/etc/basalt/euroc_config.json --marg-data euroc_marg_data --show-gui 1 +``` +The command line options have the following meaning: +* `--dataset-path` path to the dataset. +* `--dataset-type` type of the datset. Currently only `bag` and `euroc` formats of the datasets are supported. +* `--cam-calib` path to camera calibration file. Check [calibration instructions](doc/Calibration.md) to see how the calibration was generated. +* `--marg-data` folder where the data from keyframe marginalization will be stored. This data can be later used for visual-inertial mapping. +* `--show-gui` enables or disables GUI. + +This opens the GUI and runs the sequence. The processing happens in the background as fast as possible, and the visualization results are saved in GUI and can be analysed offline. +![MH_05_VIO](doc/img/MH_05_VIO.png) + +The buttons in the GUI have the following meaning: +* `show_obs` toggles the visibility the tracked landmarks in the image view. +* `show_ids` toggles the IDs of the points. +* `show_est_pos` shows the plot of the estimated position. +* `show_est_vel` shows the plot of the estimated velocity. +* `show_est_bg` shows the plot of the estimated gyro bias. +* `show_est_ba` shows the plot of the estimated accel bias. +* `show_ge` shows ground-truth trajectory in the 3D view. + +By default the system starts with `continue_fast` enabled. This option visualizes the latest processed frame until the end of the sequence. Alternatively, the `continue_btn` visualizes every frame without skipping. If both options are disabled the system shows the frame that is selected with `show_frame` slider and user can move forward and backward with `next_step` and `prev_step` buttons. The `follow` button changes between the static camera and the camera attached to the current frame. + +For evaluation the button `align_svd` is used. It aligns the GT trajectory with the current estimate with SE(3) transformation and prints the transformation and the root-mean-squared absolute trajectory error (RMS ATE). + +### Visual-inertial mapping +To run the mapping tool execute the following command: +``` +basalt_mapper --cam-calib /usr/etc/basalt/euroc_ds_calib.json --marg-data euroc_marg_data --vocabulary /usr/etc/basalt/orbvoc.dbow3 +``` +Here `--marg-data` is the folder with the results from VIO and `--vocabulary` is the path to DBoW3 vocabulary. + +This opens the GUI and extracts non-linear factors from the marginalization data. +![MH_05_MAPPING](doc/img/MH_05_MAPPING.png) + +The buttons in the GUI have the following meaning: +* `show_frame1`, `show_cam1`, `show_frame2`, `show_cam2` allows you to assign images to image view 1 and 2 from different timestamps and cameras. +* `show_detected` shows the detected keypoints in the image view window. +* `show_matches` shows feature matching results. +* `show_inliers` shows inlier matches after geometric verification. +* `show_ids` prints point IDs. Can be used to find the same point in two views to check matches and inliers. +* `show_gt` shows the ground-truth trajectory. +* `show_edges` shows the edges from the factors. Relative-pose factors in red, roll-pitch factors in magenta and bundle adjustment co-visibility edges in green. +* `show_points` shows 3D landmarks. + +The workflow for the mapping is the following: +* `detect` detect the keypoints in the keyframe images. +* `match` run the geometric 2D to 2D matching between image frames. +* `tracks` build tracks from 2D matches and triangulate the points. +* `optimize` run the optimization +* `align_svd` align ground-truth trajectory in SE(3) and print the transformation and the error. + +For more systematic evaulation see the evaluation scripts in `scripts/eval_full` folder. + +**NOTE: It appears that only the datasets in ASL Dataset Format (`euroc` dataset type in our notation) contain ground truth that is time-aligned to the IMU and camera images. It is located in `state_groundtruth_estimate0` folder. Bag files have raw Mocap measurements that are not time aligned and should not be used for evaluations.** + + + +### Optical Flow +The visual-inertial odometry relies on the optical flow results. To enable a better analysis of the system we also provide a separate optical flow executable +``` +basalt_opt_flow --dataset-path MH_05_difficult/ --cam-calib /usr/etc/basalt/euroc_ds_calib.json --dataset-type euroc --config-path /usr/etc/basalt/euroc_config.json --show-gui 1 +``` + +This will run the GUI and print an average track length after the dataset is processed. +![MH_05_OPT_FLOW](doc/img/MH_05_OPT_FLOW.png) \ No newline at end of file diff --git a/doc/img/MH_05_MAPPING.png b/doc/img/MH_05_MAPPING.png new file mode 100644 index 0000000..bc7bd1e Binary files /dev/null and b/doc/img/MH_05_MAPPING.png differ diff --git a/doc/img/MH_05_OPT_FLOW.png b/doc/img/MH_05_OPT_FLOW.png new file mode 100644 index 0000000..cb011b2 Binary files /dev/null and b/doc/img/MH_05_OPT_FLOW.png differ diff --git a/doc/img/MH_05_VIO.png b/doc/img/MH_05_VIO.png new file mode 100644 index 0000000..31bc7a4 Binary files /dev/null and b/doc/img/MH_05_VIO.png differ diff --git a/doc/img/SIM_MAPPER.png b/doc/img/SIM_MAPPER.png new file mode 100644 index 0000000..1caac36 Binary files /dev/null and b/doc/img/SIM_MAPPER.png differ diff --git a/doc/img/SIM_VIO.png b/doc/img/SIM_VIO.png new file mode 100644 index 0000000..5acefe1 Binary files /dev/null and b/doc/img/SIM_VIO.png differ diff --git a/doc/img/euroc_cam_calib.png b/doc/img/euroc_cam_calib.png new file mode 100644 index 0000000..a22ad58 Binary files /dev/null and b/doc/img/euroc_cam_calib.png differ diff --git a/doc/img/euroc_imu_calib.png b/doc/img/euroc_imu_calib.png new file mode 100644 index 0000000..7506731 Binary files /dev/null and b/doc/img/euroc_imu_calib.png differ diff --git a/doc/img/teaser.png b/doc/img/teaser.png new file mode 100644 index 0000000..a9a1bff Binary files /dev/null and b/doc/img/teaser.png differ diff --git a/doc/img/tumvi_cam_calib.png b/doc/img/tumvi_cam_calib.png new file mode 100644 index 0000000..631c4b5 Binary files /dev/null and b/doc/img/tumvi_cam_calib.png differ diff --git a/doc/img/tumvi_imu_calib.png b/doc/img/tumvi_imu_calib.png new file mode 100644 index 0000000..4fbbe94 Binary files /dev/null and b/doc/img/tumvi_imu_calib.png differ diff --git a/docker/b_image/Dockerfile b/docker/b_image/Dockerfile new file mode 100644 index 0000000..3fe3c06 --- /dev/null +++ b/docker/b_image/Dockerfile @@ -0,0 +1,3 @@ +FROM ubuntu:18.04 + +RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev openssh-client liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libopencv-dev libpython2.7-dev libgtest-dev lsb-core gcovr ggcov lcov diff --git a/docker/b_image_xenial/Dockerfile b/docker/b_image_xenial/Dockerfile new file mode 100644 index 0000000..38ceeec --- /dev/null +++ b/docker/b_image_xenial/Dockerfile @@ -0,0 +1,17 @@ +FROM ubuntu:16.04 + +RUN apt-get update && apt-get install -y cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng12-dev openssh-client liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libopencv-dev libpython2.7-dev wget libgtest-dev software-properties-common + +RUN wget https://cmake.org/files/v3.13/cmake-3.13.4-Linux-x86_64.sh +RUN chmod +x cmake-3.13.4-Linux-x86_64.sh +RUN ./cmake-3.13.4-Linux-x86_64.sh --skip-license --prefix=/usr/local +RUN update-alternatives --install /usr/bin/cmake cmake /usr/local/bin/cmake 1 --force +RUN cmake --version + + +RUN add-apt-repository ppa:ubuntu-toolchain-r/test +RUN apt update && apt-get install -y g++-7 +RUN update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 60 \ + --slave /usr/bin/g++ g++ /usr/bin/g++-7 +RUN gcc --version +RUN g++ --version diff --git a/include/basalt/calibration/aprilgrid.h b/include/basalt/calibration/aprilgrid.h new file mode 100644 index 0000000..cf1c79e --- /dev/null +++ b/include/basalt/calibration/aprilgrid.h @@ -0,0 +1,137 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +namespace basalt { + +struct AprilGrid { + AprilGrid() { + const int tagCols = 6; // number of apriltags + const int tagRows = 6; // number of apriltags + const double tagSize = 0.088; // size of apriltag, edge to edge [m] + const double tagSpacing = 0.3; // ratio of space between tags to tagSize + + double x_corner_offsets[4] = {0, tagSize, tagSize, 0}; + double y_corner_offsets[4] = {0, 0, tagSize, tagSize}; + + aprilgrid_corner_pos_3d.resize(tagCols * tagRows * 4); + + for (int y = 0; y < tagCols; y++) { + for (int x = 0; x < tagRows; x++) { + int tag_id = tagRows * y + x; + double x_offset = x * tagSize * (1 + tagSpacing); + double y_offset = y * tagSize * (1 + tagSpacing); + + for (int i = 0; i < 4; i++) { + int corner_id = (tag_id << 2) + i; + + Eigen::Vector4d &pos_3d = aprilgrid_corner_pos_3d[corner_id]; + + pos_3d[0] = x_offset + x_corner_offsets[i]; + pos_3d[1] = y_offset + y_corner_offsets[i]; + pos_3d[2] = 0; + pos_3d[3] = 1; + } + } + } + + size_t num_vign_points = 5; + size_t num_blocks = tagCols * tagRows * 2; + + aprilgrid_vignette_pos_3d.resize((num_blocks + tagCols + tagRows) * + num_vign_points); + + for (size_t k = 0; k < num_vign_points; k++) { + for (size_t i = 0; i < tagCols * tagRows; i++) { + // const Eigen::Vector3d p0 = aprilgrid_corner_pos_3d[4 * i + 0]; + const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1]; + const Eigen::Vector4d p2 = aprilgrid_corner_pos_3d[4 * i + 2]; + const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i + 3]; + + double coeff = double(k + 1) / double(num_vign_points + 1); + + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0] = + (p1 + coeff * (p2 - p1)); + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1] = + (p2 + coeff * (p3 - p2)); + + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0][0] += + tagSize * tagSpacing / 2; + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1][1] += + tagSize * tagSpacing / 2; + } + } + + size_t curr_idx = num_blocks * num_vign_points; + + for (size_t k = 0; k < num_vign_points; k++) { + for (size_t i = 0; i < tagCols; i++) { + const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i + 0]; + const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1]; + + double coeff = double(k + 1) / double(num_vign_points + 1); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] = + (p0 + coeff * (p1 - p0)); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][1] -= + tagSize * tagSpacing / 2; + } + } + + curr_idx += tagCols * num_vign_points; + + for (size_t k = 0; k < num_vign_points; k++) { + for (size_t i = 0; i < tagRows; i++) { + const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i * tagCols + 0]; + const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i * tagCols + 3]; + + double coeff = double(k + 1) / double(num_vign_points + 1); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] = + (p0 + coeff * (p3 - p0)); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][0] -= + tagSize * tagSpacing / 2; + } + } + } + + Eigen::vector aprilgrid_corner_pos_3d; + Eigen::vector aprilgrid_vignette_pos_3d; +}; +} // namespace basalt diff --git a/include/basalt/calibration/calibration_helper.h b/include/basalt/calibration/calibration_helper.h new file mode 100644 index 0000000..558ee5c --- /dev/null +++ b/include/basalt/calibration/calibration_helper.h @@ -0,0 +1,116 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include +#include + +namespace basalt { + +struct CalibCornerData { + Eigen::vector corners; + std::vector corner_ids; + std::vector radii; //!< threshold used for maximum displacement + //! during sub-pix refinement; Search region is + //! slightly larger. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct CalibInitPoseData { + Sophus::SE3d T_a_c; + size_t num_inliers; + + Eigen::vector reprojected_corners; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class CalibHelper { + public: + static void detectCorners( + const VioDatasetPtr& vio_data, + tbb::concurrent_unordered_map& calib_corners, + tbb::concurrent_unordered_map& + calib_corners_rejected); + + static void initCamPoses( + const Calibration::Ptr& calib, const VioDatasetPtr& vio_data, + const Eigen::vector& aprilgrid_corner_pos_3d, + tbb::concurrent_unordered_map& calib_corners, + tbb::concurrent_unordered_map& + calib_init_poses); + + static bool initializeIntrinsics( + const Eigen::vector& corners, + const std::vector& corner_ids, + const Eigen::vector& aprilgrid_corner_pos_3d, int cols, + int rows, Eigen::Vector4d& init_intr); + + private: + inline static double square(double x) { return x * x; } + + inline static double hypot(double a, double b) { + return sqrt(square(a) + square(b)); + } + + static void computeInitialPose( + const Calibration::Ptr& calib, size_t cam_id, + const Eigen::vector& aprilgrid_corner_pos_3d, + const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp); + + static size_t computeReprojectionError( + const UnifiedCamera& cam_calib, + const Eigen::vector& corners, + const std::vector& corner_ids, + const Eigen::vector& aprilgrid_corner_pos_3d, + const Sophus::SE3d& T_target_camera, double& error); +}; + +} // namespace basalt + +namespace cereal { +template +void serialize(Archive& ar, basalt::CalibCornerData& c) { + ar(c.corners, c.corner_ids, c.radii); +} + +template +void serialize(Archive& ar, basalt::CalibInitPoseData& c) { + ar(c.T_a_c, c.num_inliers, c.reprojected_corners); +} +} // namespace cereal diff --git a/include/basalt/calibration/cam_calib.h b/include/basalt/calibration/cam_calib.h new file mode 100644 index 0000000..aef7bd2 --- /dev/null +++ b/include/basalt/calibration/cam_calib.h @@ -0,0 +1,165 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace basalt { + +class PosesOptimization; + +class CamCalib { + public: + CamCalib(const std::string &dataset_path, const std::string &dataset_type, + const std::string &cache_path, const std::string &cache_dataset_name, + int skip_images, const std::vector &cam_types, + bool show_gui = true); + + ~CamCalib(); + + void initGui(); + + void computeVign(); + + void setNumCameras(size_t n); + + void renderingLoop(); + + void computeProjections(); + + void detectCorners(); + + void initCamIntrinsics(); + + void initCamPoses(); + + void initCamExtrinsics(); + + void initOptimization(); + + void loadDataset(); + + void optimize(); + + void optimizeWithParam(bool print_info, + std::map *stats = nullptr); + + void saveCalib(); + + void drawImageOverlay(pangolin::View &v, size_t cam_id); + + bool hasCorners() const; + + void setOptIntrinsics(bool opt) { opt_intr = opt; } + + private: + static constexpr int UI_WIDTH = 300; + + static constexpr size_t RANSAC_THRESHOLD = 10; + + // typedef Calibration::Ptr CalibrationPtr; + + AprilGrid april_grid; + + VioDatasetPtr vio_dataset; + // CalibrationPtr calib; + + tbb::concurrent_unordered_map calib_corners; + tbb::concurrent_unordered_map + calib_corners_rejected; + tbb::concurrent_unordered_map calib_init_poses; + + std::shared_ptr processing_thread; + + std::shared_ptr calib_opt; + + std::map> reprojected_corners; + std::map> reprojected_vignette; + std::map> reprojected_vignette_error; + + std::string dataset_path; + std::string dataset_type; + std::string cache_path; + std::string cache_dataset_name; + + int skip_images; + + std::vector cam_types; + + bool show_gui; + + const size_t MIN_CORNERS = 15; + + ////////////////////// + + pangolin::Var show_frame; + + pangolin::Var show_corners; + pangolin::Var show_corners_rejected; + pangolin::Var show_init_reproj; + pangolin::Var show_opt; + pangolin::Var show_vign; + pangolin::Var show_ids; + + pangolin::Var huber_thresh; + + pangolin::Var opt_intr; + + std::shared_ptr vign_plotter; + pangolin::View *img_view_display; + + std::vector> img_view; + + pangolin::DataLog vign_data_log; +}; + +} // namespace basalt diff --git a/include/basalt/calibration/cam_imu_calib.h b/include/basalt/calibration/cam_imu_calib.h new file mode 100644 index 0000000..d616d34 --- /dev/null +++ b/include/basalt/calibration/cam_imu_calib.h @@ -0,0 +1,180 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace basalt { + +template +class SplineOptimization; + +class CamImuCalib { + public: + CamImuCalib(const std::string &dataset_path, const std::string &dataset_type, + const std::string &cache_path, + const std::string &cache_dataset_name, int skip_images, + const std::vector &imu_noise, bool show_gui = true); + + ~CamImuCalib(); + + void initGui(); + + void setNumCameras(size_t n); + + void renderingLoop(); + + void computeProjections(); + + void detectCorners(); + + void initCamPoses(); + + void initCamImuTransform(); + + void initOptimization(); + + void initMocap(); + + void loadDataset(); + + void optimize(); + + void optimizeWithParam(bool print_info, + std::map *stats = nullptr); + + void saveCalib(); + + void saveMocapCalib(); + + void drawImageOverlay(pangolin::View &v, size_t cam_id); + + void recomputeDataLog(); + + void drawPlots(); + + bool hasCorners() const; + + void setOptIntrinsics(bool opt) { opt_intr = opt; } + + private: + static constexpr int UI_WIDTH = 300; + + AprilGrid april_grid; + + VioDatasetPtr vio_dataset; + + tbb::concurrent_unordered_map calib_corners; + tbb::concurrent_unordered_map + calib_corners_rejected; + tbb::concurrent_unordered_map calib_init_poses; + + std::shared_ptr processing_thread; + + std::shared_ptr> calib_opt; + + std::map> reprojected_corners; + + std::string dataset_path; + std::string dataset_type; + std::string cache_path; + std::string cache_dataset_name; + + int skip_images; + + bool show_gui; + + const size_t MIN_CORNERS = 15; + + std::vector imu_noise; + + ////////////////////// + + pangolin::Var show_frame; + + pangolin::Var show_corners; + pangolin::Var show_corners_rejected; + pangolin::Var show_init_reproj; + pangolin::Var show_opt; + pangolin::Var show_ids; + + pangolin::Var show_accel; + pangolin::Var show_gyro; + pangolin::Var show_pos; + pangolin::Var show_rot_error; + + pangolin::Var show_mocap; + pangolin::Var show_mocap_rot_error; + pangolin::Var show_mocap_rot_vel; + + pangolin::Var show_spline; + pangolin::Var show_data; + + pangolin::Var opt_intr; + pangolin::Var opt_poses; + pangolin::Var opt_corners; + pangolin::Var opt_cam_time_offset; + pangolin::Var opt_imu_scale; + + pangolin::Var opt_mocap; + + pangolin::Var huber_thresh; + + pangolin::Plotter *plotter; + pangolin::View *img_view_display; + + std::vector> img_view; + + pangolin::DataLog imu_data_log, pose_data_log, mocap_data_log, vign_data_log; +}; + +} // namespace basalt diff --git a/include/basalt/calibration/vignette.h b/include/basalt/calibration/vignette.h new file mode 100644 index 0000000..15b7fb2 --- /dev/null +++ b/include/basalt/calibration/vignette.h @@ -0,0 +1,87 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include + +#include + +namespace basalt { + +class VignetteEstimator { + public: + static const int SPLINE_N = 4; + static const int64_t knot_spacing = 1e10; + static const int border_size = 2; + + VignetteEstimator(const VioDatasetPtr &vio_dataset, + const Eigen::vector &optical_centers, + const std::map> + &reprojected_vignette, + const AprilGrid &april_grid); + + void compute_error(std::map> + *reprojected_vignette_error = nullptr); + + void opt_irradience(); + + void opt_vign(); + + void optimize(); + + void compute_data_log(pangolin::DataLog &vign_data_log); + + void save_vign_png(const std::string &path); + + inline const std::vector> &get_vign_param() { + return vign_param; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + const VioDatasetPtr vio_dataset; + Eigen::vector optical_centers; + std::map> reprojected_vignette; + const AprilGrid &april_grid; + + size_t vign_size; + std::vector irradiance; + std::vector> vign_param; +}; +} diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h new file mode 100644 index 0000000..6909019 --- /dev/null +++ b/include/basalt/io/dataset_io.h @@ -0,0 +1,247 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef DATASET_IO_H +#define DATASET_IO_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include + +namespace basalt { + +inline bool file_exists(const std::string &name) { + std::ifstream f(name.c_str()); + return f.good(); +} + +typedef std::pair TimeCamId; + +struct ImageData { + ImageData() : exposure(0) {} + + ManagedImage::Ptr img; + double exposure; +}; + +struct Observations { + Eigen::vector pos; + std::vector id; +}; + +struct GyroData { + int64_t timestamp_ns; + Eigen::Vector3d data; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct AccelData { + int64_t timestamp_ns; + Eigen::Vector3d data; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct PoseData { + int64_t timestamp_ns; + Sophus::SE3d data; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct MocapPoseData { + int64_t timestamp_ns; + Sophus::SE3d data; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct AprilgridCornersData { + int64_t timestamp_ns; + int cam_id; + + Eigen::vector corner_pos; + std::vector corner_id; +}; + +class VioDataset { + public: + virtual ~VioDataset(){}; + + virtual size_t get_num_cams() const = 0; + + virtual std::vector &get_image_timestamps() = 0; + + virtual const Eigen::vector &get_accel_data() const = 0; + virtual const Eigen::vector &get_gyro_data() const = 0; + virtual const std::vector &get_gt_timestamps() const = 0; + virtual const Eigen::vector &get_gt_pose_data() const = 0; + virtual int64_t get_mocap_to_imu_offset_ns() const = 0; + virtual std::vector get_image_data(int64_t t_ns) = 0; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +typedef std::shared_ptr VioDatasetPtr; + +class DatasetIoInterface { + public: + virtual void read(const std::string &path) = 0; + virtual void reset() = 0; + virtual VioDatasetPtr get_data() = 0; + + virtual ~DatasetIoInterface(){}; +}; + +typedef std::shared_ptr DatasetIoInterfacePtr; + +class DatasetIoFactory { + public: + static DatasetIoInterfacePtr getDatasetIo(const std::string &dataset_type, + bool with_images = true); +}; + +} // namespace basalt + +namespace cereal { + +template +inline + typename std::enable_if<_Rows == Eigen::Dynamic || _Cols == Eigen::Dynamic, + void>::type + save(Archive &ar, const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, + _MaxRows, _MaxCols> &matrix) { + const std::int32_t rows = static_cast(matrix.rows()); + const std::int32_t cols = static_cast(matrix.cols()); + ar(rows); + ar(cols); + ar(binary_data(matrix.data(), rows * cols * sizeof(_Scalar))); +}; + +template +inline + typename std::enable_if<_Rows == Eigen::Dynamic || _Cols == Eigen::Dynamic, + void>::type + load(Archive &ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, + _MaxCols> &matrix) { + std::int32_t rows; + std::int32_t cols; + ar(rows); + ar(cols); + + matrix.resize(rows, cols); + + ar(binary_data(matrix.data(), + static_cast(rows * cols * sizeof(_Scalar)))); +}; + +template +void serialize(Archive &archive, basalt::ManagedImage &m) { + archive(m.w); + archive(m.h); + + m.Reinitialise(m.w, m.h); + + archive(binary_data(m.ptr, m.size())); +} + +template +void serialize(Archive &ar, basalt::GyroData &c) { + ar(c.timestamp_ns, c.data); +} + +template +void serialize(Archive &ar, basalt::AccelData &c) { + ar(c.timestamp_ns, c.data); +} + +} // namespace cereal + +namespace std { + +inline void hash_combine(std::size_t &seed, std::size_t value) { + seed ^= value + 0x9e3779b9 + (seed << 6) + (seed >> 2); +} + +template <> +struct hash { + size_t operator()(const basalt::TimeCamId &x) const { + size_t seed = 0; + hash_combine(seed, std::hash()(x.first)); + hash_combine(seed, std::hash()(x.second)); + return seed; + } +}; + +template <> +struct hash> { + size_t operator()( + const std::pair &x) const { + size_t seed = 0; + hash_combine(seed, std::hash()(x.first.first)); + hash_combine(seed, std::hash()(x.first.second)); + hash_combine(seed, std::hash()(x.second.first)); + hash_combine(seed, std::hash()(x.second.second)); + return seed; + } +}; +} // namespace std + +#endif // DATASET_IO_H diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h new file mode 100644 index 0000000..5ca5a8c --- /dev/null +++ b/include/basalt/io/dataset_io_euroc.h @@ -0,0 +1,248 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef DATASET_IO_EUROC_H +#define DATASET_IO_EUROC_H + +#include + +namespace basalt { + +class EurocVioDataset : public VioDataset { + size_t num_cams; + + std::string path; + + std::vector image_timestamps; + std::unordered_map image_path; + + // vector of images for every timestamp + // assumes vectors size is num_cams for every timestamp with null pointers for + // missing frames + // std::unordered_map> image_data; + + Eigen::vector accel_data; + Eigen::vector gyro_data; + + std::vector gt_timestamps; // ordered gt timestamps + Eigen::vector gt_pose_data; // TODO: change to eigen aligned + + int64_t mocap_to_imu_offset_ns; + + public: + ~EurocVioDataset(){}; + + size_t get_num_cams() const { return num_cams; } + + std::vector &get_image_timestamps() { return image_timestamps; } + + const Eigen::vector &get_accel_data() const { return accel_data; } + const Eigen::vector &get_gyro_data() const { return gyro_data; } + const std::vector &get_gt_timestamps() const { + return gt_timestamps; + } + const Eigen::vector &get_gt_pose_data() const { + return gt_pose_data; + } + int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; } + + std::vector get_image_data(int64_t t_ns) { + std::vector res(num_cams); + + const std::vector folder = {"/mav0/cam0/", "/mav0/cam1/"}; + + for (size_t i = 0; i < num_cams; i++) { + std::string full_image_path = + path + folder[i] + "data/" + image_path[t_ns]; + + if (file_exists(full_image_path)) { + pangolin::TypedImage img = pangolin::LoadImage(full_image_path); + + if (img.fmt.bpp == 8) { + res[i].img.reset(new ManagedImage(img.w, img.h)); + + const uint8_t *data_in = img.ptr; + uint16_t *data_out = res[i].img->ptr; + + for (size_t i = 0; i < img.size(); i++) { + int val = data_in[i]; + val = val << 8; + data_out[i] = val; + } + } else if (img.fmt.bpp == 16) { + res[i].img.reset(new ManagedImage(img.w, img.h)); + std::memcpy(res[i].img->ptr, img.ptr, img.size() * sizeof(uint16_t)); + + } else { + std::cerr << "img.fmt.bpp " << img.fmt.bpp << std::endl; + std::abort(); + } + } + } + + return res; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + friend class EurocIO; +}; + +class EurocIO : public DatasetIoInterface { + public: + EurocIO() {} + + void read(const std::string &path) { + data.reset(new EurocVioDataset); + + data->num_cams = 2; + data->path = path; + + read_image_timestamps(path + "/mav0/cam0/"); + + read_imu_data(path + "/mav0/imu0/"); + + if (file_exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) { + read_gt_data_state(path + "/mav0/state_groundtruth_estimate0/"); + } else if (file_exists(path + "/mav0/mocap0/data.csv")) { + read_gt_data_pose(path + "/mav0/mocap0/"); + } + } + + void reset() { data.reset(); } + + VioDatasetPtr get_data() { return data; } + + private: + void read_image_timestamps(const std::string &path) { + std::ifstream f(path + "data.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + std::stringstream ss(line); + char tmp; + int64_t t_ns; + std::string path; + ss >> t_ns >> tmp >> path; + + data->image_timestamps.emplace_back(t_ns); + data->image_path[t_ns] = path; + } + } + + void read_imu_data(const std::string &path) { + data->accel_data.clear(); + data->gyro_data.clear(); + + std::ifstream f(path + "data.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + char tmp; + uint64_t timestamp; + Eigen::Vector3d gyro, accel; + + ss >> timestamp >> tmp >> gyro[0] >> tmp >> gyro[1] >> tmp >> gyro[2] >> + tmp >> accel[0] >> tmp >> accel[1] >> tmp >> accel[2]; + + data->accel_data.emplace_back(); + data->accel_data.back().timestamp_ns = timestamp; + data->accel_data.back().data = accel; + + data->gyro_data.emplace_back(); + data->gyro_data.back().timestamp_ns = timestamp; + data->gyro_data.back().data = gyro; + } + } + + void read_gt_data_state(const std::string &path) { + data->gt_timestamps.clear(); + data->gt_pose_data.clear(); + + std::ifstream f(path + "data.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + char tmp; + uint64_t timestamp; + Eigen::Quaterniond q; + Eigen::Vector3d pos, vel, accel_bias, gyro_bias; + + ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >> + tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z() >> tmp >> + vel[0] >> tmp >> vel[1] >> tmp >> vel[2] >> tmp >> accel_bias[0] >> + tmp >> accel_bias[1] >> tmp >> accel_bias[2] >> tmp >> gyro_bias[0] >> + tmp >> gyro_bias[1] >> tmp >> gyro_bias[2]; + + data->gt_timestamps.emplace_back(timestamp); + data->gt_pose_data.emplace_back(q, pos); + } + } + + void read_gt_data_pose(const std::string &path) { + data->gt_timestamps.clear(); + data->gt_pose_data.clear(); + + std::ifstream f(path + "data.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + char tmp; + uint64_t timestamp; + Eigen::Quaterniond q; + Eigen::Vector3d pos; + + ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >> + tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z(); + + data->gt_timestamps.emplace_back(timestamp); + data->gt_pose_data.emplace_back(q, pos); + } + } + + std::shared_ptr data; +}; + +} // namespace basalt + +#endif // DATASET_IO_H diff --git a/include/basalt/io/dataset_io_rosbag.h b/include/basalt/io/dataset_io_rosbag.h new file mode 100644 index 0000000..01f6919 --- /dev/null +++ b/include/basalt/io/dataset_io_rosbag.h @@ -0,0 +1,375 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef DATASET_IO_ROSBAG_H +#define DATASET_IO_ROSBAG_H + +#include +#include + +#include + +// Hack to access private functions +#define private public +#include +#include +#undef private + +#include +#include +#include +#include + +namespace basalt { + +class RosbagVioDataset : public VioDataset { + std::shared_ptr bag; + std::mutex m; + + size_t num_cams; + + std::vector image_timestamps; + + // vector of images for every timestamp + // assumes vectors size is num_cams for every timestamp with null pointers for + // missing frames + std::unordered_map>> + image_data_idx; + + Eigen::vector accel_data; + Eigen::vector gyro_data; + + std::vector gt_timestamps; // ordered gt timestamps + Eigen::vector gt_pose_data; // TODO: change to eigen aligned + + int64_t mocap_to_imu_offset_ns; + + public: + ~RosbagVioDataset() {} + + size_t get_num_cams() const { return num_cams; } + + std::vector &get_image_timestamps() { return image_timestamps; } + + const Eigen::vector &get_accel_data() const { return accel_data; } + const Eigen::vector &get_gyro_data() const { return gyro_data; } + const std::vector &get_gt_timestamps() const { + return gt_timestamps; + } + const Eigen::vector &get_gt_pose_data() const { + return gt_pose_data; + } + int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; } + + std::vector get_image_data(int64_t t_ns) { + std::vector res(num_cams); + + auto it = image_data_idx.find(t_ns); + + if (it != image_data_idx.end()) + for (size_t i = 0; i < num_cams; i++) { + ImageData &id = res[i]; + + if (!it->second[i].has_value()) continue; + + m.lock(); + sensor_msgs::ImageConstPtr img_msg = + bag->instantiateBuffer(*it->second[i]); + m.unlock(); + + // std::cerr << "img_msg->width " << img_msg->width << " + // img_msg->height " + // << img_msg->height << std::endl; + + id.img.reset( + new ManagedImage(img_msg->width, img_msg->height)); + + if (!img_msg->header.frame_id.empty() && + std::isdigit(img_msg->header.frame_id[0])) { + id.exposure = std::stol(img_msg->header.frame_id) * 1e-9; + } else { + id.exposure = -1; + } + + if (img_msg->encoding == "mono8") { + const uint8_t *data_in = img_msg->data.data(); + uint16_t *data_out = id.img->ptr; + + for (size_t i = 0; i < img_msg->data.size(); i++) { + int val = data_in[i]; + val = val << 8; + data_out[i] = val; + } + + } else if (img_msg->encoding == "mono16") { + std::memcpy(id.img->ptr, img_msg->data.data(), img_msg->data.size()); + } else { + std::cerr << "Encoding " << img_msg->encoding << " is not supported." + << std::endl; + std::abort(); + } + } + + return res; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + friend class RosbagIO; +}; + +class RosbagIO : public DatasetIoInterface { + public: + RosbagIO(bool with_images) : with_images(with_images) {} + + void read(const std::string &path) { + data.reset(new RosbagVioDataset); + + data->bag.reset(new rosbag::Bag); + data->bag->open(path, rosbag::bagmode::Read); + + rosbag::View view(*data->bag, [this](const rosbag::ConnectionInfo *ci) { + if (this->with_images) + return true; + else + return ci->datatype == std::string("sensor_msgs/Imu") || + ci->datatype == std::string("geometry_msgs/TransformStamped"); + }); + + // get topics + std::vector connection_infos = + view.getConnections(); + + std::set cam_topics; + std::string imu_topic; + std::string mocap_topic; + std::string point_topic; + + for (const rosbag::ConnectionInfo *info : connection_infos) { + // if (info->topic.substr(0, 4) == std::string("/cam")) { + // cam_topics.insert(info->topic); + // } else if (info->topic.substr(0, 4) == std::string("/imu")) { + // imu_topic = info->topic; + // } else if (info->topic.substr(0, 5) == std::string("/vrpn") || + // info->topic.substr(0, 6) == std::string("/vicon")) { + // mocap_topic = info->topic; + // } + + if (info->datatype == std::string("sensor_msgs/Image")) { + cam_topics.insert(info->topic); + } else if (info->datatype == std::string("sensor_msgs/Imu")) { + imu_topic = info->topic; + } else if (info->datatype == + std::string("geometry_msgs/TransformStamped")) { + mocap_topic = info->topic; + } else if (info->datatype == std::string("geometry_msgs/PointStamped")) { + point_topic = info->topic; + } + } + + std::cout << "imu_topic: " << imu_topic << std::endl; + std::cout << "mocap_topic: " << mocap_topic << std::endl; + std::cout << "cam_topics: "; + for (const std::string &s : cam_topics) std::cout << s << " "; + std::cout << std::endl; + + std::map topic_to_id; + int idx = 0; + for (const std::string &s : cam_topics) { + topic_to_id[s] = idx; + idx++; + } + + data->num_cams = cam_topics.size(); + + int num_msgs = 0; + + int64_t min_time = std::numeric_limits::max(); + int64_t max_time = std::numeric_limits::min(); + + std::vector mocap_msgs; + std::vector point_msgs; + + std::vector + system_to_imu_offset_vec; // t_imu = t_system + system_to_imu_offset + std::vector system_to_mocap_offset_vec; // t_mocap = t_system + + // system_to_mocap_offset + + std::set image_timestamps; + + for (rosbag::MessageInstance const m : view) { + const std::string &topic = m.getTopic(); + + if (cam_topics.find(topic) != cam_topics.end()) { + sensor_msgs::ImageConstPtr img_msg = + m.instantiate(); + int64_t timestamp_ns = img_msg->header.stamp.toNSec(); + + auto &img_vec = data->image_data_idx[timestamp_ns]; + if (img_vec.size() == 0) img_vec.resize(data->num_cams); + + img_vec[topic_to_id.at(topic)] = m.index_entry_; + image_timestamps.insert(timestamp_ns); + + min_time = std::min(min_time, timestamp_ns); + max_time = std::max(max_time, timestamp_ns); + } + + if (imu_topic == topic) { + sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); + int64_t time = imu_msg->header.stamp.toNSec(); + + data->accel_data.emplace_back(); + data->accel_data.back().timestamp_ns = time; + data->accel_data.back().data = Eigen::Vector3d( + imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, + imu_msg->linear_acceleration.z); + + data->gyro_data.emplace_back(); + data->gyro_data.back().timestamp_ns = time; + data->gyro_data.back().data = Eigen::Vector3d( + imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, + imu_msg->angular_velocity.z); + + min_time = std::min(min_time, time); + max_time = std::max(max_time, time); + + int64_t msg_arrival_time = m.getTime().toNSec(); + system_to_imu_offset_vec.push_back(time - msg_arrival_time); + } + + if (mocap_topic == topic) { + geometry_msgs::TransformStampedConstPtr mocap_msg = + m.instantiate(); + int64_t time = mocap_msg->header.stamp.toNSec(); + + mocap_msgs.push_back(mocap_msg); + + int64_t msg_arrival_time = m.getTime().toNSec(); + system_to_mocap_offset_vec.push_back(time - msg_arrival_time); + } + + if (point_topic == topic) { + geometry_msgs::PointStampedConstPtr mocap_msg = + m.instantiate(); + int64_t time = mocap_msg->header.stamp.toNSec(); + + point_msgs.push_back(mocap_msg); + + int64_t msg_arrival_time = m.getTime().toNSec(); + system_to_mocap_offset_vec.push_back(time - msg_arrival_time); + } + + num_msgs++; + } + + data->image_timestamps.clear(); + data->image_timestamps.insert(data->image_timestamps.begin(), + image_timestamps.begin(), + image_timestamps.end()); + + if (system_to_mocap_offset_vec.size() > 0) { + int64_t system_to_imu_offset = + system_to_imu_offset_vec[system_to_imu_offset_vec.size() / 2]; + + int64_t system_to_mocap_offset = + system_to_mocap_offset_vec[system_to_mocap_offset_vec.size() / 2]; + + data->mocap_to_imu_offset_ns = + system_to_imu_offset - system_to_mocap_offset; + } + + data->gt_pose_data.clear(); + data->gt_timestamps.clear(); + + if (!mocap_msgs.empty()) + for (size_t i = 0; i < mocap_msgs.size() - 1; i++) { + auto mocap_msg = mocap_msgs[i]; + + int64_t time = mocap_msg->header.stamp.toNSec(); + + Eigen::Quaterniond q( + mocap_msg->transform.rotation.w, mocap_msg->transform.rotation.x, + mocap_msg->transform.rotation.y, mocap_msg->transform.rotation.z); + + Eigen::Vector3d t(mocap_msg->transform.translation.x, + mocap_msg->transform.translation.y, + mocap_msg->transform.translation.z); + + int64_t timestamp_ns = time + data->mocap_to_imu_offset_ns; + data->gt_timestamps.emplace_back(timestamp_ns); + data->gt_pose_data.emplace_back(q, t); + } + + if (!point_msgs.empty()) + for (size_t i = 0; i < point_msgs.size() - 1; i++) { + auto point_msg = point_msgs[i]; + + int64_t time = point_msg->header.stamp.toNSec(); + + Eigen::Vector3d t(point_msg->point.x, point_msg->point.y, + point_msg->point.z); + + int64_t timestamp_ns = time; // + data->mocap_to_imu_offset_ns; + data->gt_timestamps.emplace_back(timestamp_ns); + data->gt_pose_data.emplace_back(Sophus::SO3d(), t); + } + + std::cout << "Total number of messages: " << num_msgs << std::endl; + std::cout << "Image size: " << data->image_data_idx.size() << std::endl; + + std::cout << "Min time: " << min_time << " max time: " << max_time + << " mocap to imu offset: " << data->mocap_to_imu_offset_ns + << std::endl; + + std::cout << "Number of mocap poses: " << data->gt_timestamps.size() + << std::endl; + } + + void reset() { data.reset(); } + + VioDatasetPtr get_data() { + // return std::dynamic_pointer_cast(data); + return data; + } + + private: + std::shared_ptr data; + + bool with_images; +}; + +} // namespace basalt + +#endif // DATASET_IO_ROSBAG_H diff --git a/include/basalt/io/marg_data_io.h b/include/basalt/io/marg_data_io.h new file mode 100644 index 0000000..2be5058 --- /dev/null +++ b/include/basalt/io/marg_data_io.h @@ -0,0 +1,76 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include + +namespace basalt { + +class MargDataSaver { + public: + using Ptr = std::shared_ptr; + + MargDataSaver(const std::string& path); + ~MargDataSaver() { + saving_thread->join(); + saving_img_thread->join(); + } + tbb::concurrent_bounded_queue in_marg_queue; + + private: + std::shared_ptr saving_thread; + std::shared_ptr saving_img_thread; + + tbb::concurrent_bounded_queue save_image_queue; +}; + +class MargDataLoader { + public: + using Ptr = std::shared_ptr; + + MargDataLoader(); + + void start(const std::string& path); + ~MargDataLoader() { processing_thread->join(); } + + tbb::concurrent_bounded_queue* out_marg_queue; + + private: + std::shared_ptr processing_thread; +}; +} // namespace basalt diff --git a/include/basalt/optical_flow/frame_to_frame_optical_flow.h b/include/basalt/optical_flow/frame_to_frame_optical_flow.h new file mode 100644 index 0000000..5d36243 --- /dev/null +++ b/include/basalt/optical_flow/frame_to_frame_optical_flow.h @@ -0,0 +1,382 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +#include + +#include + +namespace basalt { + +template typename Pattern> +class FrameToFrameOpticalFlow : public OpticalFlowBase { + public: + typedef OpticalFlowPatch> PatchT; + + typedef Eigen::Matrix Vector2; + typedef Eigen::Matrix Matrix2; + + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Matrix3; + + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Matrix4; + + typedef Sophus::SE2 SE2; + + FrameToFrameOpticalFlow(const VioConfig& config, + const basalt::Calibration& calib) + : t_ns(-1), last_keypoint_id(0), config(config) { + input_queue.set_capacity(10); + + this->calib = calib.cast(); + + patch_coord = PatchT::pattern2.template cast(); + + if (calib.intrinsics.size() > 1) { + Eigen::Matrix4d Ed; + Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1]; + computeEssential(T_i_j, Ed); + E = Ed.cast(); + } + + processing_thread.reset( + new std::thread(&FrameToFrameOpticalFlow::processingLoop, this)); + } + + ~FrameToFrameOpticalFlow() { processing_thread->join(); } + + void processingLoop() { + OpticalFlowInput::Ptr input_ptr; + + while (true) { + input_queue.pop(input_ptr); + + if (!input_ptr.get()) { + if (output_queue) output_queue->push(nullptr); + break; + } + + processFrame(input_ptr->t_ns, input_ptr); + } + } + + void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) { + for (const auto& v : new_img_vec->img_data) { + if (!v.img.get()) return; + } + + if (t_ns < 0) { + t_ns = curr_t_ns; + + transforms.reset(new OpticalFlowResult); + transforms->observations.resize(calib.intrinsics.size()); + transforms->t_ns = t_ns; + + pyramid.reset(new std::vector>); + pyramid->resize(calib.intrinsics.size()); + + tbb::parallel_for(tbb::blocked_range(0, calib.intrinsics.size()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + pyramid->at(i).setFromImage( + *new_img_vec->img_data[i].img, + config.optical_flow_levels); + } + }); + + transforms->input_images = new_img_vec; + + addPoints(); + filterPoints(); + + } else { + t_ns = curr_t_ns; + + old_pyramid = pyramid; + + pyramid.reset(new std::vector>); + pyramid->resize(calib.intrinsics.size()); + tbb::parallel_for(tbb::blocked_range(0, calib.intrinsics.size()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + pyramid->at(i).setFromImage( + *new_img_vec->img_data[i].img, + config.optical_flow_levels); + } + }); + + OpticalFlowResult::Ptr new_transforms; + new_transforms.reset(new OpticalFlowResult); + new_transforms->observations.resize(calib.intrinsics.size()); + new_transforms->t_ns = t_ns; + + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + trackPoints(old_pyramid->at(i), pyramid->at(i), + transforms->observations[i], + new_transforms->observations[i]); + } + + transforms = new_transforms; + transforms->input_images = new_img_vec; + + addPoints(); + filterPoints(); + } + + if (output_queue) { + output_queue->push(transforms); + } + } + + void trackPoints( + const basalt::ManagedImagePyr& pyr_1, + const basalt::ManagedImagePyr& pyr_2, + const Eigen::map& transform_map_1, + Eigen::map& transform_map_2) const { + size_t num_points = transform_map_1.size(); + + std::vector ids; + Eigen::vector init_vec; + + ids.reserve(num_points); + init_vec.reserve(num_points); + + for (const auto& kv : transform_map_1) { + ids.push_back(kv.first); + init_vec.push_back(kv.second); + } + + tbb::concurrent_unordered_map result; + + auto compute_func = [&](const tbb::blocked_range& range) { + for (size_t r = range.begin(); r != range.end(); ++r) { + const KeypointId id = ids[r]; + + const Eigen::AffineCompact2f& transform_1 = init_vec[r]; + Eigen::AffineCompact2f transform_2 = transform_1; + + bool valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2); + + if (valid) { + Eigen::AffineCompact2f transform_1_recovered = transform_2; + + valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered); + + if (valid) { + Scalar dist2 = (transform_1.translation() - + transform_1_recovered.translation()) + .squaredNorm(); + + if (dist2 < config.optical_flow_max_recovered_dist2) { + result[id] = transform_2; + } + } + } + } + }; + + tbb::blocked_range range(0, num_points); + + tbb::parallel_for(range, compute_func); + // compute_func(range); + + transform_map_2.clear(); + transform_map_2.insert(result.begin(), result.end()); + } + + inline bool trackPoint(const basalt::ManagedImagePyr& old_pyr, + const basalt::ManagedImagePyr& pyr, + const Eigen::AffineCompact2f& old_transform, + Eigen::AffineCompact2f& transform) const { + bool patch_valid = true; + + transform.linear().setIdentity(); + + for (int level = config.optical_flow_levels; level >= 0 && patch_valid; + level--) { + const Scalar scale = 1 << level; + + transform.translation() /= scale; + + PatchT p(old_pyr.lvl(level), old_transform.translation() / scale); + + // Perform tracking on current level + patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform); + + transform.translation() *= scale; + } + + transform.linear() = old_transform.linear() * transform.linear(); + + return patch_valid; + } + + inline bool trackPointAtLevel(const Image& img_2, + const PatchT& dp, + Eigen::AffineCompact2f& transform) const { + bool patch_valid = true; + + for (int iteration = 0; + patch_valid && iteration < config.optical_flow_max_iterations; + iteration++) { + typename PatchT::VectorP res; + + typename PatchT::Matrix2P transformed_pat = + transform.linear().matrix() * PatchT::pattern2; + transformed_pat.colwise() += transform.translation(); + + bool valid = dp.residual(img_2, transformed_pat, res); + + if (valid) { + Vector3 inc = -dp.H_se2_inv_J_se2_T * res; + transform *= SE2::exp(inc).matrix(); + + const int filter_margin = 2; + + if (!img_2.InBounds(transform.translation(), filter_margin)) + patch_valid = false; + } else { + patch_valid = false; + } + } + + return patch_valid; + } + + void addPoints() { + Eigen::vector pts0; + + for (const auto& kv : transforms->observations.at(0)) { + pts0.emplace_back(kv.second.translation().cast()); + } + + KeypointsData kd; + + detectKeypoints(pyramid->at(0).lvl(0), kd, + config.optical_flow_detection_grid_size, 1, pts0); + + Eigen::map new_poses0, new_poses1; + + for (size_t i = 0; i < kd.corners.size(); i++) { + Eigen::AffineCompact2f transform; + transform.setIdentity(); + transform.translation() = kd.corners[i].cast(); + + transforms->observations.at(0)[last_keypoint_id] = transform; + new_poses0[last_keypoint_id] = transform; + + last_keypoint_id++; + } + + if (calib.intrinsics.size() > 1) { + trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1); + + for (const auto& kv : new_poses1) { + transforms->observations.at(1).emplace(kv); + } + } + } + + void filterPoints() { + if (calib.intrinsics.size() < 2) return; + + std::set lm_to_remove; + + std::vector kpid; + Eigen::vector proj0, proj1; + + for (const auto& kv : transforms->observations.at(1)) { + auto it = transforms->observations.at(0).find(kv.first); + + if (it != transforms->observations.at(0).end()) { + proj0.emplace_back(it->second.translation()); + proj1.emplace_back(kv.second.translation()); + kpid.emplace_back(kv.first); + } + } + + Eigen::vector p3d0, p3d1; + std::vector p3d0_success, p3d1_success; + + calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); + calib.intrinsics[1].unproject(proj1, p3d1, p3d1_success); + + for (size_t i = 0; i < p3d0_success.size(); i++) { + if (p3d0_success[i] && p3d1_success[i]) { + const double epipolar_error = + std::abs(p3d0[i].transpose() * E * p3d1[i]); + + if (epipolar_error > config.optical_flow_epipolar_error) { + lm_to_remove.emplace(kpid[i]); + } + } else { + lm_to_remove.emplace(kpid[i]); + } + } + + for (int id : lm_to_remove) { + transforms->observations.at(1).erase(id); + } + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + int64_t t_ns; + + KeypointId last_keypoint_id; + + VioConfig config; + basalt::Calibration calib; + + OpticalFlowResult::Ptr transforms; + std::shared_ptr>> old_pyramid, + pyramid; + + Matrix4 E; + + std::shared_ptr processing_thread; +}; + +} // namespace basalt diff --git a/include/basalt/optical_flow/optical_flow.h b/include/basalt/optical_flow/optical_flow.h new file mode 100644 index 0000000..929298c --- /dev/null +++ b/include/basalt/optical_flow/optical_flow.h @@ -0,0 +1,85 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include + +#include + +#include +#include +#include +#include + +#include + +namespace basalt { + +using KeypointId = uint32_t; + +struct OpticalFlowInput { + using Ptr = std::shared_ptr; + + int64_t t_ns; + std::vector img_data; +}; + +struct OpticalFlowResult { + using Ptr = std::shared_ptr; + + int64_t t_ns; + std::vector> observations; + + OpticalFlowInput::Ptr input_images; +}; + +class OpticalFlowBase { + public: + using Ptr = std::shared_ptr; + + tbb::concurrent_bounded_queue input_queue; + tbb::concurrent_bounded_queue* output_queue = nullptr; + + Eigen::MatrixXf patch_coord; +}; + +class OpticalFlowFactory { + public: + static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config, + const Calibration& cam); +}; +} // namespace basalt diff --git a/include/basalt/optical_flow/patch.h b/include/basalt/optical_flow/patch.h new file mode 100644 index 0000000..11eff68 --- /dev/null +++ b/include/basalt/optical_flow/patch.h @@ -0,0 +1,177 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include +#include + +namespace basalt { + +template +struct OpticalFlowPatch { + static constexpr int PATTERN_SIZE = Pattern::PATTERN_SIZE; + + typedef Eigen::Matrix Vector2i; + + typedef Eigen::Matrix Vector2; + typedef Eigen::Matrix Vector2T; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Matrix4; + typedef Eigen::Matrix VectorP; + + typedef Eigen::Matrix Matrix2P; + typedef Eigen::Matrix MatrixP2; + typedef Eigen::Matrix MatrixP3; + typedef Eigen::Matrix Matrix3P; + typedef Eigen::Matrix MatrixP4; + typedef Eigen::Matrix Matrix2Pi; + + static const Matrix2P pattern2; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + OpticalFlowPatch() { mean = 0; } + + OpticalFlowPatch(const Image &img, const Vector2 &pos) { + setFromImage(img, pos); + } + + void setFromImage(const Image &img, const Vector2 &pos) { + this->pos = pos; + + int num_valid_points = 0; + Scalar sum = 0; + Vector2 grad_sum(0, 0); + + MatrixP2 grad; + + for (int i = 0; i < PATTERN_SIZE; i++) { + Vector2 p = pos + pattern2.col(i); + if (img.InBounds(p, 2)) { + Vector3 valGrad = img.interpGrad(p); + data[i] = valGrad[0]; + sum += valGrad[0]; + grad.row(i) = valGrad.template tail<2>(); + grad_sum += valGrad.template tail<2>(); + num_valid_points++; + } else { + data[i] = -1; + } + } + + mean = sum / num_valid_points; + + Scalar mean_inv = num_valid_points / sum; + + Eigen::Matrix Jw_se2; + Jw_se2.template topLeftCorner<2, 2>().setIdentity(); + + MatrixP3 J_se2; + + for (int i = 0; i < PATTERN_SIZE; i++) { + if (data[i] >= 0) { + data[i] *= mean_inv; + Vector2 grad_i = grad.row(i); + grad.row(i) = num_valid_points * (grad_i * sum - grad_sum * data[i]) / + (sum * sum); + } else { + grad.row(i).setZero(); + } + + // Fill jacobians with respect to SE2 warp + Jw_se2(0, 2) = -pattern2(1, i); + Jw_se2(1, 2) = pattern2(0, i); + J_se2.row(i) = grad.row(i) * Jw_se2; + } + + Matrix3 H_se2 = J_se2.transpose() * J_se2; + Matrix3 H_se2_inv; + H_se2_inv.setIdentity(); + H_se2.ldlt().solveInPlace(H_se2_inv); + + H_se2_inv_J_se2_T = H_se2_inv * J_se2.transpose(); + } + + inline bool residual(const Image &img, + const Matrix2P &transformed_pattern, + VectorP &residual) const { + Scalar sum = 0; + Vector2 grad_sum(0, 0); + int num_valid_points = 0; + + for (int i = 0; i < PATTERN_SIZE; i++) { + if (img.InBounds(transformed_pattern.col(i), 2)) { + residual[i] = img.interp(transformed_pattern.col(i)); + sum += residual[i]; + num_valid_points++; + } else { + residual[i] = -1; + } + } + + int num_residuals = 0; + + for (int i = 0; i < PATTERN_SIZE; i++) { + if (residual[i] >= 0 && data[i] >= 0) { + Scalar val = residual[i]; + residual[i] = num_valid_points * val / sum - data[i]; + num_residuals++; + + } else { + residual[i] = 0; + } + } + + return num_residuals > PATTERN_SIZE / 2; + } + + Vector2 pos; + VectorP data; // negative if the point is not valid + + // MatrixP3 J_se2; // total jacobian with respect to se2 warp + // Matrix3 H_se2_inv; + Matrix3P H_se2_inv_J_se2_T; + + Scalar mean; +}; + +template +const typename OpticalFlowPatch::Matrix2P + OpticalFlowPatch::pattern2 = Pattern::pattern2; + +} // namespace basalt diff --git a/include/basalt/optical_flow/patch_optical_flow.h b/include/basalt/optical_flow/patch_optical_flow.h new file mode 100644 index 0000000..ac6c99d --- /dev/null +++ b/include/basalt/optical_flow/patch_optical_flow.h @@ -0,0 +1,376 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +#include + +namespace basalt { + +template typename Pattern> +class PatchOpticalFlow : public OpticalFlowBase { + public: + typedef OpticalFlowPatch> PatchT; + + typedef Eigen::Matrix Vector2; + typedef Eigen::Matrix Matrix2; + + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Matrix3; + + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Matrix4; + + typedef Sophus::SE2 SE2; + + PatchOpticalFlow(const VioConfig& config, + const basalt::Calibration& calib) + : t_ns(-1), last_keypoint_id(0), config(config), calib(calib) { + patches.reserve(3000); + input_queue.set_capacity(10); + + patch_coord = PatchT::pattern2.template cast(); + + if (calib.intrinsics.size() > 1) { + Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1]; + computeEssential(T_i_j, E); + } + + processing_thread.reset( + new std::thread(&PatchOpticalFlow::processingLoop, this)); + } + + ~PatchOpticalFlow() { processing_thread->join(); } + + void processingLoop() { + OpticalFlowInput::Ptr input_ptr; + + while (true) { + input_queue.pop(input_ptr); + + if (!input_ptr.get()) { + output_queue->push(nullptr); + break; + } + + processFrame(input_ptr->t_ns, input_ptr); + } + } + + void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) { + for (const auto& v : new_img_vec->img_data) { + if (!v.img.get()) return; + } + + if (t_ns < 0) { + t_ns = curr_t_ns; + + transforms.reset(new OpticalFlowResult); + transforms->observations.resize(calib.intrinsics.size()); + transforms->t_ns = t_ns; + + pyramid.reset(new std::vector>); + pyramid->resize(calib.intrinsics.size()); + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img, + config.optical_flow_levels); + } + + transforms->input_images = new_img_vec; + + addPoints(); + filterPoints(); + + } else { + t_ns = curr_t_ns; + + old_pyramid = pyramid; + + pyramid.reset(new std::vector>); + pyramid->resize(calib.intrinsics.size()); + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img, + config.optical_flow_levels); + } + + OpticalFlowResult::Ptr new_transforms; + new_transforms.reset(new OpticalFlowResult); + new_transforms->observations.resize(new_img_vec->img_data.size()); + new_transforms->t_ns = t_ns; + + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + trackPoints(old_pyramid->at(i), pyramid->at(i), + transforms->observations[i], + new_transforms->observations[i]); + } + + transforms = new_transforms; + transforms->input_images = new_img_vec; + + addPoints(); + filterPoints(); + } + + if (output_queue) { + output_queue->push(transforms); + } + } + + void trackPoints( + const basalt::ManagedImagePyr& pyr_1, + const basalt::ManagedImagePyr& pyr_2, + const Eigen::map& transform_map_1, + Eigen::map& transform_map_2) const { + size_t num_points = transform_map_1.size(); + + std::vector ids; + Eigen::vector init_vec; + + ids.reserve(num_points); + init_vec.reserve(num_points); + + for (const auto& kv : transform_map_1) { + ids.push_back(kv.first); + init_vec.push_back(kv.second); + } + + tbb::concurrent_unordered_map result; + + auto compute_func = [&](const tbb::blocked_range& range) { + for (size_t r = range.begin(); r != range.end(); ++r) { + const KeypointId id = ids[r]; + + const Eigen::AffineCompact2f& transform_1 = init_vec[r]; + Eigen::AffineCompact2f transform_2 = transform_1; + + const Eigen::vector& patch_vec = patches.at(id); + + bool valid = trackPoint(pyr_2, patch_vec, transform_2); + + if (valid) { + Eigen::AffineCompact2f transform_1_recovered = transform_2; + + valid = trackPoint(pyr_1, patch_vec, transform_1_recovered); + + if (valid) { + Scalar dist2 = (transform_1.translation() - + transform_1_recovered.translation()) + .squaredNorm(); + + if (dist2 < config.optical_flow_max_recovered_dist2) { + result[id] = transform_2; + } + } + } + } + }; + + tbb::blocked_range range(0, num_points); + + tbb::parallel_for(range, compute_func); + // compute_func(range); + + transform_map_2.clear(); + transform_map_2.insert(result.begin(), result.end()); + } + + inline bool trackPoint(const basalt::ManagedImagePyr& pyr, + const Eigen::vector& patch_vec, + Eigen::AffineCompact2f& transform) const { + bool patch_valid = true; + + for (int level = config.optical_flow_levels; level >= 0 && patch_valid; + level--) { + const Scalar scale = 1 << level; + + transform.translation() /= scale; + + // Perform tracking on current level + patch_valid &= + trackPointAtLevel(pyr.lvl(level), patch_vec[level], transform); + + transform.translation() *= scale; + } + + return patch_valid; + } + + inline bool trackPointAtLevel(const Image& img_2, + const PatchT& dp, + Eigen::AffineCompact2f& transform) const { + bool patch_valid = true; + + for (int iteration = 0; + patch_valid && iteration < config.optical_flow_max_iterations; + iteration++) { + typename PatchT::VectorP res; + + typename PatchT::Matrix2P transformed_pat = + transform.linear().matrix() * PatchT::pattern2; + transformed_pat.colwise() += transform.translation(); + + bool valid = dp.residual(img_2, transformed_pat, res); + + if (valid) { + Vector3 inc = -dp.H_se2_inv_J_se2_T * res; + transform *= SE2::exp(inc).matrix(); + + const int filter_margin = 2; + + if (!img_2.InBounds(transform.translation(), filter_margin)) + patch_valid = false; + } else { + patch_valid = false; + } + } + + return patch_valid; + } + + void addPoints() { + Eigen::vector pts0; + + for (const auto& kv : transforms->observations.at(0)) { + pts0.emplace_back(kv.second.translation().cast()); + } + + KeypointsData kd; + + detectKeypoints(pyramid->at(0).lvl(0), kd, + config.optical_flow_detection_grid_size, 1, pts0); + + Eigen::map new_poses0, new_poses1; + + for (size_t i = 0; i < kd.corners.size(); i++) { + Eigen::vector& p = patches[last_keypoint_id]; + + Vector2 pos = kd.corners[i].cast(); + + for (int l = 0; l < 4; l++) { + Scalar scale = 1 << l; + Vector2 pos_scaled = pos / scale; + p.emplace_back(pyramid->at(0).lvl(l), pos_scaled); + } + + Eigen::AffineCompact2f transform; + transform.setIdentity(); + transform.translation() = kd.corners[i].cast(); + + transforms->observations.at(0)[last_keypoint_id] = transform; + new_poses0[last_keypoint_id] = transform; + + last_keypoint_id++; + } + + if (calib.intrinsics.size() > 1) { + trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1); + + for (const auto& kv : new_poses1) { + transforms->observations.at(1).emplace(kv); + } + } + } + + void filterPoints() { + if (calib.intrinsics.size() < 2) return; + + std::set lm_to_remove; + + std::vector kpid; + Eigen::vector proj0, proj1; + + for (const auto& kv : transforms->observations.at(1)) { + auto it = transforms->observations.at(0).find(kv.first); + + if (it != transforms->observations.at(0).end()) { + proj0.emplace_back(it->second.translation().cast()); + proj1.emplace_back(kv.second.translation().cast()); + kpid.emplace_back(kv.first); + } + } + + Eigen::vector p3d0, p3d1; + std::vector p3d0_success, p3d1_success; + + calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); + calib.intrinsics[1].unproject(proj1, p3d1, p3d1_success); + + for (size_t i = 0; i < p3d0_success.size(); i++) { + if (p3d0_success[i] && p3d1_success[i]) { + const double epipolar_error = + std::abs(p3d0[i].transpose() * E * p3d1[i]); + + if (epipolar_error > config.optical_flow_epipolar_error) { + lm_to_remove.emplace(kpid[i]); + } + } else { + lm_to_remove.emplace(kpid[i]); + } + } + + for (int id : lm_to_remove) { + transforms->observations.at(1).erase(id); + } + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + int64_t t_ns; + + KeypointId last_keypoint_id; + + VioConfig config; + basalt::Calibration calib; + + Eigen::unordered_map> patches; + + OpticalFlowResult::Ptr transforms; + std::shared_ptr>> old_pyramid, + pyramid; + + Eigen::Matrix4d E; + + std::shared_ptr processing_thread; +}; + +} // namespace basalt diff --git a/include/basalt/optical_flow/patterns.h b/include/basalt/optical_flow/patterns.h new file mode 100644 index 0000000..0b05102 --- /dev/null +++ b/include/basalt/optical_flow/patterns.h @@ -0,0 +1,171 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +namespace basalt { + +template +struct Pattern24 { + // 00 01 + // + // 02 03 04 05 + // + // 06 07 08 09 10 11 + // + // 12 13 14 15 16 17 + // + // 18 19 20 21 + // + // 22 23 + // + // -----> x + // | + // | + // y + + static constexpr Scalar pattern_raw[][2] = { + {-1, 5}, {1, 5}, + + {-3, 3}, {-1, 3}, {1, 3}, {3, 3}, + + {-5, 1}, {-3, 1}, {-1, 1}, {1, 1}, {3, 1}, {5, 1}, + + {-5, -1}, {-3, -1}, {-1, -1}, {1, -1}, {3, -1}, {5, -1}, + + {-3, -3}, {-1, -3}, {1, -3}, {3, -3}, + + {-1, -5}, {1, -5} + + }; + + static constexpr int PATTERN_SIZE = + sizeof(pattern_raw) / (2 * sizeof(Scalar)); + + typedef Eigen::Matrix Matrix2P; + static const Matrix2P pattern2; +}; + +template +const typename Pattern24::Matrix2P Pattern24::pattern2 = + Eigen::Map::Matrix2P>((Scalar *) + Pattern24::pattern_raw); + +template +struct Pattern52 { + // 00 01 02 03 + // + // 04 05 06 07 08 09 + // + // 10 11 12 13 14 15 16 17 + // + // 18 19 20 21 22 23 24 25 + // + // 26 27 28 29 30 31 32 33 + // + // 34 35 36 37 38 39 40 41 + // + // 42 43 44 45 46 47 + // + // 48 49 50 51 + // + // -----> x + // | + // | + // y + + static constexpr Scalar pattern_raw[][2] = { + {-3, 7}, {-1, 7}, {1, 7}, {3, 7}, + + {-5, 5}, {-3, 5}, {-1, 5}, {1, 5}, {3, 5}, {5, 5}, + + {-7, 3}, {-5, 3}, {-3, 3}, {-1, 3}, {1, 3}, {3, 3}, + {5, 3}, {7, 3}, + + {-7, 1}, {-5, 1}, {-3, 1}, {-1, 1}, {1, 1}, {3, 1}, + {5, 1}, {7, 1}, + + {-7, -1}, {-5, -1}, {-3, -1}, {-1, -1}, {1, -1}, {3, -1}, + {5, -1}, {7, -1}, + + {-7, -3}, {-5, -3}, {-3, -3}, {-1, -3}, {1, -3}, {3, -3}, + {5, -3}, {7, -3}, + + {-5, -5}, {-3, -5}, {-1, -5}, {1, -5}, {3, -5}, {5, -5}, + + {-3, -7}, {-1, -7}, {1, -7}, {3, -7} + + }; + + static constexpr int PATTERN_SIZE = + sizeof(pattern_raw) / (2 * sizeof(Scalar)); + + typedef Eigen::Matrix Matrix2P; + static const Matrix2P pattern2; +}; + +template +const typename Pattern52::Matrix2P Pattern52::pattern2 = + Eigen::Map::Matrix2P>((Scalar *) + Pattern52::pattern_raw); + +// Same as Pattern52 but twice smaller +template +struct Pattern51 { + static constexpr int PATTERN_SIZE = Pattern52::PATTERN_SIZE; + + typedef Eigen::Matrix Matrix2P; + static const Matrix2P pattern2; +}; + +template +const typename Pattern51::Matrix2P Pattern51::pattern2 = + 0.5 * Pattern52::pattern2; + +// Same as Pattern52 but 0.75 smaller +template +struct Pattern50 { + static constexpr int PATTERN_SIZE = Pattern52::PATTERN_SIZE; + + typedef Eigen::Matrix Matrix2P; + static const Matrix2P pattern2; +}; + +template +const typename Pattern50::Matrix2P Pattern50::pattern2 = + 0.75 * Pattern52::pattern2; + +} // namespace basalt diff --git a/include/basalt/optimization/accumulator.h b/include/basalt/optimization/accumulator.h new file mode 100644 index 0000000..2e12356 --- /dev/null +++ b/include/basalt/optimization/accumulator.h @@ -0,0 +1,336 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#ifndef BASALT_ACCUMULATOR_H +#define BASALT_ACCUMULATOR_H + +#include +#include + +#include +#include +#include + +#include + +namespace basalt { + +template +class DenseAccumulator { + public: + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + + template + inline void addH(int i, int j, const Eigen::MatrixBase& data) { + BASALT_ASSERT_STREAM(i >= 0, "i " << i); + BASALT_ASSERT_STREAM(j >= 0, "j " << j); + + BASALT_ASSERT_STREAM(i + ROWS <= H.cols(), "i " << i << " ROWS " << ROWS + << " H.rows() " + << H.rows()); + BASALT_ASSERT_STREAM(j + COLS <= H.rows(), "j " << j << " COLS " << COLS + << " H.cols() " + << H.cols()); + + H.template block(i, j) += data; + } + + template + inline void addB(int i, const Eigen::MatrixBase& data) { + BASALT_ASSERT_STREAM(i >= 0, "i " << i); + + BASALT_ASSERT_STREAM(i + ROWS <= H.cols(), "i " << i << " ROWS " << ROWS + << " H.rows() " + << H.rows()); + + b.template segment(i) += data; + } + + inline VectorX solve() const { return H.ldlt().solve(b); } + + inline VectorX solveWithPrior(const MatrixX& H_prior, const VectorX& b_prior, + size_t pos) const { + const int prior_size = b_prior.rows(); + + BASALT_ASSERT_STREAM( + H_prior.cols() == prior_size, + "H_prior.cols() " << H_prior.cols() << " prior_size " << prior_size); + BASALT_ASSERT_STREAM( + H_prior.rows() == prior_size, + "H_prior.rows() " << H_prior.rows() << " prior_size " << prior_size); + + MatrixX H_new = H; + VectorX b_new = b; + + H_new.block(pos, pos, prior_size, prior_size) += H_prior; + b_new.segment(pos, prior_size) += b_prior; + + return H_new.ldlt().solve(-b_new); + } + + inline void reset(int opt_size) { + H.setZero(opt_size, opt_size); + b.setZero(opt_size); + } + + inline void join(const DenseAccumulator& other) { + H += other.H; + b += other.b; + } + + inline void print() { + Eigen::IOFormat CleanFmt(2); + std::cerr << "H\n" << H.format(CleanFmt) << std::endl; + std::cerr << "b\n" << b.transpose().format(CleanFmt) << std::endl; + } + + inline const MatrixX& getH() const { return H; } + inline const VectorX& getB() const { return b; } + + inline MatrixX& getH() { return H; } + inline VectorX& getB() { return b; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + MatrixX H; + VectorX b; +}; + +template +class SparseAccumulator { + public: + typedef Eigen::Matrix VectorX; + typedef Eigen::Triplet T; + typedef Eigen::SparseMatrix SparseMatrix; + + template + inline void addH(int si, int sj, const Eigen::MatrixBase& data) { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, ROWS, COLS); + + for (int i = 0; i < ROWS; i++) { + for (int j = 0; j < COLS; j++) { + triplets.emplace_back(si + i, sj + j, data(i, j)); + } + } + } + + template + inline void addB(int i, const Eigen::MatrixBase& data) { + b.template segment(i) += data; + } + + inline VectorX solve() const { + SparseMatrix sm(b.rows(), b.rows()); + + auto triplets_copy = triplets; + for (int i = 0; i < b.rows(); i++) { + triplets_copy.emplace_back(i, i, 0.000001); + } + + sm.setFromTriplets(triplets_copy.begin(), triplets_copy.end()); + + // Eigen::IOFormat CleanFmt(2); + // std::cerr << "sm\n" << sm.toDense().format(CleanFmt) << std::endl; + + Eigen::SimplicialLDLT chol(sm); + return chol.solve(-b); + // return sm.toDense().ldlt().solve(-b); + } + + inline void reset(int opt_size) { + triplets.clear(); + b.setZero(opt_size); + } + + inline void join(const SparseAccumulator& other) { + triplets.reserve(triplets.size() + other.triplets.size()); + triplets.insert(triplets.end(), other.triplets.begin(), + other.triplets.end()); + b += other.b; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + std::vector triplets; + VectorX b; +}; + +template +class SparseHashAccumulator { + public: + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + typedef Eigen::Triplet T; + typedef Eigen::SparseMatrix SparseMatrix; + + template + inline void addH(int si, int sj, const Eigen::MatrixBase& data) { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, ROWS, COLS); + + KeyT id; + id[0] = si; + id[1] = sj; + id[2] = ROWS; + id[3] = COLS; + + auto it = hash_map.find(id); + + if (it == hash_map.end()) { + hash_map.emplace(id, data); + } else { + it->second += data; + } + } + + template + inline void addB(int i, const Eigen::MatrixBase& data) { + b.template segment(i) += data; + } + + inline VectorX solve() const { + SparseMatrix sm(b.rows(), b.rows()); + + auto t1 = std::chrono::high_resolution_clock::now(); + std::vector triplets; + triplets.reserve(hash_map.size() * 36 + b.rows()); + + for (int i = 0; i < b.rows(); i++) { + triplets.emplace_back(i, i, 0.000001); + } + + for (const auto& kv : hash_map) { + // if (kv.first[2] != kv.second.rows()) std::cerr << "rows mismatch" << + // std::endl; + // if (kv.first[3] != kv.second.cols()) std::cerr << "cols mismatch" << + // std::endl; + + for (int i = 0; i < kv.second.rows(); i++) { + for (int j = 0; j < kv.second.cols(); j++) { + triplets.emplace_back(kv.first[0] + i, kv.first[1] + j, + kv.second(i, j)); + } + } + } + + sm.setFromTriplets(triplets.begin(), triplets.end()); + + auto t2 = std::chrono::high_resolution_clock::now(); + + // sm.diagonal() *= 1.01; + + // Eigen::IOFormat CleanFmt(2); + // std::cerr << "sm\n" << sm.toDense().format(CleanFmt) << std::endl; + + VectorX res; + + if (iterative_solver) { + Eigen::ConjugateGradient, + Eigen::Lower | Eigen::Upper> + cg; + + cg.setTolerance(tolerance); + cg.compute(sm); + res = cg.solve(b); + } else { + Eigen::SimplicialLDLT chol(sm); + res = chol.solve(b); + } + + auto t3 = std::chrono::high_resolution_clock::now(); + + auto elapsed1 = + std::chrono::duration_cast(t2 - t1); + auto elapsed2 = + std::chrono::duration_cast(t3 - t2); + + if (print_info) { + std::cout << "Forming matrix: " << elapsed1.count() * 1e-6 + << "s. Solving linear system: " << elapsed2.count() * 1e-6 + << "s." << std::endl; + } + + return res; + } + + inline void reset(int opt_size) { + hash_map.clear(); + b.setZero(opt_size); + } + + inline void join(const SparseHashAccumulator& other) { + for (const auto& kv : other.hash_map) { + auto it = hash_map.find(kv.first); + + if (it == hash_map.end()) { + hash_map.emplace(kv.first, kv.second); + } else { + it->second += kv.second; + } + } + + b += other.b; + } + + double tolerance = 1e-4; + bool iterative_solver = false; + bool print_info = false; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + using KeyT = std::array; + + struct KeyHash { + inline size_t operator()(const KeyT& c) const { + size_t seed = 0; + for (int i = 0; i < 4; i++) { + hash_combine(seed, std::hash()(c[i])); + } + return seed; + } + + inline void hash_combine(std::size_t& seed, std::size_t value) const { + seed ^= value + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + }; + + std::unordered_map hash_map; + + VectorX b; +}; + +} // namespace basalt + +#endif diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h new file mode 100644 index 0000000..2927966 --- /dev/null +++ b/include/basalt/optimization/linearize.h @@ -0,0 +1,187 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef BASALT_LINEARIZE_H +#define BASALT_LINEARIZE_H + +#include +#include +#include +#include + +#include + +#include + +namespace basalt { + +template +struct LinearizeBase { + static const int POSE_SIZE = 6; + + static const int POS_SIZE = 3; + static const int POS_OFFSET = 0; + static const int ROT_SIZE = 3; + static const int ROT_OFFSET = 3; + static const int ACCEL_BIAS_SIZE = 9; + static const int GYRO_BIAS_SIZE = 12; + static const int G_SIZE = 3; + + static const int TIME_SIZE = 1; + + static const int ACCEL_BIAS_OFFSET = 0; + static const int GYRO_BIAS_OFFSET = ACCEL_BIAS_SIZE; + static const int G_OFFSET = GYRO_BIAS_OFFSET + GYRO_BIAS_SIZE; + + typedef Sophus::SE3 SE3; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector6; + + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Matrix6; + + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + + typedef typename Eigen::vector::const_iterator PoseDataIter; + typedef typename Eigen::vector::const_iterator GyroDataIter; + typedef typename Eigen::vector::const_iterator AccelDataIter; + typedef typename Eigen::vector::const_iterator + AprilgridCornersDataIter; + + template + struct PoseCalibH { + Sophus::Matrix6d H_pose_accum; + Sophus::Vector6d b_pose_accum; + Eigen::Matrix H_intr_pose_accum; + Eigen::Matrix H_intr_accum; + Eigen::Matrix b_intr_accum; + + PoseCalibH() { + H_pose_accum.setZero(); + b_pose_accum.setZero(); + H_intr_pose_accum.setZero(); + H_intr_accum.setZero(); + b_intr_accum.setZero(); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + struct CalibCommonData { + const Calibration* calibration = nullptr; + const MocapCalibration* mocap_calibration = nullptr; + const Eigen::vector* aprilgrid_corner_pos_3d = nullptr; + + // Calib data + const std::vector* offset_T_i_c = nullptr; + const std::vector* offset_intrinsics = nullptr; + + // Cam data + size_t mocap_block_offset; + size_t bias_block_offset; + const std::unordered_map* offset_poses = nullptr; + + // Cam-IMU data + const Vector3* g = nullptr; + + bool opt_intrinsics; + + // Cam-IMU options + bool opt_cam_time_offset; + bool opt_g; + bool opt_imu_scale; + + Scalar pose_var_inv; + Scalar gyro_var_inv; + Scalar accel_var_inv; + Scalar mocap_var_inv; + + Scalar huber_thresh; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + template + inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id, + const Eigen::Matrix4d& T_c_w, const CamT& cam, + PoseCalibH& cph, double& error, + int& num_points, double& reproj_err) const { + Eigen::Matrix d_r_d_p; + Eigen::Matrix d_r_d_param; + Eigen::Matrix d_point_d_xi; + + BASALT_ASSERT_STREAM( + corner_id < int(common_data.aprilgrid_corner_pos_3d->size()), + "corner_id " << corner_id); + + Eigen::Vector4d point3d = + T_c_w * common_data.aprilgrid_corner_pos_3d->at(corner_id); + + d_point_d_xi.topLeftCorner<3, 3>().setIdentity(); + d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(point3d.head<3>()); + d_point_d_xi.row(3).setZero(); + + Eigen::Vector2d proj; + bool valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param); + + if (!valid) return; + + Eigen::Matrix d_r_d_xi = d_r_d_p * d_point_d_xi; + Eigen::Vector2d residual = proj - corner_pos; + + double e = residual.norm(); + double huber_weight = + e < common_data.huber_thresh ? 1.0 : common_data.huber_thresh / e; + + cph.H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi; + cph.b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual; + + cph.H_intr_pose_accum += huber_weight * d_r_d_param.transpose() * d_r_d_xi; + cph.H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param; + cph.b_intr_accum += huber_weight * d_r_d_param.transpose() * residual; + + error += huber_weight * e * e * (2 - huber_weight); + reproj_err += e; + num_points++; + } + + CalibCommonData common_data; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace basalt + +#endif diff --git a/include/basalt/optimization/poses_linearize.h b/include/basalt/optimization/poses_linearize.h new file mode 100644 index 0000000..510f229 --- /dev/null +++ b/include/basalt/optimization/poses_linearize.h @@ -0,0 +1,180 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef BASALT_POSES_LINEARIZE_H +#define BASALT_POSES_LINEARIZE_H + +#include +#include +#include + +#include + +#include + +#include + +namespace basalt { + +template +struct LinearizePosesOpt : public LinearizeBase { + static const int POSE_SIZE = LinearizeBase::POSE_SIZE; + + typedef Sophus::SE3 SE3; + typedef Eigen::Matrix Vector2; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector6; + + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Matrix6; + + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + + typedef typename Eigen::vector::const_iterator + AprilgridCornersDataIter; + + typedef typename LinearizeBase::CalibCommonData CalibCommonData; + + AccumT accum; + Scalar error; + Scalar reprojection_error; + int num_points; + + size_t opt_size; + + const Eigen::unordered_map& timestam_to_pose; + + LinearizePosesOpt(size_t opt_size, + const Eigen::unordered_map& timestam_to_pose, + const CalibCommonData& common_data) + : opt_size(opt_size), timestam_to_pose(timestam_to_pose) { + this->common_data = common_data; + accum.reset(opt_size); + error = 0; + reprojection_error = 0; + num_points = 0; + } + LinearizePosesOpt(const LinearizePosesOpt& other, tbb::split) + : opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) { + this->common_data = other.common_data; + accum.reset(opt_size); + error = 0; + reprojection_error = 0; + num_points = 0; + } + + void operator()(const tbb::blocked_range& r) { + for (const AprilgridCornersData& acd : r) { + std::visit( + [&](const auto& cam) { + constexpr int INTRINSICS_SIZE = + std::remove_reference::type::N; + typename LinearizeBase::template PoseCalibH + cph; + + SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns); + SE3 T_w_c = + T_w_i * this->common_data.calibration->T_i_c[acd.cam_id]; + SE3 T_c_w = T_w_c.inverse(); + Eigen::Matrix4d T_c_w_m = T_c_w.matrix(); + + double err = 0; + double reproj_err = 0; + int num_inliers = 0; + + for (size_t i = 0; i < acd.corner_pos.size(); i++) { + this->linearize_point(acd.corner_pos[i], acd.corner_id[i], + T_c_w_m, cam, cph, err, num_inliers, + reproj_err); + } + + error += err; + reprojection_error += reproj_err; + num_points += num_inliers; + + const Matrix6 Adj = + -this->common_data.calibration->T_i_c[acd.cam_id] + .inverse() + .Adj(); + + const size_t po = + this->common_data.offset_poses->at(acd.timestamp_ns); + const size_t co = this->common_data.offset_T_i_c->at(acd.cam_id); + const size_t io = + this->common_data.offset_intrinsics->at(acd.cam_id); + + accum.template addH( + po, po, Adj.transpose() * cph.H_pose_accum * Adj); + accum.template addB(po, + Adj.transpose() * cph.b_pose_accum); + + if (acd.cam_id > 0) { + accum.template addH( + co, po, -cph.H_pose_accum * Adj); + accum.template addH(co, co, + cph.H_pose_accum); + + accum.template addB(co, -cph.b_pose_accum); + } + + if (this->common_data.opt_intrinsics) { + accum.template addH( + io, po, cph.H_intr_pose_accum * Adj); + + if (acd.cam_id > 0) + accum.template addH( + io, co, -cph.H_intr_pose_accum); + + accum.template addH( + io, io, cph.H_intr_accum); + accum.template addB(io, cph.b_intr_accum); + } + }, + this->common_data.calibration->intrinsics[acd.cam_id].variant); + } + } + + void join(LinearizePosesOpt& rhs) { + accum.join(rhs.accum); + error += rhs.error; + reprojection_error += rhs.reprojection_error; + num_points += rhs.num_points; + } +}; + +} // namespace basalt + +#endif diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h new file mode 100644 index 0000000..d0ad7a3 --- /dev/null +++ b/include/basalt/optimization/poses_optimize.h @@ -0,0 +1,259 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include + +#include + +namespace basalt { + +class PosesOptimization { + static constexpr size_t POSE_SIZE = 6; + + using Scalar = double; + + typedef LinearizePosesOpt> LinearizeT; + + using SE3 = typename LinearizeT::SE3; + using Vector2 = typename LinearizeT::Vector2; + using Vector3 = typename LinearizeT::Vector3; + using Vector4 = typename LinearizeT::Vector4; + using VectorX = typename LinearizeT::VectorX; + + using AprilgridCornersDataIter = + typename Eigen::vector::const_iterator; + + public: + PosesOptimization() {} + + bool initializeIntrinsics( + size_t cam_id, const Eigen::vector &corners, + const std::vector &corner_ids, + const Eigen::vector &aprilgrid_corner_pos_3d, int cols, + int rows) { + Eigen::Vector4d init_intr; + + bool val = CalibHelper::initializeIntrinsics( + corners, corner_ids, aprilgrid_corner_pos_3d, cols, rows, init_intr); + + calib->intrinsics[cam_id].setFromInit(init_intr); + + return val; + } + + Vector2 getOpticalCenter(size_t i) { + return calib->intrinsics[i].getParam().template segment<2>(2); + } + + void resetCalib(size_t num_cams, const std::vector &cam_types) { + BASALT_ASSERT(cam_types.size() == num_cams); + + calib.reset(new Calibration); + + for (size_t i = 0; i < cam_types.size(); i++) { + calib->intrinsics.emplace_back( + GenericCamera::fromString(cam_types[i])); + + if (calib->intrinsics.back().getName() != cam_types[i]) { + std::cerr << "Unknown camera type " << cam_types[i] << " default to " + << calib->intrinsics.back().getName() << std::endl; + } + } + calib->T_i_c.resize(num_cams); + } + + void loadCalib(const std::string &p) { + std::string path = p + "calibration.json"; + + std::ifstream is(path); + + if (is.good()) { + cereal::JSONInputArchive archive(is); + calib.reset(new Calibration); + archive(*calib); + std::cout << "Loaded calibration from: " << path << std::endl; + } else { + std::cout << "No calibration found" << std::endl; + } + } + + void saveCalib(const std::string &path, + int64_t mocap_to_imu_offset_ns = 0) const { + if (calib) { + std::ofstream os(path + "calibration.json"); + cereal::JSONOutputArchive archive(os); + + archive(*calib); + } + } + + bool calibInitialized() const { return calib != nullptr; } + bool initialized() const { return true; } + + void optimize(bool opt_intrinsics, double huber_thresh, double &error, + int &num_points, double &reprojection_error) { + error = 0; + num_points = 0; + + ccd.opt_intrinsics = opt_intrinsics; + ccd.huber_thresh = huber_thresh; + + LinearizePosesOpt> lopt( + problem_size, timestam_to_pose, ccd); + + tbb::blocked_range april_range( + aprilgrid_corners_measurements.begin(), + aprilgrid_corners_measurements.end()); + tbb::parallel_reduce(april_range, lopt); + + error = lopt.error; + num_points = lopt.num_points; + reprojection_error = lopt.reprojection_error; + + Eigen::VectorXd inc = -lopt.accum.solve(); + + for (auto &kv : timestam_to_pose) { + kv.second *= Sophus::expd(inc.segment(offset_poses[kv.first])); + } + + for (size_t i = 0; i < calib->T_i_c.size(); i++) { + calib->T_i_c[i] *= Sophus::expd(inc.segment(offset_T_i_c[i])); + } + + for (size_t i = 0; i < calib->intrinsics.size(); i++) { + auto &c = calib->intrinsics[i]; + c.applyInc(inc.segment(offset_cam_intrinsics[i], c.getN())); + } + } + + void recompute_size() { + offset_poses.clear(); + offset_T_i_c.clear(); + offset_cam_intrinsics.clear(); + + size_t curr_offset = 0; + + for (const auto &kv : timestam_to_pose) { + offset_poses[kv.first] = curr_offset; + curr_offset += POSE_SIZE; + } + + offset_T_i_c.emplace_back(curr_offset); + for (size_t i = 0; i < calib->T_i_c.size(); i++) + offset_T_i_c.emplace_back(offset_T_i_c.back() + POSE_SIZE); + + offset_cam_intrinsics.emplace_back(offset_T_i_c.back()); + for (size_t i = 0; i < calib->intrinsics.size(); i++) + offset_cam_intrinsics.emplace_back(offset_cam_intrinsics.back() + + calib->intrinsics[i].getN()); + + problem_size = offset_cam_intrinsics.back(); + } + + Sophus::SE3d getT_w_i(int64_t i) { + auto it = timestam_to_pose.find(i); + + if (it == timestam_to_pose.end()) + return Sophus::SE3d(); + else + return it->second; + } + + void setAprilgridCorners3d(const Eigen::vector &v) { + aprilgrid_corner_pos_3d = v; + } + + void init() { + recompute_size(); + + ccd.calibration = calib.get(); + ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d; + ccd.offset_poses = &offset_poses; + ccd.offset_T_i_c = &offset_T_i_c; + ccd.offset_intrinsics = &offset_cam_intrinsics; + } + + void addAprilgridMeasurement( + int64_t t_ns, int cam_id, + const Eigen::vector &corners_pos, + const std::vector &corner_id) { + aprilgrid_corners_measurements.emplace_back(); + + aprilgrid_corners_measurements.back().timestamp_ns = t_ns; + aprilgrid_corners_measurements.back().cam_id = cam_id; + aprilgrid_corners_measurements.back().corner_pos = corners_pos; + aprilgrid_corners_measurements.back().corner_id = corner_id; + } + + void addPoseMeasurement(int64_t t_ns, const Sophus::SE3d &pose) { + timestam_to_pose[t_ns] = pose; + } + + void setVignette(const std::vector> &vign) { + calib->vignette = vign; + } + + void setResolution(const Eigen::vector &resolution) { + calib->resolution = resolution; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::shared_ptr> calib; + + private: + typename LinearizePosesOpt< + Scalar, SparseHashAccumulator>::CalibCommonData ccd; + + size_t problem_size; + + std::unordered_map offset_poses; + std::vector offset_cam_intrinsics; + std::vector offset_T_i_c; + + // frame poses + Eigen::unordered_map timestam_to_pose; + + Eigen::vector aprilgrid_corners_measurements; + + Eigen::vector aprilgrid_corner_pos_3d; + +}; // namespace basalt + +} // namespace basalt diff --git a/include/basalt/optimization/spline_linearize.h b/include/basalt/optimization/spline_linearize.h new file mode 100644 index 0000000..cf0eb7d --- /dev/null +++ b/include/basalt/optimization/spline_linearize.h @@ -0,0 +1,635 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef BASALT_SPLINE_LINEARIZE_H +#define BASALT_SPLINE_LINEARIZE_H + +#include +#include +#include +#include + +#include + +#include + +#include + +namespace basalt { + +template +struct LinearizeSplineOpt : public LinearizeBase { + static const int POSE_SIZE = LinearizeBase::POSE_SIZE; + static const int POS_SIZE = LinearizeBase::POS_SIZE; + static const int POS_OFFSET = LinearizeBase::POS_OFFSET; + static const int ROT_SIZE = LinearizeBase::ROT_SIZE; + static const int ROT_OFFSET = LinearizeBase::ROT_OFFSET; + static const int ACCEL_BIAS_SIZE = LinearizeBase::ACCEL_BIAS_SIZE; + static const int GYRO_BIAS_SIZE = LinearizeBase::GYRO_BIAS_SIZE; + static const int G_SIZE = LinearizeBase::G_SIZE; + static const int TIME_SIZE = LinearizeBase::TIME_SIZE; + static const int ACCEL_BIAS_OFFSET = LinearizeBase::ACCEL_BIAS_OFFSET; + static const int GYRO_BIAS_OFFSET = LinearizeBase::GYRO_BIAS_OFFSET; + static const int G_OFFSET = LinearizeBase::G_OFFSET; + + typedef Sophus::SE3 SE3; + + typedef Eigen::Matrix Vector2; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector6; + + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Matrix6; + + typedef Eigen::Matrix Matrix24; + + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + + typedef Se3Spline SplineT; + + typedef typename Eigen::deque::const_iterator PoseDataIter; + typedef typename Eigen::deque::const_iterator GyroDataIter; + typedef typename Eigen::deque::const_iterator AccelDataIter; + typedef typename Eigen::deque::const_iterator + AprilgridCornersDataIter; + typedef + typename Eigen::deque::const_iterator MocapPoseDataIter; + + // typedef typename LinearizeBase::PoseCalibH PoseCalibH; + typedef typename LinearizeBase::CalibCommonData CalibCommonData; + + AccumT accum; + Scalar error; + Scalar reprojection_error; + int num_points; + + size_t opt_size; + + const SplineT* spline; + + LinearizeSplineOpt(size_t opt_size, const SplineT* spl, + const CalibCommonData& common_data, + const SplineT* spl_lin = nullptr) + : opt_size(opt_size), spline(spl) { + this->common_data = common_data; + + accum.reset(opt_size); + error = 0; + reprojection_error = 0; + num_points = 0; + + BASALT_ASSERT(spline); + } + + LinearizeSplineOpt(const LinearizeSplineOpt& other, tbb::split) + : opt_size(other.opt_size), spline(other.spline) { + this->common_data = other.common_data; + accum.reset(opt_size); + error = 0; + reprojection_error = 0; + num_points = 0; + } + + void operator()(const tbb::blocked_range& r) { + for (const PoseData& pm : r) { + int64_t start_idx; + + typename SplineT::SO3JacobianStruct J_rot; + typename SplineT::PosJacobianStruct J_pos; + + int64_t time_ns = pm.timestamp_ns; + + // std::cout << "time " << time << std::endl; + // std::cout << "sline.minTime() " << spline.minTime() << std::endl; + + BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(), + "time " << time_ns << " spline.minTimeNs() " + << spline->minTimeNs()); + + // Residual from current value of spline + Vector3 residual_pos = + spline->positionResidual(time_ns, pm.data.translation(), &J_pos); + Vector3 residual_rot = + spline->orientationResidual(time_ns, pm.data.so3(), &J_rot); + + // std::cerr << "residual_pos " << residual_pos.transpose() << std::endl; + // std::cerr << "residual_rot " << residual_rot.transpose() << std::endl; + + BASALT_ASSERT(J_pos.start_idx == J_rot.start_idx); + + start_idx = J_pos.start_idx; + + // std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl; + + const Scalar& pose_var_inv = this->common_data.pose_var_inv; + + error += pose_var_inv * + (residual_pos.squaredNorm() + residual_rot.squaredNorm()); + + for (size_t i = 0; i < N; i++) { + size_t start_i = (start_idx + i) * POSE_SIZE; + + // std::cout << "start_idx " << start_idx << std::endl; + // std::cout << "start_i " << start_i << std::endl; + + BASALT_ASSERT(start_i < opt_size); + + for (size_t j = 0; j <= i; j++) { + size_t start_j = (start_idx + j) * POSE_SIZE; + + BASALT_ASSERT(start_j < opt_size); + + accum.template addH( + start_i + POS_OFFSET, start_j + POS_OFFSET, + this->common_data.pose_var_inv * J_pos.d_val_d_knot[i] * + J_pos.d_val_d_knot[j] * Matrix3::Identity()); + + accum.template addH( + start_i + ROT_OFFSET, start_j + ROT_OFFSET, + this->common_data.pose_var_inv * + J_rot.d_val_d_knot[i].transpose() * J_rot.d_val_d_knot[j]); + } + + accum.template addB( + start_i + POS_OFFSET, + pose_var_inv * J_pos.d_val_d_knot[i] * residual_pos); + accum.template addB( + start_i + ROT_OFFSET, + pose_var_inv * J_rot.d_val_d_knot[i].transpose() * residual_rot); + } + } + } + + void operator()(const tbb::blocked_range& r) { + // size_t num_knots = spline.numKnots(); + // size_t bias_block_offset = POSE_SIZE * num_knots; + + for (const AccelData& pm : r) { + typename SplineT::AccelPosSO3JacobianStruct J; + typename SplineT::Mat39 J_bias; + Matrix3 J_g; + + int64_t t = pm.timestamp_ns; + + // std::cout << "time " << t << std::endl; + // std::cout << "sline.minTime() " << spline.minTime() << std::endl; + + BASALT_ASSERT_STREAM(t >= spline->minTimeNs(), + "t " << t << " spline.minTime() " + << spline->minTimeNs()); + BASALT_ASSERT_STREAM(t <= spline->maxTimeNs(), + "t " << t << " spline.maxTime() " + << spline->maxTimeNs()); + + Vector3 residual = spline->accelResidual( + t, pm.data, this->common_data.calibration->calib_accel_bias, + *(this->common_data.g), &J, &J_bias, &J_g); + + if (!this->common_data.opt_imu_scale) { + J_bias.template block<3, 6>(0, 3).setZero(); + } + + // std::cerr << "================================" << std::endl; + // std::cerr << "accel res: " << residual.transpose() << std::endl; + // std::cerr << "accel_bias.transpose(): " + // << this->common_data.calibration->accel_bias.transpose() + // << std::endl; + // std::cerr << "*(this->common_data.g): " + // << this->common_data.g->transpose() << std::endl; + // std::cerr << "pm.data " << pm.data.transpose() << std::endl; + + // std::cerr << "time " << t << std::endl; + // std::cerr << "sline.minTime() " << spline.minTime() << std::endl; + // std::cerr << "sline.maxTime() " << spline.maxTime() << std::endl; + // std::cerr << "================================" << std::endl; + + const Scalar& accel_var_inv = this->common_data.accel_var_inv; + + error += accel_var_inv * residual.squaredNorm(); + + size_t start_bias = + this->common_data.bias_block_offset + ACCEL_BIAS_OFFSET; + size_t start_g = this->common_data.bias_block_offset + G_OFFSET; + + for (size_t i = 0; i < N; i++) { + size_t start_i = (J.start_idx + i) * POSE_SIZE; + + BASALT_ASSERT(start_i < opt_size); + + for (size_t j = 0; j <= i; j++) { + size_t start_j = (J.start_idx + j) * POSE_SIZE; + + BASALT_ASSERT(start_j < opt_size); + + accum.template addH( + start_i, start_j, accel_var_inv * J.d_val_d_knot[i].transpose() * + J.d_val_d_knot[j]); + } + accum.template addH( + start_bias, start_i, + accel_var_inv * J_bias.transpose() * J.d_val_d_knot[i]); + + if (this->common_data.opt_g) { + accum.template addH( + start_g, start_i, + accel_var_inv * J_g.transpose() * J.d_val_d_knot[i]); + } + + accum.template addB( + start_i, accel_var_inv * J.d_val_d_knot[i].transpose() * residual); + } + + accum.template addH( + start_bias, start_bias, accel_var_inv * J_bias.transpose() * J_bias); + + if (this->common_data.opt_g) { + accum.template addH( + start_g, start_bias, accel_var_inv * J_g.transpose() * J_bias); + accum.template addH( + start_g, start_g, accel_var_inv * J_g.transpose() * J_g); + } + + accum.template addB( + start_bias, accel_var_inv * J_bias.transpose() * residual); + + if (this->common_data.opt_g) { + accum.template addB(start_g, + accel_var_inv * J_g.transpose() * residual); + } + } + } + + void operator()(const tbb::blocked_range& r) { + // size_t num_knots = spline.numKnots(); + // size_t bias_block_offset = POSE_SIZE * num_knots; + + for (const GyroData& pm : r) { + typename SplineT::SO3JacobianStruct J; + typename SplineT::Mat312 J_bias; + + int64_t t_ns = pm.timestamp_ns; + + BASALT_ASSERT(t_ns >= spline->minTimeNs()); + BASALT_ASSERT(t_ns <= spline->maxTimeNs()); + + const Scalar& gyro_var_inv = this->common_data.gyro_var_inv; + + Vector3 residual = spline->gyroResidual( + t_ns, pm.data, this->common_data.calibration->calib_gyro_bias, &J, + &J_bias); + + if (!this->common_data.opt_imu_scale) { + J_bias.template block<3, 9>(0, 3).setZero(); + } + + // std::cerr << "==============================" << std::endl; + // std::cerr << "residual " << residual.transpose() << std::endl; + // std::cerr << "gyro_bias " + // << this->common_data.calibration->gyro_bias.transpose() + // << std::endl; + // std::cerr << "pm.data " << pm.data.transpose() << std::endl; + // std::cerr << "t_ns " << t_ns << std::endl; + + error += gyro_var_inv * residual.squaredNorm(); + + size_t start_bias = + this->common_data.bias_block_offset + GYRO_BIAS_OFFSET; + for (size_t i = 0; i < N; i++) { + size_t start_i = (J.start_idx + i) * POSE_SIZE + ROT_OFFSET; + + // std::cout << "start_idx " << J.start_idx << std::endl; + // std::cout << "start_i " << start_i << std::endl; + + BASALT_ASSERT(start_i < opt_size); + + for (size_t j = 0; j <= i; j++) { + size_t start_j = (J.start_idx + j) * POSE_SIZE + ROT_OFFSET; + + // std::cout << "start_j " << start_j << std::endl; + + BASALT_ASSERT(start_i < opt_size); + + accum.template addH( + start_i, start_j, + gyro_var_inv * J.d_val_d_knot[i].transpose() * J.d_val_d_knot[j]); + } + accum.template addH( + start_bias, start_i, + gyro_var_inv * J_bias.transpose() * J.d_val_d_knot[i]); + accum.template addB( + start_i, gyro_var_inv * J.d_val_d_knot[i].transpose() * residual); + } + + accum.template addH( + start_bias, start_bias, gyro_var_inv * J_bias.transpose() * J_bias); + accum.template addB( + start_bias, gyro_var_inv * J_bias.transpose() * residual); + } + } + + void operator()(const tbb::blocked_range& r) { + for (const AprilgridCornersData& acd : r) { + std::visit( + [&](const auto& cam) { + constexpr int INTRINSICS_SIZE = + std::remove_reference::type::N; + + typename SplineT::PosePosSO3JacobianStruct J; + + int64_t time_ns = acd.timestamp_ns + + this->common_data.calibration->cam_time_offset_ns; + + if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs()) + return; + + SE3 T_w_i = spline->pose(time_ns, &J); + + Vector6 d_T_wi_d_time; + spline->d_pose_d_t(time_ns, d_T_wi_d_time); + + SE3 T_w_c = + T_w_i * this->common_data.calibration->T_i_c[acd.cam_id]; + SE3 T_c_w = T_w_c.inverse(); + Eigen::Matrix4d T_c_w_m = T_c_w.matrix(); + + typename LinearizeBase::template PoseCalibH + cph; + + double err = 0; + double reproj_err = 0; + int num_inliers = 0; + + for (size_t i = 0; i < acd.corner_pos.size(); i++) { + this->linearize_point(acd.corner_pos[i], acd.corner_id[i], + T_c_w_m, cam, cph, err, num_inliers, + reproj_err); + } + + error += err; + reprojection_error += reproj_err; + num_points += num_inliers; + + Matrix6 Adj = -this->common_data.calibration->T_i_c[acd.cam_id] + .inverse() + .Adj(); + Matrix6 A_H_p_A = Adj.transpose() * cph.H_pose_accum * Adj; + + size_t T_i_c_start = this->common_data.offset_T_i_c->at(acd.cam_id); + size_t calib_start = + this->common_data.offset_intrinsics->at(acd.cam_id); + size_t start_cam_time_offset = + this->common_data.mocap_block_offset + 2 * POSE_SIZE + 1; + + for (int i = 0; i < N; i++) { + int start_i = (J.start_idx + i) * POSE_SIZE; + + Matrix6 Ji_A_H_p_A = J.d_val_d_knot[i].transpose() * A_H_p_A; + + for (int j = 0; j <= i; j++) { + int start_j = (J.start_idx + j) * POSE_SIZE; + + accum.template addH( + start_i, start_j, Ji_A_H_p_A * J.d_val_d_knot[j]); + } + + accum.template addH( + T_i_c_start, start_i, + -cph.H_pose_accum * Adj * J.d_val_d_knot[i]); + + if (this->common_data.opt_intrinsics) + accum.template addH( + calib_start, start_i, + cph.H_intr_pose_accum * Adj * J.d_val_d_knot[i]); + + if (this->common_data.opt_cam_time_offset) + accum.template addH( + start_cam_time_offset, start_i, + d_T_wi_d_time.transpose() * A_H_p_A * J.d_val_d_knot[i]); + + accum.template addB( + start_i, J.d_val_d_knot[i].transpose() * Adj.transpose() * + cph.b_pose_accum); + } + + accum.template addH(T_i_c_start, T_i_c_start, + cph.H_pose_accum); + accum.template addB(T_i_c_start, -cph.b_pose_accum); + + if (this->common_data.opt_intrinsics) { + accum.template addH( + calib_start, T_i_c_start, -cph.H_intr_pose_accum); + accum.template addH( + calib_start, calib_start, cph.H_intr_accum); + accum.template addB(calib_start, + cph.b_intr_accum); + } + + if (this->common_data.opt_cam_time_offset) { + accum.template addH( + start_cam_time_offset, T_i_c_start, + -d_T_wi_d_time.transpose() * Adj.transpose() * + cph.H_pose_accum); + + if (this->common_data.opt_intrinsics) + accum.template addH( + start_cam_time_offset, calib_start, + d_T_wi_d_time.transpose() * Adj.transpose() * + cph.H_intr_pose_accum.transpose()); + + accum.template addH( + start_cam_time_offset, start_cam_time_offset, + d_T_wi_d_time.transpose() * A_H_p_A * d_T_wi_d_time); + + accum.template addB(start_cam_time_offset, + d_T_wi_d_time.transpose() * + Adj.transpose() * + cph.b_pose_accum); + } + }, + this->common_data.calibration->intrinsics[acd.cam_id].variant); + } + } + + void operator()(const tbb::blocked_range& r) { + for (const MocapPoseData& pm : r) { + typename SplineT::PosePosSO3JacobianStruct J_pose; + + int64_t time_ns = + pm.timestamp_ns + + this->common_data.mocap_calibration->mocap_time_offset_ns; + + // std::cout << "time " << time << std::endl; + // std::cout << "sline.minTime() " << spline.minTime() << std::endl; + + if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs()) + continue; + + BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(), + "time " << time_ns << " spline.minTimeNs() " + << spline->minTimeNs()); + + const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w; + const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark; + + const SE3 T_w_i = spline->pose(time_ns, &J_pose); + const SE3 T_moc_mark = T_moc_w * T_w_i * T_i_mark; + + const SE3 T_mark_moc_meas = pm.data.inverse(); + + Vector6 residual = Sophus::logd(T_mark_moc_meas * T_moc_mark); + + // TODO: check derivatives + Matrix6 d_res_d_T_i_mark; + Sophus::rightJacobianInvSE3Decoupled(residual, d_res_d_T_i_mark); + Matrix6 d_res_d_T_w_i = d_res_d_T_i_mark * T_i_mark.inverse().Adj(); + Matrix6 d_res_d_T_moc_w = + d_res_d_T_i_mark * (T_w_i * T_i_mark).inverse().Adj(); + + Vector6 d_T_wi_d_time; + spline->d_pose_d_t(time_ns, d_T_wi_d_time); + + Vector6 d_res_d_time = d_res_d_T_w_i * d_T_wi_d_time; + + size_t start_idx = J_pose.start_idx; + + size_t start_T_moc_w = this->common_data.mocap_block_offset; + size_t start_T_i_mark = this->common_data.mocap_block_offset + POSE_SIZE; + size_t start_mocap_time_offset = + this->common_data.mocap_block_offset + 2 * POSE_SIZE; + + // std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl; + + const Scalar& mocap_var_inv = this->common_data.mocap_var_inv; + + error += mocap_var_inv * residual.squaredNorm(); + + Matrix6 H_T_w_i = d_res_d_T_w_i.transpose() * d_res_d_T_w_i; + + for (size_t i = 0; i < N; i++) { + size_t start_i = (start_idx + i) * POSE_SIZE; + + // std::cout << "start_idx " << start_idx << std::endl; + // std::cout << "start_i " << start_i << std::endl; + + BASALT_ASSERT(start_i < opt_size); + + Matrix6 Ji_H_T_w_i = J_pose.d_val_d_knot[i].transpose() * H_T_w_i; + + for (size_t j = 0; j <= i; j++) { + size_t start_j = (start_idx + j) * POSE_SIZE; + + BASALT_ASSERT(start_j < opt_size); + + accum.template addH( + start_i, start_j, + mocap_var_inv * Ji_H_T_w_i * J_pose.d_val_d_knot[j]); + } + + accum.template addB( + start_i, mocap_var_inv * J_pose.d_val_d_knot[i].transpose() * + d_res_d_T_w_i.transpose() * residual); + + accum.template addH( + start_T_moc_w, start_i, mocap_var_inv * + d_res_d_T_moc_w.transpose() * + d_res_d_T_w_i * J_pose.d_val_d_knot[i]); + + accum.template addH( + start_T_i_mark, start_i, + mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_w_i * + J_pose.d_val_d_knot[i]); + + accum.template addH( + start_mocap_time_offset, start_i, + mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_w_i * + J_pose.d_val_d_knot[i]); + } + + // start_T_moc_w + accum.template addH( + start_T_moc_w, start_T_moc_w, + mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_moc_w); + + // start_T_i_mark + accum.template addH( + start_T_i_mark, start_T_moc_w, + mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_moc_w); + + accum.template addH( + start_T_i_mark, start_T_i_mark, + mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_i_mark); + + // start_mocap_time_offset + accum.template addH( + start_mocap_time_offset, start_T_moc_w, + mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_moc_w); + + accum.template addH( + start_mocap_time_offset, start_T_i_mark, + mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_i_mark); + + accum.template addH( + start_mocap_time_offset, start_mocap_time_offset, + mocap_var_inv * d_res_d_time.transpose() * d_res_d_time); + + // B + accum.template addB( + start_T_moc_w, + mocap_var_inv * d_res_d_T_moc_w.transpose() * residual); + + accum.template addB( + start_T_i_mark, + mocap_var_inv * d_res_d_T_i_mark.transpose() * residual); + + accum.template addB( + start_mocap_time_offset, + mocap_var_inv * d_res_d_time.transpose() * residual); + } + } + + void join(LinearizeSplineOpt& rhs) { + accum.join(rhs.accum); + error += rhs.error; + reprojection_error += rhs.reprojection_error; + num_points += rhs.num_points; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace basalt + +#endif diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h new file mode 100644 index 0000000..839991b --- /dev/null +++ b/include/basalt/optimization/spline_optimize.h @@ -0,0 +1,488 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include + +#include + +#include + +#include + +#include + +namespace basalt { + +template +class SplineOptimization { + public: + typedef LinearizeSplineOpt> + LinearizeT; + + typedef typename LinearizeT::SE3 SE3; + typedef typename LinearizeT::Vector2 Vector2; + typedef typename LinearizeT::Vector3 Vector3; + typedef typename LinearizeT::Vector4 Vector4; + typedef typename LinearizeT::VectorX VectorX; + typedef typename LinearizeT::Matrix24 Matrix24; + + static const int POSE_SIZE = LinearizeT::POSE_SIZE; + static const int POS_SIZE = LinearizeT::POS_SIZE; + static const int POS_OFFSET = LinearizeT::POS_OFFSET; + static const int ROT_SIZE = LinearizeT::ROT_SIZE; + static const int ROT_OFFSET = LinearizeT::ROT_OFFSET; + static const int ACCEL_BIAS_SIZE = LinearizeT::ACCEL_BIAS_SIZE; + static const int GYRO_BIAS_SIZE = LinearizeT::GYRO_BIAS_SIZE; + static const int G_SIZE = LinearizeT::G_SIZE; + + static const int ACCEL_BIAS_OFFSET = LinearizeT::ACCEL_BIAS_OFFSET; + static const int GYRO_BIAS_OFFSET = LinearizeT::GYRO_BIAS_OFFSET; + static const int G_OFFSET = LinearizeT::G_OFFSET; + + const Scalar pose_var; + + Scalar pose_var_inv; + + typedef Eigen::Matrix Matrix3; + + typedef Eigen::Matrix Vector6; + + typedef Eigen::Matrix MatrixX; + + typedef Se3Spline SplineT; + + SplineOptimization(int64_t dt_ns = 1e7) + : pose_var(1e-4), spline(dt_ns), dt_ns(dt_ns) { + pose_var_inv = 1.0 / pose_var; + + // reasonable default values + // calib.accelerometer_noise_var = 2e-2; + // calib.gyroscope_noise_var = 1e-4; + + g.setZero(); + + min_time_us = std::numeric_limits::max(); + max_time_us = std::numeric_limits::min(); + } + + int64_t getCamTimeOffsetNs() const { return calib->cam_time_offset_ns; } + int64_t getMocapTimeOffsetNs() const { + return mocap_calib->mocap_time_offset_ns; + } + + const SE3& getCamT_i_c(size_t i) const { return calib->T_i_c[i]; } + SE3& getCamT_i_c(size_t i) { return calib->T_i_c[i]; } + + VectorX getIntrinsics(size_t i) const { + return calib->intrinsics[i].getParam(); + } + + const CalibAccelBias& getAccelBias() const { + return calib->calib_accel_bias; + } + const CalibGyroBias& getGyroBias() const { + return calib->calib_gyro_bias; + } + + void resetCalib(size_t num_cams, const std::vector& cam_types) { + BASALT_ASSERT(cam_types.size() == num_cams); + + calib.reset(new Calibration); + + calib->intrinsics.resize(num_cams); + calib->T_i_c.resize(num_cams); + + mocap_calib.reset(new MocapCalibration); + } + + void resetMocapCalib() { mocap_calib.reset(new MocapCalibration); } + + void loadCalib(const std::string& p) { + std::string path = p + "calibration.json"; + + std::ifstream is(path); + + if (is.good()) { + cereal::JSONInputArchive archive(is); + calib.reset(new Calibration); + archive(*calib); + std::cout << "Loaded calibration from: " << path << std::endl; + } else { + std::cerr << "No calibration found. Run camera calibration first!!!" + << std::endl; + } + } + + void saveCalib(const std::string& path) const { + if (calib) { + std::ofstream os(path + "calibration.json"); + cereal::JSONOutputArchive archive(os); + + archive(*calib); + } + } + + void saveMocapCalib(const std::string& path, + int64_t mocap_to_imu_offset_ns = 0) const { + if (calib) { + std::ofstream os(path + "mocap_calibration.json"); + cereal::JSONOutputArchive archive(os); + + mocap_calib->mocap_to_imu_offset_ns = mocap_to_imu_offset_ns; + + archive(*mocap_calib); + } + } + + bool calibInitialized() const { return calib != nullptr; } + + bool initialized() const { return spline.numKnots() > 0; } + + void initSpline(const SE3& pose, int num_knots) { + spline.setKnots(pose, num_knots); + } + + void initSpline(const SplineT& other) { + spline.setKnots(other); + + // std::cerr << "spline.numKnots() " << spline.numKnots() << std::endl; + // std::cerr << "other.numKnots() " << other.numKnots() << std::endl; + + size_t num_knots = spline.numKnots(); + + for (size_t i = 0; i < num_knots; i++) { + Eigen::Vector3d rot_rand_inc = Eigen::Vector3d::Random() / 20; + Eigen::Vector3d trans_rand_inc = Eigen::Vector3d::Random(); + + // std::cerr << "rot_rand_inc " << rot_rand_inc.transpose() << std::endl; + // std::cerr << "trans_rand_inc " << trans_rand_inc.transpose() << + // std::endl; + + spline.getKnotSO3(i) *= Sophus::SO3d::exp(rot_rand_inc); + spline.getKnotPos(i) += trans_rand_inc; + } + } + + const SplineT& getSpline() const { return spline; } + + Vector3 getG() const { return g; } + + void setG(const Vector3& g_a) { g = g_a; } + + // const Calibration& getCalib() const { return calib; } + // void setCalib(const Calibration& c) { calib = c; } + + SE3 getT_w_moc() const { return mocap_calib->T_moc_w.inverse(); } + void setT_w_moc(const SE3& val) { mocap_calib->T_moc_w = val.inverse(); } + + SE3 getT_mark_i() const { return mocap_calib->T_i_mark.inverse(); } + void setT_mark_i(const SE3& val) { mocap_calib->T_i_mark = val.inverse(); } + + Eigen::Vector3d getTransAccelWorld(int64_t t_ns) const { + return spline.transAccelWorld(t_ns); + } + + Eigen::Vector3d getRotVelBody(int64_t t_ns) const { + return spline.rotVelBody(t_ns); + } + + SE3 getT_w_i(int64_t t_ns) { return spline.pose(t_ns); } + + void setAprilgridCorners3d(const Eigen::vector& v) { + aprilgrid_corner_pos_3d = v; + } + + void addPoseMeasurement(int64_t t_ns, const SE3& pose) { + min_time_us = std::min(min_time_us, t_ns); + max_time_us = std::max(max_time_us, t_ns); + + pose_measurements.emplace_back(); + pose_measurements.back().timestamp_ns = t_ns; + pose_measurements.back().data = pose; + } + + void addMocapMeasurement(int64_t t_ns, const SE3& pose) { + mocap_measurements.emplace_back(); + mocap_measurements.back().timestamp_ns = t_ns; + mocap_measurements.back().data = pose; + } + + void addAccelMeasurement(int64_t t_ns, const Vector3& meas) { + min_time_us = std::min(min_time_us, t_ns); + max_time_us = std::max(max_time_us, t_ns); + + accel_measurements.emplace_back(); + accel_measurements.back().timestamp_ns = t_ns; + accel_measurements.back().data = meas; + } + + void addGyroMeasurement(int64_t t_ns, const Vector3& meas) { + min_time_us = std::min(min_time_us, t_ns); + max_time_us = std::max(max_time_us, t_ns); + + gyro_measurements.emplace_back(); + gyro_measurements.back().timestamp_ns = t_ns; + gyro_measurements.back().data = meas; + } + + void addAprilgridMeasurement( + int64_t t_ns, int cam_id, + const Eigen::vector& corners_pos, + const std::vector& corner_id) { + min_time_us = std::min(min_time_us, t_ns); + max_time_us = std::max(max_time_us, t_ns); + + aprilgrid_corners_measurements.emplace_back(); + aprilgrid_corners_measurements.back().timestamp_ns = t_ns; + aprilgrid_corners_measurements.back().cam_id = cam_id; + aprilgrid_corners_measurements.back().corner_pos = corners_pos; + aprilgrid_corners_measurements.back().corner_id = corner_id; + } + + Scalar getMinTime() const { return min_time_us * 1e-9; } + Scalar getMaxTime() const { return max_time_us * 1e-9; } + + int64_t getMinTimeNs() const { return min_time_us; } + int64_t getMaxTimeNs() const { return max_time_us; } + + void init() { + int64_t time_interval_us = max_time_us - min_time_us; + + if (spline.numKnots() == 0) { + spline.setStartTimeNs(min_time_us); + spline.setKnots(pose_measurements.front().data, + time_interval_us / dt_ns + N + 1); + } + + recompute_size(); + + // std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl; + // std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl; + + ccd.calibration = calib.get(); + ccd.mocap_calibration = mocap_calib.get(); + ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d; + ccd.g = &g; + ccd.offset_intrinsics = &offset_cam_intrinsics; + ccd.offset_T_i_c = &offset_T_i_c; + ccd.bias_block_offset = bias_block_offset; + ccd.mocap_block_offset = mocap_block_offset; + + ccd.opt_g = true; + + ccd.pose_var_inv = pose_var_inv; + ccd.gyro_var_inv = 1.0 / (calib->gyro_noise_std * calib->gyro_noise_std); + ccd.accel_var_inv = 1.0 / (calib->accel_noise_std * calib->accel_noise_std); + ccd.mocap_var_inv = pose_var_inv; + } + + void recompute_size() { + offset_cam_intrinsics.clear(); + + size_t num_knots = spline.numKnots(); + + bias_block_offset = POSE_SIZE * num_knots; + + size_t T_i_c_block_offset = + bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE; + + offset_T_i_c.emplace_back(T_i_c_block_offset); + for (size_t i = 0; i < calib->T_i_c.size(); i++) + offset_T_i_c.emplace_back(offset_T_i_c.back() + POSE_SIZE); + + offset_cam_intrinsics.emplace_back(offset_T_i_c.back()); + for (size_t i = 0; i < calib->intrinsics.size(); i++) + offset_cam_intrinsics.emplace_back(offset_cam_intrinsics.back() + + calib->intrinsics[i].getN()); + + mocap_block_offset = offset_cam_intrinsics.back(); + + opt_size = mocap_block_offset + 2 * POSE_SIZE + 2; + + // std::cerr << "bias_block_offset " << bias_block_offset << std::endl; + // std::cerr << "mocap_block_offset " << mocap_block_offset << std::endl; + // std::cerr << "opt_size " << opt_size << std::endl; + // std::cerr << "offset_T_i_c.back() " << offset_T_i_c.back() << + // std::endl; std::cerr << "offset_cam_intrinsics.back() " << + // offset_cam_intrinsics.back() + // << std::endl; + } + + void optimize(bool use_intr, bool use_poses, bool use_april_corners, + bool opt_cam_time_offset, bool opt_imu_scale, bool use_mocap, + double huber_thresh, double& error, int& num_points, + double& reprojection_error) { + // std::cerr << "optimize num_knots " << num_knots << std::endl; + + for (int i = 0; i < 1; i++) { + ccd.opt_intrinsics = use_intr; + ccd.opt_cam_time_offset = opt_cam_time_offset; + ccd.opt_imu_scale = opt_imu_scale; + ccd.huber_thresh = huber_thresh; + + LinearizeT lopt(opt_size, &spline, ccd); + + // auto t1 = std::chrono::high_resolution_clock::now(); + + if (use_poses) { + tbb::blocked_range pose_range(pose_measurements.begin(), + pose_measurements.end()); + + tbb::parallel_reduce(pose_range, lopt); + // lopt(pose_range); + } + + if (use_april_corners) { + tbb::blocked_range april_range( + aprilgrid_corners_measurements.begin(), + aprilgrid_corners_measurements.end()); + tbb::parallel_reduce(april_range, lopt); + // lopt(april_range); + } + + if (use_mocap) { + tbb::blocked_range mocap_pose_range( + mocap_measurements.begin(), mocap_measurements.end()); + tbb::parallel_reduce(mocap_pose_range, lopt); + // lopt(mocap_pose_range); + } + + tbb::blocked_range accel_range(accel_measurements.begin(), + accel_measurements.end()); + tbb::parallel_reduce(accel_range, lopt); + + tbb::blocked_range gyro_range(gyro_measurements.begin(), + gyro_measurements.end()); + + tbb::parallel_reduce(gyro_range, lopt); + + VectorX inc_full = -lopt.accum.solve(); + + applyInc(inc_full, offset_cam_intrinsics); + + error = lopt.error; + num_points = lopt.num_points; + reprojection_error = lopt.reprojection_error; + } + } + + typename Calibration::Ptr calib; + typename MocapCalibration::Ptr mocap_calib; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + typedef typename Eigen::deque::const_iterator PoseDataIter; + typedef typename Eigen::deque::const_iterator GyroDataIter; + typedef typename Eigen::deque::const_iterator AccelDataIter; + typedef typename Eigen::deque::const_iterator + AprilgridCornersDataIter; + typedef + typename Eigen::deque::const_iterator MocapPoseDataIter; + + void applyInc(VectorX& inc_full, + const std::vector& offset_cam_intrinsics) { + size_t num_knots = spline.numKnots(); + + for (size_t i = 0; i < num_knots; i++) { + Vector6 inc = inc_full.template segment(POSE_SIZE * i); + + // std::cerr << "i: " << i << " inc: " << inc.transpose() << std::endl; + + spline.applyInc(i, inc); + } + + size_t bias_block_offset = POSE_SIZE * num_knots; + calib->calib_accel_bias += inc_full.template segment( + bias_block_offset + ACCEL_BIAS_OFFSET); + + calib->calib_gyro_bias += inc_full.template segment( + bias_block_offset + GYRO_BIAS_OFFSET); + g += inc_full.template segment(bias_block_offset + G_OFFSET); + + size_t T_i_c_block_offset = + bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE; + for (size_t i = 0; i < calib->T_i_c.size(); i++) { + calib->T_i_c[i] *= Sophus::expd(inc_full.template segment( + T_i_c_block_offset + i * POSE_SIZE)); + } + + for (size_t i = 0; i < calib->intrinsics.size(); i++) { + calib->intrinsics[i].applyInc(inc_full.segment( + offset_cam_intrinsics[i], calib->intrinsics[i].getN())); + } + + size_t mocap_block_offset = offset_cam_intrinsics.back(); + + mocap_calib->T_moc_w *= + Sophus::expd(inc_full.template segment(mocap_block_offset)); + mocap_calib->T_i_mark *= Sophus::expd( + inc_full.template segment(mocap_block_offset + POSE_SIZE)); + + mocap_calib->mocap_time_offset_ns += + 1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE]; + + calib->cam_time_offset_ns += + 1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1]; + } + + int64_t min_time_us, max_time_us; + + Eigen::deque pose_measurements; + Eigen::deque gyro_measurements; + Eigen::deque accel_measurements; + Eigen::deque aprilgrid_corners_measurements; + Eigen::deque mocap_measurements; + + typename LinearizeT::CalibCommonData ccd; + + std::vector offset_cam_intrinsics; + std::vector offset_T_i_c; + size_t mocap_block_offset; + size_t bias_block_offset; + size_t opt_size; + + SplineT spline; + Vector3 g; + + Eigen::vector aprilgrid_corner_pos_3d; + + int64_t dt_ns; +}; // namespace basalt + +} // namespace basalt diff --git a/include/basalt/utils/common_types.h b/include/basalt/utils/common_types.h new file mode 100644 index 0000000..c9a543e --- /dev/null +++ b/include/basalt/utils/common_types.h @@ -0,0 +1,254 @@ +/** +BSD 3-Clause License + +Copyright (c) 2018, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +namespace basalt { + +/// identifies a frame of multiple images (stereo pair) +using FrameId = int64_t; + +/// identifies the camera (left or right) +using CamId = std::size_t; + +/// pair of image timestamp and camera id identifies an image (imageId) +typedef std::pair TimeCamId; +inline std::ostream& operator<<(std::ostream& os, const TimeCamId& tcid) { + os << tcid.first << "_" << tcid.second; + return os; +} + +/// ids for 2D features detected in images +using FeatureId = int; + +/// keypoint positions and descriptors for an image +struct KeypointsData { + /// collection of 2d corner points (indexed by FeatureId) + std::vector> + corners; + /// collection of feature orientation (in radian) with same index as `corners` + /// (indexed by FeatureId) + std::vector corner_angles; + /// collection of feature descriptors with same index as `corners` (indexed by + /// FeatureId) + std::vector> corner_descriptors; + + Eigen::vector corners_3d; +}; + +/// feature corners is a collection of { imageId => KeypointsData } +using Corners = tbb::concurrent_unordered_map; + +/// feature matches for an image pair +struct MatchData { + /// estimated transformation (based on inliers or calibration) from the second + /// image's coordinate system to the first image's corrdinate system + Sophus::SE3d T_i_j; + /// collection of {featureId_i, featureId_j} pairs of all matches + std::vector> matches; + /// collection of {featureId_i, featureId_j} pairs of inlier matches + std::vector> inliers; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// feature matches is a collection of { (imageId, imageId) => MatchData } +using Matches = tbb::concurrent_unordered_map< + std::pair, MatchData, + tbb::tbb_hash>, + std::equal_to>, + Eigen::aligned_allocator< + std::pair, MatchData>>>; + +/// pair of image and feature indices +using ImageFeaturePair = std::pair; + +/// Feature tracks are collections of {ImageId => FeatureId}. +/// I.e. a collection of all images that observed this feature and the +/// corresponding feature index in that image. +using FeatureTrack = std::map; + +/// Ids for feature tracks; also used for landmarks created from (some of) the +/// tracks; +using TrackId = int64_t; + +/// FeatureTracks is a collection {TrackId => FeatureTrack} +using FeatureTracks = std::unordered_map; + +/// cameras in the map +struct Camera { + // camera pose (transforms from camera to world) + Sophus::SE3d T_w_c; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// landmarks in the map +struct Landmark { + /// 3d position in world coordinates + Eigen::Vector3d p; + + /// Inlier observations in the current map. + /// This is a subset of the original feature track. + FeatureTrack obs; + + /// Outlier observations in the current map. + /// This is a subset of the original feature track. + FeatureTrack outlier_obs; +}; + +/// collection {imageId => Camera} for all cameras in the map +using Cameras = + std::map, + Eigen::aligned_allocator>>; + +/// collection {trackId => Landmark} for all landmarks in the map. +/// trackIds correspond to feature_tracks +using Landmarks = std::unordered_map; + +/// camera candidate to be added to map +struct CameraCandidate { + TimeCamId tcid; + std::vector shared_tracks; + + // keep track of different stages of adding a set of candidate cameras and its + // landmarks to the map + bool tried = false; //!< tried to add to map + bool camera_added = false; //!< succeeded to add to map + bool landmarks_added = false; //!< added new landmarks to map +}; + +/// list of current candidates and some book keeping for the different stages +struct CameraCandidates { + enum Stage { + ComputeCandidates, + AddCameras, + AddLandmarks, + Optimize, + RemoveOutliers, + Done + }; + + std::vector cameras; + Stage current_stage = ComputeCandidates; + int min_localization_inliers = 0; + int max_cameras_to_add = 0; + + int num_cameras_added() { + int num_added = 0; + for (const auto& c : cameras) { + if (c.camera_added) { + ++num_added; + } + } + return num_added; + } + + int num_landmarks_added() { + int num_added = 0; + for (const auto& c : cameras) { + if (c.landmarks_added) { + ++num_added; + } + } + return num_added; + } +}; + +/// Flags for different landmark outlier criteria +enum OutlierFlags { + OutlierNone = 0, + // reprojection error much too large + OutlierReprojectionErrorHuge = 1 << 0, + // reprojection error too large + OutlierReprojectionErrorNormal = 1 << 1, + // distance to a camera too small + OutlierCameraDistance = 1 << 2, + // z-coord in some camera frame too small + OutlierZCoordinate = 1 << 3 +}; + +/// info on a single projected landmark +struct ProjectedLandmark { + Eigen::Vector2d point_measured; //!< detected feature location + Eigen::Vector2d point_reprojected; //!< landmark projected into image + Eigen::Vector3d point_3d_c; //!< 3d point in camera coordinates + TrackId track_id = -1; //!< corresponding track_id + double reprojection_error = 0; //!< current reprojection error + unsigned int outlier_flags = OutlierNone; //!< flags for outlier + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using ProjectedLandmarkPtr = std::shared_ptr; +using ProjectedLandmarkConstPtr = std::shared_ptr; + +/// all landmark projections for inlier and outlier observations for a single +/// image +struct ImageProjection { + std::vector obs; + std::vector outlier_obs; +}; + +/// projections for all images +using ImageProjections = std::map; + +/// inlier projections indexed per track +using TrackProjections = + std::unordered_map>; +} + +namespace cereal { + +template +void serialize(Archive& ar, basalt::KeypointsData& c) { + ar(c.corners, c.corner_angles, c.corner_descriptors); +} + +template +void serialize(Archive& ar, basalt::MatchData& c) { + ar(c.T_i_j, c.matches, c.inliers); +} +} diff --git a/include/basalt/utils/image.h b/include/basalt/utils/image.h new file mode 100644 index 0000000..11c6a1f --- /dev/null +++ b/include/basalt/utils/image.h @@ -0,0 +1,711 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +// This file is adapted from Pangolin. Original license: + +/* This file is part of the Pangolin Project. + * http://github.com/stevenlovegrove/Pangolin + * + * Copyright (c) 2011 Steven Lovegrove + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, + * copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following + * conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT + * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, + * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR + * OTHER DEALINGS IN THE SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include + +namespace basalt { + +inline void PitchedCopy(char* dst, unsigned int dst_pitch_bytes, + const char* src, unsigned int src_pitch_bytes, + unsigned int width_bytes, unsigned int height) { + if (dst_pitch_bytes == width_bytes && src_pitch_bytes == width_bytes) { + std::memcpy(dst, src, height * width_bytes); + } else { + for (unsigned int row = 0; row < height; ++row) { + std::memcpy(dst, src, width_bytes); + dst += dst_pitch_bytes; + src += src_pitch_bytes; + } + } +} + +template +struct Image { + inline Image() : pitch(0), ptr(0), w(0), h(0) {} + + inline Image(T* ptr, size_t w, size_t h, size_t pitch) + : pitch(pitch), ptr(ptr), w(w), h(h) {} + + PANGO_HOST_DEVICE inline size_t SizeBytes() const { return pitch * h; } + + PANGO_HOST_DEVICE inline size_t Area() const { return w * h; } + + PANGO_HOST_DEVICE inline bool IsValid() const { return ptr != 0; } + + PANGO_HOST_DEVICE inline bool IsContiguous() const { + return w * sizeof(T) == pitch; + } + + pangolin::Image toPangoImage() { + pangolin::Image img(ptr, w, h, pitch); + return img; + } + + ////////////////////////////////////////////////////// + // Iterators + ////////////////////////////////////////////////////// + + PANGO_HOST_DEVICE inline T* begin() { return ptr; } + + PANGO_HOST_DEVICE inline T* end() { return RowPtr(h - 1) + w; } + + PANGO_HOST_DEVICE inline const T* begin() const { return ptr; } + + PANGO_HOST_DEVICE inline const T* end() const { return RowPtr(h - 1) + w; } + + PANGO_HOST_DEVICE inline size_t size() const { return w * h; } + + ////////////////////////////////////////////////////// + // Image transforms + ////////////////////////////////////////////////////// + + template + PANGO_HOST_DEVICE inline void Transform(UnaryOperation unary_op) { + PANGO_ASSERT(IsValid()); + + for (size_t y = 0; y < h; ++y) { + T* el = RowPtr(y); + const T* el_end = el + w; + for (; el != el_end; ++el) { + *el = unary_op(*el); + } + } + } + + PANGO_HOST_DEVICE inline void Fill(const T& val) { + Transform([&](const T&) { return val; }); + } + + PANGO_HOST_DEVICE inline void Replace(const T& oldval, const T& newval) { + Transform([&](const T& val) { return (val == oldval) ? newval : val; }); + } + + inline void Memset(unsigned char v = 0) { + PANGO_ASSERT(IsValid()); + if (IsContiguous()) { + ::pangolin::Memset((char*)ptr, v, pitch * h); + } else { + for (size_t y = 0; y < h; ++y) { + ::pangolin::Memset((char*)RowPtr(y), v, pitch); + } + } + } + + inline void CopyFrom(const Image& img) { + if (IsValid() && img.IsValid()) { + PANGO_ASSERT(w >= img.w && h >= img.h); + PitchedCopy((char*)ptr, pitch, (char*)img.ptr, img.pitch, + std::min(img.w, w) * sizeof(T), std::min(img.h, h)); + } else if (img.IsValid() != IsValid()) { + PANGO_ASSERT(false && "Cannot copy from / to an unasigned image."); + } + } + + ////////////////////////////////////////////////////// + // Reductions + ////////////////////////////////////////////////////// + + template + PANGO_HOST_DEVICE inline T Accumulate(const T init, + BinaryOperation binary_op) { + PANGO_ASSERT(IsValid()); + + T val = init; + for (size_t y = 0; y < h; ++y) { + T* el = RowPtr(y); + const T* el_end = el + w; + for (; el != el_end; ++el) { + val = binary_op(val, *el); + } + } + return val; + } + + std::pair MinMax() const { + PANGO_ASSERT(IsValid()); + + std::pair minmax(std::numeric_limits::max(), + std::numeric_limits::lowest()); + for (size_t r = 0; r < h; ++r) { + const T* ptr = RowPtr(r); + const T* end = ptr + w; + while (ptr != end) { + minmax.first = std::min(*ptr, minmax.first); + minmax.second = std::max(*ptr, minmax.second); + ++ptr; + } + } + return minmax; + } + + template + Tout Sum() const { + return Accumulate((T)0, + [](const T& lhs, const T& rhs) { return lhs + rhs; }); + } + + template + Tout Mean() const { + return Sum() / Area(); + } + + ////////////////////////////////////////////////////// + // Direct Pixel Access + ////////////////////////////////////////////////////// + + PANGO_HOST_DEVICE inline T* RowPtr(size_t y) { + return (T*)((unsigned char*)(ptr) + y * pitch); + } + + PANGO_HOST_DEVICE inline const T* RowPtr(size_t y) const { + return (T*)((unsigned char*)(ptr) + y * pitch); + } + + PANGO_HOST_DEVICE inline T& operator()(size_t x, size_t y) { + PANGO_BOUNDS_ASSERT(InBounds(x, y)); + return RowPtr(y)[x]; + } + + PANGO_HOST_DEVICE inline const T& operator()(size_t x, size_t y) const { + PANGO_BOUNDS_ASSERT(InBounds(x, y)); + return RowPtr(y)[x]; + } + + template + PANGO_HOST_DEVICE inline T& operator()(const TVec& p) { + PANGO_BOUNDS_ASSERT(InBounds(p[0], p[1])); + return RowPtr(p[1])[p[0]]; + } + + template + PANGO_HOST_DEVICE inline const T& operator()(const TVec& p) const { + PANGO_BOUNDS_ASSERT(InBounds(p[0], p[1])); + return RowPtr(p[1])[p[0]]; + } + + PANGO_HOST_DEVICE inline T& operator[](size_t ix) { + PANGO_BOUNDS_ASSERT(InImage(ptr + ix)); + return ptr[ix]; + } + + PANGO_HOST_DEVICE inline const T& operator[](size_t ix) const { + PANGO_BOUNDS_ASSERT(InImage(ptr + ix)); + return ptr[ix]; + } + + ////////////////////////////////////////////////////// + // Interpolated Pixel Access + ////////////////////////////////////////////////////// + + template + inline S interp(const Eigen::Matrix& p) const { + return interp(p[0], p[1]); + } + + template + inline Eigen::Matrix interpGrad( + const Eigen::Matrix& p) const { + return interpGrad(p[0], p[1]); + } + + template + inline float interp(S x, S y) const { + int ix = x; + int iy = y; + + S dx = x - ix; + S dy = y - iy; + + S ddx = 1.0f - dx; + S ddy = 1.0f - dy; + + return ddx * ddy * (*this)(ix, iy) + ddx * dy * (*this)(ix, iy + 1) + + dx * ddy * (*this)(ix + 1, iy) + dx * dy * (*this)(ix + 1, iy + 1); + } + + template + inline Eigen::Matrix interpGrad(S x, S y) const { + int ix = x; + int iy = y; + + S dx = x - ix; + S dy = y - iy; + + S ddx = 1.0f - dx; + S ddy = 1.0f - dy; + + Eigen::Matrix res; + + const T& px0y0 = (*this)(ix, iy); + const T& px1y0 = (*this)(ix + 1, iy); + const T& px0y1 = (*this)(ix, iy + 1); + const T& px1y1 = (*this)(ix + 1, iy + 1); + + res[0] = ddx * ddy * px0y0 + ddx * dy * px0y1 + dx * ddy * px1y0 + + dx * dy * px1y1; + + const T& pxm1y0 = (*this)(ix - 1, iy); + const T& pxm1y1 = (*this)(ix - 1, iy + 1); + + S res_mx = ddx * ddy * pxm1y0 + ddx * dy * pxm1y1 + dx * ddy * px0y0 + + dx * dy * px0y1; + + const T& px2y0 = (*this)(ix + 2, iy); + const T& px2y1 = (*this)(ix + 2, iy + 1); + + S res_px = ddx * ddy * px1y0 + ddx * dy * px1y1 + dx * ddy * px2y0 + + dx * dy * px2y1; + + res[1] = 0.5 * (res_px - res_mx); + + const T& px0ym1 = (*this)(ix, iy - 1); + const T& px1ym1 = (*this)(ix + 1, iy - 1); + + S res_my = ddx * ddy * px0ym1 + ddx * dy * px0y0 + dx * ddy * px1ym1 + + dx * dy * px1y0; + + const T& px0y2 = (*this)(ix, iy + 2); + const T& px1y2 = (*this)(ix + 1, iy + 2); + + S res_py = ddx * ddy * px0y1 + ddx * dy * px0y2 + dx * ddy * px1y1 + + dx * dy * px1y2; + + res[2] = 0.5 * (res_py - res_my); + + return res; + } + + ////////////////////////////////////////////////////// + // Bounds Checking + ////////////////////////////////////////////////////// + + PANGO_HOST_DEVICE + bool InImage(const T* ptest) const { + return ptr <= ptest && ptest < RowPtr(h); + } + + PANGO_HOST_DEVICE inline bool InBounds(int x, int y) const { + return 0 <= x && x < (int)w && 0 <= y && y < (int)h; + } + + PANGO_HOST_DEVICE inline bool InBounds(float x, float y, float border) const { + return border <= x && x < (w - border) && border <= y && y < (h - border); + } + + template + PANGO_HOST_DEVICE inline bool InBounds( + const TVec& p, const TBorder border = (TBorder)0) const { + return border <= p[0] && p[0] < ((int)w - border) && border <= p[1] && + p[1] < ((int)h - border); + } + + ////////////////////////////////////////////////////// + // Obtain slices / subimages + ////////////////////////////////////////////////////// + + PANGO_HOST_DEVICE inline const Image SubImage(size_t x, size_t y, + size_t width, + size_t height) const { + PANGO_ASSERT((x + width) <= w && (y + height) <= h); + return Image(RowPtr(y) + x, width, height, pitch); + } + + PANGO_HOST_DEVICE inline Image SubImage(size_t x, size_t y, size_t width, + size_t height) { + PANGO_ASSERT((x + width) <= w && (y + height) <= h); + return Image(RowPtr(y) + x, width, height, pitch); + } + + PANGO_HOST_DEVICE inline Image Row(int y) const { + return SubImage(0, y, w, 1); + } + + PANGO_HOST_DEVICE inline Image Col(int x) const { + return SubImage(x, 0, 1, h); + } + + ////////////////////////////////////////////////////// + // Data mangling + ////////////////////////////////////////////////////// + + template + PANGO_HOST_DEVICE inline Image Reinterpret() const { + PANGO_ASSERT(sizeof(TRecast) == sizeof(T), + "sizeof(TRecast) must match sizeof(T): % != %", + sizeof(TRecast), sizeof(T)); + return UnsafeReinterpret(); + } + + template + PANGO_HOST_DEVICE inline Image UnsafeReinterpret() const { + return Image((TRecast*)ptr, w, h, pitch); + } + + ////////////////////////////////////////////////////// + // Deprecated methods + ////////////////////////////////////////////////////// + + // PANGOLIN_DEPRECATED inline + Image(size_t w, size_t h, size_t pitch, T* ptr) + : pitch(pitch), ptr(ptr), w(w), h(h) {} + + // Use RAII/move aware pangolin::ManagedImage instead + // PANGOLIN_DEPRECATED inline + void Dealloc() { + if (ptr) { + ::operator delete(ptr); + ptr = nullptr; + } + } + + // Use RAII/move aware pangolin::ManagedImage instead + // PANGOLIN_DEPRECATED inline + void Alloc(size_t w, size_t h, size_t pitch) { + Dealloc(); + this->w = w; + this->h = h; + this->pitch = pitch; + this->ptr = (T*)::operator new(h* pitch); + } + + ////////////////////////////////////////////////////// + // Data members + ////////////////////////////////////////////////////// + + size_t pitch; + T* ptr; + size_t w; + size_t h; + + PANGO_EXTENSION_IMAGE +}; + +template +using DefaultImageAllocator = std::allocator; + +// Image that manages it's own memory, storing a strong pointer to it's memory +template > +class ManagedImage : public Image { + public: + typedef std::shared_ptr> Ptr; + + // Destructor + inline ~ManagedImage() { Deallocate(); } + + // Null image + inline ManagedImage() {} + + // Row image + inline ManagedImage(size_t w) + : Image(Allocator().allocate(w), w, 1, w * sizeof(T)) {} + + inline ManagedImage(size_t w, size_t h) + : Image(Allocator().allocate(w * h), w, h, w * sizeof(T)) {} + + inline ManagedImage(size_t w, size_t h, size_t pitch_bytes) + : Image(Allocator().allocate((h * pitch_bytes) / sizeof(T) + 1), w, h, + pitch_bytes) {} + + // Not copy constructable + inline ManagedImage(const ManagedImage& other) = delete; + + // Move constructor + inline ManagedImage(ManagedImage&& img) { + *this = std::move(img); + } + + // Move asignment + inline void operator=(ManagedImage&& img) { + Deallocate(); + Image::pitch = img.pitch; + Image::ptr = img.ptr; + Image::w = img.w; + Image::h = img.h; + img.ptr = nullptr; + } + + // Move constructor + inline ManagedImage(pangolin::ManagedImage&& img) { + *this = std::move(img); + } + + // Move asignment + inline void operator=(pangolin::ManagedImage&& img) { + Deallocate(); + Image::pitch = img.pitch; + Image::ptr = img.ptr; + Image::w = img.w; + Image::h = img.h; + img.ptr = nullptr; + } + + // Explicit copy constructor + template + ManagedImage(const pangolin::CopyObject& other) { + CopyFrom(other.obj); + } + + // Explicit copy assignment + template + void operator=(const pangolin::CopyObject& other) { + CopyFrom(other.obj); + } + + inline void Swap(ManagedImage& img) { + std::swap(img.pitch, Image::pitch); + std::swap(img.ptr, Image::ptr); + std::swap(img.w, Image::w); + std::swap(img.h, Image::h); + } + + inline void CopyFrom(const Image& img) { + if (!Image::IsValid() || Image::w != img.w || Image::h != img.h) { + Reinitialise(img.w, img.h); + } + Image::CopyFrom(img); + } + + inline void Reinitialise(size_t w, size_t h) { + if (!Image::ptr || Image::w != w || Image::h != h) { + *this = ManagedImage(w, h); + } + } + + inline void Reinitialise(size_t w, size_t h, size_t pitch) { + if (!Image::ptr || Image::w != w || Image::h != h || + Image::pitch != pitch) { + *this = ManagedImage(w, h, pitch); + } + } + + inline void Deallocate() { + if (Image::ptr) { + Allocator().deallocate(Image::ptr, + (Image::h * Image::pitch) / sizeof(T)); + Image::ptr = nullptr; + } + } + + // Move asignment + template + inline void OwnAndReinterpret(ManagedImage&& img) { + Deallocate(); + Image::pitch = img.pitch; + Image::ptr = (T*)img.ptr; + Image::w = img.w; + Image::h = img.h; + img.ptr = nullptr; + } + + template + inline void ConvertFrom(const ManagedImage& img) { + Reinitialise(img.w, img.h); + + for (size_t j = 0; j < img.h; j++) { + T* this_row = this->RowPtr(j); + const T1* other_row = img.RowPtr(j); + for (size_t i = 0; i < img.w; i++) { + this_row[i] = T(other_row[i]); + } + } + } + + inline void operator-=(const ManagedImage& img) { + for (size_t j = 0; j < img.h; j++) { + T* this_row = this->RowPtr(j); + const T* other_row = img.RowPtr(j); + for (size_t i = 0; i < img.w; i++) { + this_row[i] -= other_row[i]; + } + } + } +}; + +template > +class ManagedImagePyr { + public: + inline ManagedImagePyr() {} + + inline ManagedImagePyr(ManagedImage& other, size_t num_levels) { + setFromImage(other, num_levels); + } + + inline void setFromImage(const ManagedImage& other, size_t num_levels) { + orig_w = other.w; + image.Reinitialise(other.w + other.w / 2, other.h); + image.Fill(0); + lvl_internal(0).CopyFrom(other); + + for (size_t i = 0; i < num_levels; i++) { + const Image l = lvl(i); + Image lp1 = lvl_internal(i + 1); + subsample(l, lp1); + } + } + + static inline int border101(int x, int h) { + return h - 1 - std::abs(h - 1 - x); + } + + static void subsample(const Image& img, Image& img_sub) { + static_assert(std::is_same::value || + std::is_same::value); + + constexpr int kernel[5] = {1, 4, 6, 4, 1}; + + // accumulator + ManagedImage tmp(img_sub.h, img.w); + + // Vertical convolution + { + for (int r = 0; r < int(img_sub.h); r++) { + const T* row_m2 = img.RowPtr(std::abs(2 * r - 2)); + const T* row_m1 = img.RowPtr(std::abs(2 * r - 1)); + const T* row = img.RowPtr(2 * r); + const T* row_p1 = img.RowPtr(border101(2 * r + 1, img.h)); + const T* row_p2 = img.RowPtr(border101(2 * r + 2, img.h)); + + for (int c = 0; c < int(img.w); c++) { + tmp(r, c) = kernel[0] * int(row_m2[c]) + kernel[1] * int(row_m1[c]) + + kernel[2] * int(row[c]) + kernel[3] * int(row_p1[c]) + + kernel[4] * int(row_p2[c]); + } + } + } + + // Horizontal convolution + { + for (int c = 0; c < int(img_sub.w); c++) { + const int* row_m2 = tmp.RowPtr(std::abs(2 * c - 2)); + const int* row_m1 = tmp.RowPtr(std::abs(2 * c - 1)); + const int* row = tmp.RowPtr(2 * c); + const int* row_p1 = tmp.RowPtr(border101(2 * c + 1, tmp.h)); + const int* row_p2 = tmp.RowPtr(border101(2 * c + 2, tmp.h)); + + for (int r = 0; r < int(tmp.w); r++) { + int val_int = kernel[0] * row_m2[r] + kernel[1] * row_m1[r] + + kernel[2] * row[r] + kernel[3] * row_p1[r] + + kernel[4] * row_p2[r]; + T val = ((val_int + (1 << 7)) >> 8); + img_sub(c, r) = val; + } + } + } + } + + inline const Image lvl(size_t lvl) const { + size_t x = (lvl == 0) ? 0 : orig_w; + size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1))); + size_t width = (orig_w >> lvl); + size_t height = (image.h >> lvl); + + return image.SubImage(x, y, width, height); + } + + template + inline Eigen::Matrix lvl_offset(size_t lvl) { + size_t x = (lvl == 0) ? 0 : orig_w; + size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1))); + + return Eigen::Matrix(x, y); + } + + inline pangolin::Image toPangoImage() { return image.toPangoImage(); } + + private: + inline Image lvl_internal(size_t lvl) { + size_t x = (lvl == 0) ? 0 : orig_w; + size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1))); + size_t width = (orig_w >> lvl); + size_t height = (image.h >> lvl); + + return image.SubImage(x, y, width, height); + } + + size_t orig_w; + ManagedImage image; +}; + +inline void rgb_to_gray(const pangolin::TypedImage& rgb, + basalt::ManagedImage& gray) { + gray.Reinitialise(rgb.w, rgb.h); + + for (size_t x = 0; x < rgb.w; x++) { + for (size_t y = 0; y < rgb.h; y++) { + double val = 0.2989 * (double)rgb(3 * x + 0, y) + + 0.5870 * (double)rgb(3 * x + 1, y) + + 0.1140 * (double)rgb(3 * x + 2, y); + + gray(x, y) = val; + } + } +} + +} // namespace basalt diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h new file mode 100644 index 0000000..c96b8f0 --- /dev/null +++ b/include/basalt/utils/imu_types.h @@ -0,0 +1,270 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace basalt { + +namespace constants { +static const Eigen::Vector3d g(0, 0, -9.81); +static const Eigen::Vector3d g_dir(0, 0, -1); +} // namespace constants + +struct PoseVelBiasStateWithLin : private PoseVelBiasState { + PoseVelBiasStateWithLin() { + linearized = false; + delta.setZero(); + }; + + PoseVelBiasStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i, + const Eigen::Vector3d& vel_w_i, + const Eigen::Vector3d& bias_gyro, + const Eigen::Vector3d& bias_accel, bool linearized) + : PoseVelBiasState(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel), + linearized(linearized) { + delta.setZero(); + state_current = *this; + } + + PoseVelBiasStateWithLin(const PoseVelBiasState& other) + : PoseVelBiasState(other), linearized(false) { + delta.setZero(); + state_current = other; + } + + void setLinFalse() { + linearized = false; + delta.setZero(); + } + + void setLinTrue() { linearized = true; } + + void applyInc(const VecN& inc) { + if (!linearized) { + PoseVelBiasState::applyInc(inc); + } else { + delta += inc; + state_current = *this; + state_current.applyInc(delta); + } + } + + inline const PoseVelBiasState& getState() const { + if (!linearized) { + return *this; + } else { + return state_current; + } + } + + inline const PoseVelBiasState& getStateLin() const { return *this; } + + inline bool isLinearized() const { return linearized; } + inline const VecN& getDelta() const { return delta; } + inline int64_t getT_ns() const { return t_ns; } + + friend struct PoseStateWithLin; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + bool linearized; + VecN delta; + + PoseVelBiasState state_current; + + friend class cereal::access; + + template + void serialize(Archive& ar) { + ar(T_w_i); + ar(vel_w_i); + ar(bias_gyro); + ar(bias_accel); + ar(state_current.T_w_i); + ar(state_current.vel_w_i); + ar(state_current.bias_gyro); + ar(state_current.bias_accel); + ar(delta); + ar(linearized); + ar(t_ns); + } +}; + +struct PoseStateWithLin : private PoseState { + PoseStateWithLin() { + linearized = false; + delta.setZero(); + }; + + PoseStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i) + : PoseState(t_ns, T_w_i), linearized(false) { + delta.setZero(); + T_w_i_current = T_w_i; + } + + PoseStateWithLin(const PoseVelBiasStateWithLin& other) + : PoseState(other.t_ns, other.T_w_i), + linearized(other.linearized), + delta(other.delta.head<6>()) { + T_w_i_current = T_w_i; + incPose(delta, T_w_i_current); + } + + inline const Sophus::SE3d& getPose() const { + if (!linearized) { + return T_w_i; + } else { + return T_w_i_current; + } + } + + inline const Sophus::SE3d& getPoseLin() const { return T_w_i; } + + inline void applyInc(const VecN& inc) { + if (!linearized) { + incPose(inc, T_w_i); + } else { + delta += inc; + T_w_i_current = T_w_i; + incPose(delta, T_w_i_current); + } + } + + inline bool isLinearized() const { return linearized; } + inline const VecN& getDelta() const { return delta; } + inline int64_t getT_ns() const { return t_ns; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + bool linearized; + VecN delta; + + Sophus::SE3d T_w_i_current; + + friend class cereal::access; + + template + void serialize(Archive& ar) { + ar(T_w_i); + ar(T_w_i_current); + ar(delta); + ar(linearized); + ar(t_ns); + } +}; + +struct AbsOrderMap { + std::map> abs_order_map; + size_t items = 0; + size_t total_size = 0; + + void print_order() { + for (const auto& kv : abs_order_map) { + std::cout << kv.first << " (" << kv.second.first << "," + << kv.second.second << ")" << std::endl; + } + std::cout << std::endl; + } +}; + +struct MargData { + typedef std::shared_ptr Ptr; + + AbsOrderMap aom; + Eigen::MatrixXd abs_H; + Eigen::VectorXd abs_b; + Eigen::map frame_states; + Eigen::map frame_poses; + std::set kfs_all; + std::set kfs_to_marg; + + std::vector opt_flow_res; +}; + +struct RelPoseFactor { + int64_t t_i_ns, t_j_ns; + + Sophus::SE3d T_i_j; + Sophus::Matrix6d cov_inv; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct RollPitchFactor { + int64_t t_ns; + + Sophus::SO3d R_w_i_meas; + + Eigen::Matrix2d cov_inv; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace basalt + +namespace cereal { + +template +void serialize(Archive& ar, basalt::AbsOrderMap& a) { + ar(a.total_size); + ar(a.items); + ar(a.abs_order_map); +} + +template +void serialize(Archive& ar, basalt::MargData& m) { + ar(m.aom); + ar(m.abs_H); + ar(m.abs_b); + ar(m.frame_poses); + ar(m.frame_states); + ar(m.kfs_all); + ar(m.kfs_to_marg); +} + +} // namespace cereal diff --git a/include/basalt/utils/keypoints.h b/include/basalt/utils/keypoints.h new file mode 100644 index 0000000..c556477 --- /dev/null +++ b/include/basalt/utils/keypoints.h @@ -0,0 +1,111 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include + +namespace basalt { + +typedef std::bitset<256> Descriptor; + +void detectKeypointsMapping(const basalt::Image& img_raw, + KeypointsData& kd, int num_features); + +void detectKeypoints(const basalt::Image& img_raw, + KeypointsData& kd, int PATCH_SIZE = 32, + int num_points_cell = 1, + const Eigen::vector& current_points = + Eigen::vector()); + +void computeAngles(const basalt::Image& img_raw, + KeypointsData& kd, bool rotate_features); + +void computeDescriptors(const basalt::Image& img_raw, + KeypointsData& kd); + +void matchFastHelper(const std::vector>& corner_descriptors_1, + const std::vector>& corner_descriptors_2, + std::map& matches, int threshold, + double test_dist); + +void matchDescriptors(const std::vector>& corner_descriptors_1, + const std::vector>& corner_descriptors_2, + std::vector>& matches, int threshold, + double dist_2_best); + +inline void computeEssential(const Sophus::SE3d& T_0_1, Eigen::Matrix4d& E) { + E.setZero(); + const Eigen::Vector3d t_0_1 = T_0_1.translation(); + const Eigen::Matrix3d R_0_1 = T_0_1.rotationMatrix(); + + E.topLeftCorner<3, 3>() = Sophus::SO3d::hat(t_0_1.normalized()) * R_0_1; +} + +inline void findInliersEssential(const KeypointsData& kd1, + const KeypointsData& kd2, + const Eigen::Matrix4d& E, + double epipolar_error_threshold, + MatchData& md) { + md.inliers.clear(); + + for (size_t j = 0; j < md.matches.size(); j++) { + const Eigen::Vector4d p0_3d = kd1.corners_3d[md.matches[j].first]; + const Eigen::Vector4d p1_3d = kd2.corners_3d[md.matches[j].second]; + + const double epipolar_error = std::abs(p0_3d.transpose() * E * p1_3d); + + if (epipolar_error < epipolar_error_threshold) { + md.inliers.push_back(md.matches[j]); + } + } +} + +void findInliersRansac(const KeypointsData& kd1, const KeypointsData& kd2, + const double ransac_thresh, const int ransac_min_inliers, + MatchData& md); + +} // namespace basalt diff --git a/include/basalt/utils/nfr.h b/include/basalt/utils/nfr.h new file mode 100644 index 0000000..24d9fab --- /dev/null +++ b/include/basalt/utils/nfr.h @@ -0,0 +1,120 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include + +namespace basalt { + +inline Sophus::Vector6d relPoseError( + const Sophus::SE3d& T_i_j, const Sophus::SE3d& T_w_i, + const Sophus::SE3d& T_w_j, Sophus::Matrix6d* d_res_d_T_w_i = nullptr, + Sophus::Matrix6d* d_res_d_T_w_j = nullptr) { + Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i; + Sophus::Vector6d res = Sophus::logd(T_i_j * T_j_i); + + if (d_res_d_T_w_i || d_res_d_T_w_j) { + Sophus::Matrix6d J; + Sophus::rightJacobianInvSE3Decoupled(res, J); + + Eigen::Matrix3d R = T_w_i.so3().inverse().matrix(); + + Sophus::Matrix6d Adj; + Adj.setZero(); + Adj.topLeftCorner<3, 3>() = R; + Adj.bottomRightCorner<3, 3>() = R; + + if (d_res_d_T_w_i) { + *d_res_d_T_w_i = J * Adj; + } + + if (d_res_d_T_w_j) { + Adj.topRightCorner<3, 3>() = + Sophus::SO3d::hat(T_j_i.inverse().translation()) * R; + *d_res_d_T_w_j = -J * Adj; + } + } + + return res; +} + +inline Sophus::Vector3d absPositionError( + const Sophus::SE3d& T_w_i, const Eigen::Vector3d pos, + Eigen::Matrix* d_res_d_T_w_i = nullptr) { + if (d_res_d_T_w_i) { + d_res_d_T_w_i->topLeftCorner<3, 3>().setIdentity(); + d_res_d_T_w_i->topRightCorner<3, 3>().setZero(); + } + + return T_w_i.translation() - pos; +} + +inline double yawError(const Sophus::SE3d& T_w_i, + const Eigen::Vector3d yaw_dir_body, + Eigen::Matrix* d_res_d_T_w_i = nullptr) { + Eigen::Matrix3d curr_R_w_i = T_w_i.so3().matrix(); + Eigen::Vector3d tmp = curr_R_w_i * yaw_dir_body; + double res_yaw = tmp[1]; + + if (d_res_d_T_w_i) { + d_res_d_T_w_i->setZero(); + (*d_res_d_T_w_i)[3] = -tmp[2]; + (*d_res_d_T_w_i)[5] = tmp[0]; + } + + return res_yaw; +} + +inline Sophus::Vector2d rollPitchError( + const Sophus::SE3d& T_w_i, const Sophus::SO3d& R_w_i_meas, + Eigen::Matrix* d_res_d_T_w_i = nullptr) { + // Assumes g direction is negative Z in world coordinate frame + + Eigen::Matrix3d R = (R_w_i_meas * T_w_i.so3().inverse()).matrix(); + Eigen::Vector3d res = R * (-Eigen::Vector3d::UnitZ()); + + if (d_res_d_T_w_i) { + d_res_d_T_w_i->setZero(); + (*d_res_d_T_w_i)(0, 3) = -R(0, 1); + (*d_res_d_T_w_i)(1, 3) = -R(1, 1); + (*d_res_d_T_w_i)(0, 4) = R(0, 0); + (*d_res_d_T_w_i)(1, 4) = R(1, 0); + } + + return res.head<2>(); +} +} diff --git a/include/basalt/utils/sim_utils.h b/include/basalt/utils/sim_utils.h new file mode 100644 index 0000000..dc7dc42 --- /dev/null +++ b/include/basalt/utils/sim_utils.h @@ -0,0 +1,55 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +namespace basalt { + +struct SimObservations { + Eigen::vector pos; + std::vector id; +}; + +} + +namespace cereal { +template +void serialize(Archive& ar, basalt::SimObservations& c) { + ar(c.pos, c.id); +} +} + + diff --git a/include/basalt/utils/system_utils.h b/include/basalt/utils/system_utils.h new file mode 100644 index 0000000..1693791 --- /dev/null +++ b/include/basalt/utils/system_utils.h @@ -0,0 +1,52 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef BASALT_SYSTEM_UTILS_H +#define BASALT_SYSTEM_UTILS_H + +#include + +namespace basalt { + +inline std::string ensure_trailing_slash(const std::string& path) { + if (!path.empty() && path[path.size() - 1] != '/') { + return path + "/"; + } else { + return path; + } +} + +} // namespace basalt + +#endif // include guard diff --git a/include/basalt/utils/test_utils.h b/include/basalt/utils/test_utils.h new file mode 100644 index 0000000..10a2d5f --- /dev/null +++ b/include/basalt/utils/test_utils.h @@ -0,0 +1,77 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#ifndef BASALT_TEST_UTILS_H +#define BASALT_TEST_UTILS_H + +#include + +template +void test_jacobian_code(const std::string& name, + const Eigen::MatrixBase& Ja, F func, + const Eigen::MatrixBase& x0, + double eps = 1e-8, double max_norm = 1e-4) { + typedef typename Derived1::Scalar Scalar; + + Eigen::Matrix Jn = Ja; + Jn.setZero(); + + Eigen::Matrix inc = x0; + for (int i = 0; i < Jn.cols(); i++) { + inc.setZero(); + inc[i] += eps; + + Eigen::Matrix fpe = func(x0 + inc); + Eigen::Matrix fme = func(x0 - inc); + + Jn.col(i) = (fpe - fme) / (2 * eps); + } + + Scalar diff = (Ja - Jn).norm(); + + if (diff > max_norm || !Ja.allFinite()) { + std::cerr << name << std::endl; + std::cerr << "Numeric Jacobian is different from analytic. Norm difference " + << diff << std::endl; + std::cerr << "Ja\n" << Ja << std::endl; + std::cerr << "Jn\n" << Jn << std::endl; + } else { + // std::cout << name << std::endl; + // std::cout << "Success" << std::endl; + // std::cout << "Ja\n" << Ja << std::endl; + // std::cout << "Jn\n" << Jn << std::endl; + } +} + +#endif // BASALT_TEST_UTILS_H diff --git a/include/basalt/utils/tracks.h b/include/basalt/utils/tracks.h new file mode 100644 index 0000000..b6a4b83 --- /dev/null +++ b/include/basalt/utils/tracks.h @@ -0,0 +1,225 @@ +// Adapted from OpenMVG + +// Copyright (c) 2012, 2013 Pierre MOULON +// 2018 Nikolaus DEMMEL + +// This file was originally part of OpenMVG, an Open Multiple View Geometry C++ +// library. + +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +// Implementation of [1] an efficient algorithm to compute track from pairwise +// correspondences. +// +// [1] Pierre Moulon and Pascal Monasse, +// "Unordered feature tracking made fast and easy" CVMP 2012. +// +// It tracks the position of features along the series of image from pairwise +// correspondences. +// +// From map<[imageI,ImageJ], [indexed matches array] > it builds tracks. +// +// Usage : +// //--------------------------------------- +// // Compute tracks from matches +// //--------------------------------------- +// TrackBuilder trackBuilder; +// FeatureTracks tracks; +// trackBuilder.Build(matches); // Build: Efficient fusion of correspondences +// trackBuilder.Filter(); // Filter: Remove tracks that have conflict +// trackBuilder.Export(tracks); // Export tree to usable data structure + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +namespace basalt { + + + +/// TrackBuild class creates feature tracks from matches +struct TrackBuilder { + std::map map_node_to_index; + UnionFind uf_tree; + + /// Build tracks for a given series of pairWise matches + void Build(const Matches& map_pair_wise_matches) { + // 1. We need to know how much single set we will have. + // i.e each set is made of a tuple : (imageIndex, featureIndex) + std::set allFeatures; + // For each couple of images list the used features + for (const auto& iter : map_pair_wise_matches) { + const auto I = iter.first.first; + const auto J = iter.first.second; + const MatchData& matchData = iter.second; + + // Retrieve all shared features and add them to a set + for (const auto& match : matchData.inliers) { + allFeatures.emplace(I, match.first); + allFeatures.emplace(J, match.second); + } + } + + // 2. Build the 'flat' representation where a tuple (the node) + // is attached to a unique index. + TrackId cpt = 0; + for (const auto& feat : allFeatures) { + map_node_to_index.emplace(feat, cpt); + ++cpt; + } + // Clean some memory + allFeatures.clear(); + + // 3. Add the node and the pairwise correpondences in the UF tree. + uf_tree.InitSets(map_node_to_index.size()); + + // 4. Union of the matched features corresponding UF tree sets + for (const auto& iter : map_pair_wise_matches) { + const auto I = iter.first.first; + const auto J = iter.first.second; + const MatchData& matchData = iter.second; + for (const auto& match : matchData.inliers) { + const ImageFeaturePair pairI(I, match.first); + const ImageFeaturePair pairJ(J, match.second); + // Link feature correspondences to the corresponding containing sets. + uf_tree.Union(map_node_to_index[pairI], map_node_to_index[pairJ]); + } + } + } + + /// Remove bad tracks (too short or track with ids collision) + bool Filter(size_t minimumTrackLength = 2) { + // Remove bad tracks: + // - track that are too short, + // - track with id conflicts: + // i.e. tracks that have many times the same image index + + // From the UF tree, create tracks of the image indexes. + // If an image index appears twice the track must disappear + // If a track is too short it has to be removed. + std::map> tracks; + + std::set problematic_track_id; + // Build tracks from the UF tree, track problematic ids. + for (const auto& iter : map_node_to_index) { + const TrackId track_id = uf_tree.Find(iter.second); + if (problematic_track_id.count(track_id) != 0) { + continue; // Track already marked + } + + const ImageFeaturePair& feat = iter.first; + + if (tracks[track_id].count(feat.first)) { + problematic_track_id.insert(track_id); + } else { + tracks[track_id].insert(feat.first); + } + } + + // - track that are too short, + for (const auto& val : tracks) { + if (val.second.size() < minimumTrackLength) { + problematic_track_id.insert(val.first); + } + } + + for (uint32_t& root_index : uf_tree.m_cc_parent) { + if (problematic_track_id.count(root_index) > 0) { + // reset selected root + uf_tree.m_cc_size[root_index] = 1; + root_index = UnionFind::InvalidIndex(); + } + } + return false; + } + + /// Return the number of connected set in the UnionFind structure (tree + /// forest) + size_t TrackCount() const { + std::set parent_id(uf_tree.m_cc_parent.begin(), + uf_tree.m_cc_parent.end()); + // Erase the "special marker" that depicted rejected tracks + parent_id.erase(UnionFind::InvalidIndex()); + return parent_id.size(); + } + + /// Export tracks as a map (each entry is a map of imageId and + /// featureIndex): + /// {TrackIndex => {imageIndex => featureIndex}} + void Export(FeatureTracks& tracks) { + tracks.clear(); + for (const auto& iter : map_node_to_index) { + const TrackId track_id = uf_tree.Find(iter.second); + const ImageFeaturePair& feat = iter.first; + // ensure never add rejected elements (track marked as invalid) + if (track_id != UnionFind::InvalidIndex()) { + tracks[track_id].emplace(feat); + } + } + } +}; + +/// Find common tracks between images. +bool GetTracksInImages(const std::set& image_ids, + const FeatureTracks& all_tracks, + std::vector& shared_track_ids) { + shared_track_ids.clear(); + + // Go along the tracks + for (const auto& kv_track : all_tracks) { + // Look if the track contains the provided view index & save the point ids + size_t observed_image_count = 0; + for (const auto& imageId : image_ids) { + if (kv_track.second.count(imageId) > 0) { + ++observed_image_count; + } else { + break; + } + } + + if (observed_image_count == image_ids.size()) { + shared_track_ids.push_back(kv_track.first); + } + } + return !shared_track_ids.empty(); +} + +/// Find all tracks in an image. +bool GetTracksInImage(const TimeCamId& image_id, + const FeatureTracks& all_tracks, + std::vector& track_ids) { + std::set image_set; + image_set.insert(image_id); + return GetTracksInImages(image_set, all_tracks, track_ids); +} + +/// Find shared tracks between map and image +bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks, + const Landmarks& landmarks, + std::vector& track_ids) { + track_ids.clear(); + for (const auto& kv : landmarks) { + const TrackId trackId = kv.first; + if (all_tracks.at(trackId).count(image_id) > 0) { + track_ids.push_back(trackId); + } + } + return !track_ids.empty(); +} + +} diff --git a/include/basalt/utils/union_find.h b/include/basalt/utils/union_find.h new file mode 100644 index 0000000..4f4a4c1 --- /dev/null +++ b/include/basalt/utils/union_find.h @@ -0,0 +1,94 @@ +// Adapted from OpenMVG + +// Copyright (c) 2016 Pierre MOULON +// 2018 Nikolaus DEMMEL + +// This file was originally part of OpenMVG, an Open Multiple View Geometry C++ +// library. + +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include + +// Union-Find/Disjoint-Set data structure +//-- +// A disjoint-set data structure also called a union–find data structure +// or merge–find set, is a data structure that keeps track of a set of elements +// partitioned into a number of disjoint (non-overlapping) subsets. +// It supports two operations: +// - Find: Determine which subset a particular element is in. +// - It returns an item from this set that serves as its "representative"; +// - Union: Join two subsets into a single subset. +// Sometime a Connected method is implemented: +// - Connected: +// - By comparing the result of two Find operations, one can determine whether +// two elements are in the same subset. +//-- +struct UnionFind { + using ValueType = uint32_t; + + // Special Value for invalid parent index + static ValueType InvalidIndex() { + return std::numeric_limits::max(); + } + + // Represent the DS/UF forest thanks to two array: + // A parent 'pointer tree' where each node holds a reference to its parent + // node + std::vector m_cc_parent; + // A rank array used for union by rank + std::vector m_cc_rank; + // A 'size array' to know the size of each connected component + std::vector m_cc_size; + + // Init the UF structure with num_cc nodes + void InitSets(const ValueType num_cc) { + // all set size are 1 (independent nodes) + m_cc_size.resize(num_cc, 1); + // Parents id have their own CC id {0,n} + m_cc_parent.resize(num_cc); + std::iota(m_cc_parent.begin(), m_cc_parent.end(), 0); + // Rank array (0) + m_cc_rank.resize(num_cc, 0); + } + + // Return the number of nodes that have been initialized in the UF tree + std::size_t GetNumNodes() const { return m_cc_size.size(); } + + // Return the representative set id of I nth component + ValueType Find(ValueType i) { + // Recursively set all branch as children of root (Path compression) + if (m_cc_parent[i] != i && m_cc_parent[i] != InvalidIndex()) + m_cc_parent[i] = Find(m_cc_parent[i]); + return m_cc_parent[i]; + } + + // Replace sets containing I and J with their union + void Union(ValueType i, ValueType j) { + i = Find(i); + j = Find(j); + if (i == j) { // Already in the same set. Nothing to do + return; + } + + // x and y are not already in same set. Merge them. + // Perform an union by rank: + // - always attach the smaller tree to the root of the larger tree + if (m_cc_rank[i] < m_cc_rank[j]) { + m_cc_parent[i] = j; + m_cc_size[j] += m_cc_size[i]; + + } else { + m_cc_parent[j] = i; + m_cc_size[i] += m_cc_size[j]; + if (m_cc_rank[i] == m_cc_rank[j]) ++m_cc_rank[i]; + } + } +}; diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h new file mode 100644 index 0000000..abb8477 --- /dev/null +++ b/include/basalt/utils/vio_config.h @@ -0,0 +1,75 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +namespace basalt { + +struct VioConfig { + VioConfig(); + void load(const std::string& filename); + void save(const std::string& filename); + + std::string optical_flow_type; + int optical_flow_detection_grid_size; + float optical_flow_max_recovered_dist2; + int optical_flow_pattern; + int optical_flow_max_iterations; + int optical_flow_levels; + float optical_flow_epipolar_error; + + int vio_max_states; + int vio_max_kfs; + int vio_min_frames_after_kf; + float vio_new_kf_keypoints_thresh; + int vio_max_iterations; + bool vio_debug; + + double vio_obs_std_dev; + double vio_obs_huber_thresh; + + double mapper_obs_std_dev; + double mapper_obs_huber_thresh; + int mapper_detection_num_points; + double mapper_num_frames_to_match; + double mapper_frames_to_match_threshold; + double mapper_min_matches; + double mapper_ransac_threshold; + double mapper_min_track_length; + double mapper_max_hamming_distance; + double mapper_second_best_test_ratio; +}; +} // namespace basalt diff --git a/include/basalt/utils/vis_utils.h b/include/basalt/utils/vis_utils.h new file mode 100644 index 0000000..41c581e --- /dev/null +++ b/include/basalt/utils/vis_utils.h @@ -0,0 +1,106 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include + +#include + +const u_int8_t cam_color[3]{250, 0, 26}; +const u_int8_t state_color[3]{250, 0, 26}; +const u_int8_t pose_color[3]{0, 50, 255}; +const u_int8_t gt_color[3]{0, 171, 47}; + +inline void render_camera(const Eigen::Matrix4d& T_w_c, float lineWidth, + const u_int8_t* color, float sizeFactor) { + const float sz = sizeFactor; + const float width = 640, height = 480, fx = 500, fy = 500, cx = 320, cy = 240; + + const Eigen::vector lines = { + {0, 0, 0}, + {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz}, + {0, 0, 0}, + {sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz}, + {0, 0, 0}, + {sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz}, + {0, 0, 0}, + {sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz}, + {sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz}, + {sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz}, + {sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz}, + {sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz}, + {sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz}, + {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz}, + {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz}, + {sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz}}; + + glPushMatrix(); + glMultMatrixd(T_w_c.data()); + glColor3ubv(color); + glLineWidth(lineWidth); + pangolin::glDrawLines(lines); + glPopMatrix(); +} + +inline void getcolor(float p, float np, float& r, float& g, float& b) { + float inc = 4.0 / np; + float x = p * inc; + r = 0.0f; + g = 0.0f; + b = 0.0f; + + if ((0 <= x && x <= 1) || (5 <= x && x <= 6)) + r = 1.0f; + else if (4 <= x && x <= 5) + r = x - 4; + else if (1 <= x && x <= 2) + r = 1.0f - (x - 1); + + if (1 <= x && x <= 3) + g = 1.0f; + else if (0 <= x && x <= 1) + g = x - 0; + else if (3 <= x && x <= 4) + g = 1.0f - (x - 3); + + if (3 <= x && x <= 5) + b = 1.0f; + else if (2 <= x && x <= 3) + b = x - 2; + else if (5 <= x && x <= 6) + b = 1.0f - (x - 5); +} diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h new file mode 100644 index 0000000..829aca5 --- /dev/null +++ b/include/basalt/vi_estimator/ba_base.h @@ -0,0 +1,372 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include + +namespace basalt { + +class BundleAdjustmentBase { + public: + // keypoint position defined relative to some frame + struct KeypointPosition { + TimeCamId kf_id; + Eigen::Vector2d dir; + double id; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + struct KeypointObservation { + int kpt_id; + Eigen::Vector2d pos; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + struct RelLinDataBase { + std::vector> order; + + Eigen::vector d_rel_d_h; + Eigen::vector d_rel_d_t; + }; + + struct FrameRelLinData { + Sophus::Matrix6d Hpp; + Sophus::Vector6d bp; + + std::vector lm_id; + Eigen::vector> Hpl; + + FrameRelLinData() { + Hpp.setZero(); + bp.setZero(); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + struct RelLinData : public RelLinDataBase { + RelLinData(size_t num_keypoints, size_t num_rel_poses) { + Hll.reserve(num_keypoints); + bl.reserve(num_keypoints); + lm_to_obs.reserve(num_keypoints); + + Hpppl.reserve(num_rel_poses); + order.reserve(num_rel_poses); + + d_rel_d_h.reserve(num_rel_poses); + d_rel_d_t.reserve(num_rel_poses); + + error = 0; + } + + void invert_keypoint_hessians() { + for (auto& kv : Hll) { + Eigen::Matrix3d Hll_inv; + Hll_inv.setIdentity(); + kv.second.ldlt().solveInPlace(Hll_inv); + kv.second = Hll_inv; + } + } + + Eigen::unordered_map Hll; + Eigen::unordered_map bl; + Eigen::unordered_map>> lm_to_obs; + + Eigen::vector Hpppl; + + double error; + }; + + void computeError(double& error) const; + + void linearizeHelper( + Eigen::vector& rld_vec, + const Eigen::map< + TimeCamId, Eigen::map>>& + obs_to_lin, + double& error) const; + + static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H, + Eigen::VectorXd& b); + + template + static bool linearizePoint( + const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos, + const Eigen::Matrix4d& T_t_h, const CamT& cam, Eigen::Vector2d& res, + Eigen::Matrix* d_res_d_xi = nullptr, + Eigen::Matrix* d_res_d_p = nullptr, + Eigen::Vector4d* proj = nullptr) { + // Todo implement without jacobians + Eigen::Matrix Jup; + Eigen::Vector4d p_h_3d; + p_h_3d = StereographicParam::unproject(kpt_pos.dir, &Jup); + p_h_3d[3] = kpt_pos.id; + + Eigen::Vector4d p_t_3d = T_t_h * p_h_3d; + + Eigen::Matrix d_point_d_xi; + d_point_d_xi.topLeftCorner<3, 3>() = + Eigen::Matrix3d::Identity() * kpt_pos.id; + d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(p_t_3d.head<3>()); + d_point_d_xi.row(3).setZero(); + + Eigen::Matrix Jp; + bool valid = cam.project(p_t_3d, res, &Jp); + + if (!valid) { + // std::cerr << " Invalid projection! kpt_pos.dir " + // << kpt_pos.dir.transpose() << " kpt_pos.id " << + // kpt_pos.id + // << " idx " << kpt_obs.kpt_id << std::endl; + + // std::cerr << "T_t_h\n" << T_t_h << std::endl; + // std::cerr << "p_h_3d\n" << p_h_3d.transpose() << std::endl; + // std::cerr << "p_t_3d\n" << p_t_3d.transpose() << std::endl; + + return false; + } + + if (proj) { + proj->head<2>() = res; + (*proj)[2] = p_t_3d[3] / p_t_3d.head<3>().norm(); + } + res -= kpt_obs.pos; + + if (d_res_d_xi) { + *d_res_d_xi = Jp * d_point_d_xi; + } + + if (d_res_d_p) { + Eigen::Matrix Jpp; + Jpp.block<3, 2>(0, 0) = T_t_h.topLeftCorner<3, 4>() * Jup; + Jpp.col(2) = T_t_h.col(3); + + *d_res_d_p = Jp * Jpp; + } + + return true; + } + + template + inline static bool linearizePoint( + const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos, + const CamT& cam, Eigen::Vector2d& res, + Eigen::Matrix* d_res_d_p = nullptr, + Eigen::Vector4d* proj = nullptr) { + // Todo implement without jacobians + Eigen::Matrix Jup; + Eigen::Vector4d p_h_3d; + p_h_3d = StereographicParam::unproject(kpt_pos.dir, &Jup); + + Eigen::Matrix Jp; + bool valid = cam.project(p_h_3d, res, &Jp); + + if (!valid) { + // std::cerr << " Invalid projection! kpt_pos.dir " + // << kpt_pos.dir.transpose() << " kpt_pos.id " << + // kpt_pos.id + // << " idx " << kpt_obs.kpt_id << std::endl; + // std::cerr << "p_h_3d\n" << p_h_3d.transpose() << std::endl; + + return false; + } + + if (proj) { + proj->head<2>() = res; + (*proj)[2] = kpt_pos.id; + } + res -= kpt_obs.pos; + + if (d_res_d_p) { + Eigen::Matrix Jpp; + Jpp.block<4, 2>(0, 0) = Jup; + Jpp.col(2).setZero(); + + *d_res_d_p = Jp * Jpp; + } + + return true; + } + + void updatePoints(const AbsOrderMap& aom, const RelLinData& rld, + const Eigen::VectorXd& inc); + + static Sophus::SE3d computeRelPose(const Sophus::SE3d& T_w_i_h, + const Sophus::SE3d& T_w_i_t, + const Sophus::SE3d& T_i_c_h, + const Sophus::SE3d& T_i_c_t, + Sophus::Matrix6d* d_rel_d_h = nullptr, + Sophus::Matrix6d* d_rel_d_t = nullptr); + + void get_current_points(Eigen::vector& points, + std::vector& ids) const; + + // Modifies abs_H and abs_b as a side effect. + static void marginalizeHelper(Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, + const std::set& idx_to_keep, + const std::set& idx_to_marg, + Eigen::MatrixXd& marg_H, + Eigen::VectorXd& marg_b); + + static Eigen::Vector4d triangulate(const Eigen::Vector3d& p0_3d, + const Eigen::Vector3d& p1_3d, + const Sophus::SE3d& T_0_1); + + template + static void linearizeAbs(const Eigen::MatrixXd& rel_H, + const Eigen::VectorXd& rel_b, + const RelLinDataBase& rld, const AbsOrderMap& aom, + AccumT& accum) { + // int asize = aom.total_size; + + // BASALT_ASSERT(abs_H.cols() == asize); + // BASALT_ASSERT(abs_H.rows() == asize); + // BASALT_ASSERT(abs_b.rows() == asize); + + for (size_t i = 0; i < rld.order.size(); i++) { + const TimeCamId& tcid_h = rld.order[i].first; + const TimeCamId& tcid_ti = rld.order[i].second; + + int abs_h_idx = aom.abs_order_map.at(tcid_h.first).first; + int abs_ti_idx = aom.abs_order_map.at(tcid_ti.first).first; + + accum.template addB( + abs_h_idx, rld.d_rel_d_h[i].transpose() * + rel_b.segment(i * POSE_SIZE)); + accum.template addB( + abs_ti_idx, rld.d_rel_d_t[i].transpose() * + rel_b.segment(i * POSE_SIZE)); + + for (size_t j = 0; j < rld.order.size(); j++) { + BASALT_ASSERT(rld.order[i].first == rld.order[j].first); + + const TimeCamId& tcid_tj = rld.order[j].second; + + int abs_tj_idx = aom.abs_order_map.at(tcid_tj.first).first; + + if (tcid_h.first == tcid_ti.first || tcid_h.first == tcid_tj.first) + continue; + + accum.template addH( + abs_h_idx, abs_h_idx, rld.d_rel_d_h[i].transpose() * + rel_H.block( + POSE_SIZE * i, POSE_SIZE * j) * + rld.d_rel_d_h[j]); + + accum.template addH( + abs_ti_idx, abs_h_idx, rld.d_rel_d_t[i].transpose() * + rel_H.block( + POSE_SIZE * i, POSE_SIZE * j) * + rld.d_rel_d_h[j]); + + accum.template addH( + abs_h_idx, abs_tj_idx, rld.d_rel_d_h[i].transpose() * + rel_H.block( + POSE_SIZE * i, POSE_SIZE * j) * + rld.d_rel_d_t[j]); + + accum.template addH( + abs_ti_idx, abs_tj_idx, rld.d_rel_d_t[i].transpose() * + rel_H.block( + POSE_SIZE * i, POSE_SIZE * j) * + rld.d_rel_d_t[j]); + } + } + } + + template + struct LinearizeAbsReduce { + using RelLinDataIter = Eigen::vector::iterator; + + LinearizeAbsReduce(AbsOrderMap& aom) : aom(aom) { + accum.reset(aom.total_size); + } + + LinearizeAbsReduce(const LinearizeAbsReduce& other, tbb::split) + : aom(other.aom) { + accum.reset(aom.total_size); + } + + void operator()(const tbb::blocked_range& range) { + for (RelLinData& rld : range) { + rld.invert_keypoint_hessians(); + + Eigen::MatrixXd rel_H; + Eigen::VectorXd rel_b; + linearizeRel(rld, rel_H, rel_b); + + linearizeAbs(rel_H, rel_b, rld, aom, accum); + } + } + + void join(LinearizeAbsReduce& rhs) { accum.join(rhs.accum); } + + AbsOrderMap& aom; + AccumT accum; + }; + + // protected: + PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const { + auto it = frame_poses.find(t_ns); + if (it != frame_poses.end()) return it->second; + + auto it2 = frame_states.find(t_ns); + if (it2 == frame_states.end()) { + std::cerr << "Could not find pose " << t_ns << std::endl; + std::abort(); + } + + return PoseStateWithLin(it2->second); + } + + Eigen::map frame_states; + Eigen::map frame_poses; + + // Point management + Eigen::unordered_map kpts; + Eigen::map>> + obs; + + double obs_std_dev; + double huber_thresh; + + basalt::Calibration calib; +}; +} diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h new file mode 100644 index 0000000..ad6ee28 --- /dev/null +++ b/include/basalt/vi_estimator/keypoint_vio.h @@ -0,0 +1,229 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace basalt { + +class KeypointVioEstimator : public VioEstimatorBase, + public BundleAdjustmentBase { + public: + typedef std::shared_ptr Ptr; + + static const int N = 9; + typedef Eigen::Matrix VecN; + typedef Eigen::Matrix MatNN; + typedef Eigen::Matrix MatN3; + + static constexpr double prior_weight = 1e8; + + KeypointVioEstimator(int64_t t_ns, const Sophus::SE3d& T_w_i, + const Eigen::Vector3d& vel_w_i, + const Eigen::Vector3d& bg, const Eigen::Vector3d& ba, + double int_std_dev, const Eigen::Vector3d& g, + const basalt::Calibration& calib, + const VioConfig& config); + + ~KeypointVioEstimator() { processing_thread->join(); } + + void addIMUToQueue(const ImuData::Ptr& data); + void addVisionToQueue(const OpticalFlowResult::Ptr& data); + + bool measure(const OpticalFlowResult::Ptr& data, + const IntegratedImuMeasurement::Ptr& meas); + + static void linearizeAbsIMU( + const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, + double& imu_error, double& bg_error, double& ba_error, + const Eigen::map& states, + const Eigen::map& imu_meas, + const Eigen::Vector3d& gyro_bias_weight, + const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); + + static void computeImuError( + const AbsOrderMap& aom, double& imu_error, double& bg_error, + double& ba_error, + const Eigen::map& states, + const Eigen::map& imu_meas, + const Eigen::Vector3d& gyro_bias_weight, + const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); + + void linearizeMargPrior(const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, + Eigen::VectorXd& abs_b, + double& marg_prior_error) const; + + void computeMargPriorError(double& marg_prior_error) const; + + void computeDelta(const AbsOrderMap& marg_order, + Eigen::VectorXd& delta) const; + + // int64_t propagate(); + // void addNewState(int64_t data_t_ns); + + void marginalize(const std::map& num_points_connected); + + void optimize(); + + void checkMargNullspace() const; + static Eigen::VectorXd checkNullspace( + const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b, + const AbsOrderMap& marg_order, + const Eigen::map& frame_states, + const Eigen::map& frame_poses); + + int64_t get_t_ns() const { + return frame_states.at(last_state_t_ns).getState().t_ns; + } + const Sophus::SE3d& get_T_w_i() const { + return frame_states.at(last_state_t_ns).getState().T_w_i; + } + const Eigen::Vector3d& get_vel_w_i() const { + return frame_states.at(last_state_t_ns).getState().vel_w_i; + } + + const PoseVelBiasState& get_state() const { + return frame_states.at(last_state_t_ns).getState(); + } + PoseVelBiasState get_state(int64_t t_ns) const { + PoseVelBiasState state; + + auto it = frame_states.find(t_ns); + + if (it != frame_states.end()) { + return it->second.getState(); + } + + auto it2 = frame_poses.find(t_ns); + if (it2 != frame_poses.end()) { + state.T_w_i = it2->second.getPose(); + } + + return state; + } + // const MatNN get_cov() const { return cov.bottomRightCorner(); } + + void computeProjections( + std::vector>& res) const; + + inline void setMaxStates(size_t val) { max_states = val; } + inline void setMaxKfs(size_t val) { max_kfs = val; } + + Eigen::vector getFrameStates() const { + Eigen::vector res; + + for (const auto& kv : frame_states) { + res.push_back(kv.second.getState().T_w_i); + } + + return res; + } + + Eigen::vector getFramePoses() const { + Eigen::vector res; + + for (const auto& kv : frame_poses) { + res.push_back(kv.second.getPose()); + } + + return res; + } + + Eigen::map getAllPosesMap() const { + Eigen::map res; + + for (const auto& kv : frame_poses) { + res[kv.first] = kv.second.getPose(); + } + + for (const auto& kv : frame_states) { + res[kv.first] = kv.second.getState().T_w_i; + } + + return res; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + bool take_kf; + int frames_after_kf; + std::set kf_ids; + + int64_t last_state_t_ns; + Eigen::map imu_meas; + + const Eigen::Vector3d g; + + // Input + + Eigen::map prev_opt_flow_res; + + std::map num_points_kf; + + // Marginalization + AbsOrderMap marg_order; + Eigen::MatrixXd marg_H; + Eigen::VectorXd marg_b; + + Eigen::Vector3d gyro_bias_weight, accel_bias_weight; + + size_t max_states; + size_t max_kfs; + + bool opt_started; + + VioConfig config; + + int64_t msckf_kf_id; + + std::shared_ptr processing_thread; +}; +} // namespace basalt diff --git a/include/basalt/vi_estimator/nfr_mapper.h b/include/basalt/vi_estimator/nfr_mapper.h new file mode 100644 index 0000000..d1710ab --- /dev/null +++ b/include/basalt/vi_estimator/nfr_mapper.h @@ -0,0 +1,218 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace basalt { + +class NfrMapper : public BundleAdjustmentBase { + public: + using Ptr = std::shared_ptr; + using TimeCamId = std::pair; + using Matches = tbb::concurrent_unordered_map< + std::pair, MatchData, + tbb::tbb_hash>, + std::equal_to>, + Eigen::aligned_allocator< + std::pair, MatchData>>>; + + template + struct MapperLinearizeAbsReduce + : public BundleAdjustmentBase::LinearizeAbsReduce { + using RollPitchFactorConstIter = + Eigen::vector::const_iterator; + using RelPoseFactorConstIter = Eigen::vector::const_iterator; + using RelLinDataIter = Eigen::vector::iterator; + + MapperLinearizeAbsReduce( + AbsOrderMap& aom, + const Eigen::map* frame_poses) + : BundleAdjustmentBase::LinearizeAbsReduce(aom), + frame_poses(frame_poses) { + this->accum.reset(aom.total_size); + roll_pitch_error = 0; + rel_error = 0; + } + + MapperLinearizeAbsReduce(const MapperLinearizeAbsReduce& other, tbb::split) + : BundleAdjustmentBase::LinearizeAbsReduce(other.aom), + frame_poses(other.frame_poses) { + this->accum.reset(this->aom.total_size); + roll_pitch_error = 0; + rel_error = 0; + } + + void join(MapperLinearizeAbsReduce& rhs) { + this->accum.join(rhs.accum); + roll_pitch_error += rhs.roll_pitch_error; + rel_error += rhs.rel_error; + } + + void operator()(const tbb::blocked_range& range) { + for (RelLinData& rld : range) { + rld.invert_keypoint_hessians(); + + Eigen::MatrixXd rel_H; + Eigen::VectorXd rel_b; + linearizeRel(rld, rel_H, rel_b); + + linearizeAbs(rel_H, rel_b, rld, this->aom, this->accum); + } + } + + void operator()(const tbb::blocked_range& range) { + for (const RollPitchFactor& rpf : range) { + const Sophus::SE3d& pose = frame_poses->at(rpf.t_ns).getPose(); + + int idx = this->aom.abs_order_map.at(rpf.t_ns).first; + + Eigen::Matrix J; + Sophus::Vector2d res = basalt::rollPitchError(pose, rpf.R_w_i_meas, &J); + + this->accum.template addH( + idx, idx, J.transpose() * rpf.cov_inv * J); + this->accum.template addB(idx, + J.transpose() * rpf.cov_inv * res); + + roll_pitch_error += res.transpose() * rpf.cov_inv * res; + } + } + + void operator()(const tbb::blocked_range& range) { + for (const RelPoseFactor& rpf : range) { + const Sophus::SE3d& pose_i = frame_poses->at(rpf.t_i_ns).getPose(); + const Sophus::SE3d& pose_j = frame_poses->at(rpf.t_j_ns).getPose(); + + int idx_i = this->aom.abs_order_map.at(rpf.t_i_ns).first; + int idx_j = this->aom.abs_order_map.at(rpf.t_j_ns).first; + + Sophus::Matrix6d Ji, Jj; + Sophus::Vector6d res = + basalt::relPoseError(rpf.T_i_j, pose_i, pose_j, &Ji, &Jj); + + this->accum.template addH( + idx_i, idx_i, Ji.transpose() * rpf.cov_inv * Ji); + this->accum.template addH( + idx_i, idx_j, Ji.transpose() * rpf.cov_inv * Jj); + this->accum.template addH( + idx_j, idx_i, Jj.transpose() * rpf.cov_inv * Ji); + this->accum.template addH( + idx_j, idx_j, Jj.transpose() * rpf.cov_inv * Jj); + + this->accum.template addB( + idx_i, Ji.transpose() * rpf.cov_inv * res); + this->accum.template addB( + idx_j, Jj.transpose() * rpf.cov_inv * res); + + rel_error += res.transpose() * rpf.cov_inv * res; + } + } + + double roll_pitch_error; + double rel_error; + + const Eigen::map* frame_poses; + }; + + NfrMapper(const basalt::Calibration& calib, const VioConfig& config, + const std::string& vocabulary = ""); + + void addMargData(basalt::MargData::Ptr& data); + + void processMargData(basalt::MargData& m); + + void extractNonlinearFactors(basalt::MargData& m); + + void optimize(int num_iterations = 10); + + Eigen::map& getFramePoses(); + + void computeRelPose(double& rel_error); + + void computeRollPitch(double& roll_pitch_error); + + void detect_keypoints(); + + // Feature matching and inlier filtering for stereo pairs with known pose + void match_stereo(); + + void match_all(); + + void build_tracks(); + + void setup_opt(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Eigen::vector roll_pitch_factors; + Eigen::vector rel_pose_factors; + + std::unordered_map img_data; + + tbb::concurrent_unordered_map< + TimeCamId, std::pair> + bow_data; + + Corners feature_corners; + + Matches feature_matches; + + FeatureTracks feature_tracks; + + DBoW3::Database bow_database; + + std::unordered_map bow_id_to_tcid; + + VioConfig config; +}; +} // namespace basalt diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h new file mode 100644 index 0000000..13f4bd6 --- /dev/null +++ b/include/basalt/vi_estimator/vio_estimator.h @@ -0,0 +1,102 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ +#pragma once + +#include + +#include +#include + +namespace basalt { + +struct VioVisualizationData { + typedef std::shared_ptr Ptr; + + int64_t t_ns; + + Eigen::vector states; + Eigen::vector frames; + + Eigen::vector points; + std::vector point_ids; + + OpticalFlowResult::Ptr opt_flow_res; + + std::vector> projections; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class VioEstimatorBase { + public: + typedef std::shared_ptr Ptr; + + VioEstimatorBase() + : out_state_queue(nullptr), + out_marg_queue(nullptr), + out_vis_queue(nullptr) { + vision_data_queue.set_capacity(10); + imu_data_queue.set_capacity(300); + last_processed_t_ns = 0; + finished = false; + } + + std::atomic last_processed_t_ns; + std::atomic finished; + + tbb::concurrent_bounded_queue vision_data_queue; + tbb::concurrent_bounded_queue imu_data_queue; + + tbb::concurrent_bounded_queue* out_state_queue = + nullptr; + tbb::concurrent_bounded_queue* out_marg_queue = nullptr; + tbb::concurrent_bounded_queue* out_vis_queue = + nullptr; +}; + +class VioEstimatorFactory { + public: + static VioEstimatorBase::Ptr getVioEstimator( + const VioConfig& config, const Calibration& cam, int64_t t_ns, + const Sophus::SE3d& T_w_i, const Eigen::Vector3d& vel_w_i, + const Eigen::Vector3d& bg, const Eigen::Vector3d& ba, double int_std_dev, + const Eigen::Vector3d& g); +}; + +double alignSVD(const std::vector& filter_t_ns, + const Eigen::vector& filter_t_w_i, + const std::vector& gt_t_ns, + Eigen::vector& gt_t_w_i); +} // namespace basalt diff --git a/scripts/eval_full/gen_results.py b/scripts/eval_full/gen_results.py new file mode 100755 index 0000000..a1a551a --- /dev/null +++ b/scripts/eval_full/gen_results.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +import os +import sys +import numpy as np + + +datasets = ['MH_01_easy', 'MH_02_easy', 'MH_03_medium', 'MH_04_difficult', +'MH_05_difficult', 'V1_01_easy', 'V1_02_medium', +'V1_03_difficult', 'V2_01_easy', 'V2_02_medium'] + + +# Other results. +results_vio = [] +results_mapping = [] + +out_dir = sys.argv[1] + + +for key in datasets: + fname = out_dir + '/vio_' + key + if os.path.isfile(fname): + res = round(float(np.loadtxt(fname)), 3) + results_vio.append(float(res)) + else: + results_vio.append(float('Inf')) + + fname = out_dir + '/mapper_' + key + if os.path.isfile(fname): + res = round(float(np.loadtxt(fname)), 3) + results_mapping.append(float(res)) + else: + results_mapping.append(float('Inf')) + +row_format ="{:>17}" * (len(datasets)) +print row_format.format(*datasets) +print row_format.format(*results_vio) +print row_format.format(*results_mapping) + diff --git a/scripts/eval_full/run_evaluations.sh b/scripts/eval_full/run_evaluations.sh new file mode 100755 index 0000000..8b4e5e5 --- /dev/null +++ b/scripts/eval_full/run_evaluations.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +set -e +set -x + +DATASET_PATH=/data/euroc + +DATASETS=(MH_01_easy MH_02_easy MH_03_medium MH_04_difficult MH_05_difficult V1_01_easy V1_02_medium V1_03_difficult V2_01_easy V2_02_medium V2_03_difficult) + + +folder_name=eval_$(date +%Y%m%d_%H%M%S) +mkdir $folder_name + +cp ../../data/euroc_config.json $folder_name/config.json + + +for d in ${DATASETS[@]}; do + ../../build/basalt_vio --dataset-path $DATASET_PATH/$d --cam-calib ../../data/euroc_ds_calib.json \ + --dataset-type euroc --show-gui 0 --config-path $folder_name/config.json \ + --result-path $folder_name/vio_$d --marg-data $folder_name/marg_$d/ + + ../../build/basalt_mapper --show-gui 0 --cam-calib ../../data/euroc_ds_calib.json --marg-data $folder_name/marg_$d/ \ + --vocabulary ../../thirdparty/DBoW3/vocab/orbvoc.dbow3 --result-path $folder_name/mapper_$d +done + +./gen_results.py $folder_name diff --git a/scripts/install_deps.sh b/scripts/install_deps.sh new file mode 100755 index 0000000..b46f731 --- /dev/null +++ b/scripts/install_deps.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" + +if [[ "$OSTYPE" == "darwin"* ]]; then +${DIR}/install_mac_os_deps.sh +else +${DIR}/install_ubuntu_deps.sh +fi diff --git a/scripts/install_mac_os_deps.sh b/scripts/install_mac_os_deps.sh new file mode 100755 index 0000000..237c253 --- /dev/null +++ b/scripts/install_mac_os_deps.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +brew install clang-format tbb glew eigen ccache +brew install --with-toolchain llvm diff --git a/scripts/install_ubuntu_deps.sh b/scripts/install_ubuntu_deps.sh new file mode 100755 index 0000000..51521de --- /dev/null +++ b/scripts/install_ubuntu_deps.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +sudo apt-get update +sudo apt-get install -y gcc g++ cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libgtest-dev libopencv-dev diff --git a/scripts/update_submodules.sh b/scripts/update_submodules.sh new file mode 100755 index 0000000..3d7d068 --- /dev/null +++ b/scripts/update_submodules.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash + +set -x + +git submodule sync --recursive +git submodule update --init --recursive + diff --git a/src/calibrate.cpp b/src/calibrate.cpp new file mode 100644 index 0000000..8555328 --- /dev/null +++ b/src/calibrate.cpp @@ -0,0 +1,82 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include + +#include + +#include + +int main(int argc, char **argv) { + tbb::task_scheduler_init init( + tbb::task_scheduler_init::default_num_threads()); + + std::string dataset_path; + std::string dataset_type; + std::string result_path; + std::vector cam_types; + std::string cache_dataset_name = "calib-cam"; + int skip_images = 1; + + CLI::App app{"Calibrate IMU"}; + + app.add_option("--dataset-path", dataset_path, "Path to dataset")->required(); + app.add_option("--result-path", result_path, "Path to result folder") + ->required(); + app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)") + ->required(); + + app.add_option("--cache-name", cache_dataset_name, + "Name to save cached files"); + + app.add_option("--skip-images", skip_images, "Number of images to skip"); + app.add_option("--cam-types", cam_types, + "Type of cameras (eucm, ds, kb4, pinhole)") + ->required(); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError &e) { + return app.exit(e); + } + + basalt::CamCalib cv(dataset_path, dataset_type, result_path, + cache_dataset_name, skip_images, cam_types); + + cv.renderingLoop(); + + return 0; +} diff --git a/src/calibrate_imu.cpp b/src/calibrate_imu.cpp new file mode 100644 index 0000000..f5add27 --- /dev/null +++ b/src/calibrate_imu.cpp @@ -0,0 +1,94 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include + +#include + +#include + +int main(int argc, char **argv) { + tbb::task_scheduler_init init( + tbb::task_scheduler_init::default_num_threads()); + + std::string dataset_path; + std::string dataset_type; + std::string result_path; + std::string cache_dataset_name = "calib-cam-imu"; + int skip_images = 1; + + double accel_noise_std = 0.016; + double gyro_noise_std = 0.000282; + double accel_bias_std = 0.001; + double gyro_bias_std = 0.0001; + + CLI::App app{"Calibrate IMU"}; + + app.add_option("--dataset-path", dataset_path, "Path to dataset")->required(); + app.add_option("--result-path", result_path, "Path to result folder") + ->required(); + app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)") + ->required(); + + app.add_option("--gyro-noise-std", gyro_noise_std, "Gyroscope noise std"); + app.add_option("--accel-noise-std", accel_noise_std, + "Accelerometer noise std"); + + app.add_option("--gyro-bias-std", accel_bias_std, + "Gyroscope bias random walk std"); + app.add_option("--accel-bias-std", gyro_bias_std, + "Accelerometer bias random walk std"); + + app.add_option("--cache-name", cache_dataset_name, + "Name to save cached files"); + + app.add_option("--skip-images", skip_images, "Number of images to skip"); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError &e) { + return app.exit(e); + } + + basalt::CamImuCalib cv( + dataset_path, dataset_type, result_path, cache_dataset_name, skip_images, + {accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std}); + + cv.renderingLoop(); + + return 0; +} diff --git a/src/calibration/calibraiton_helper.cpp b/src/calibration/calibraiton_helper.cpp new file mode 100644 index 0000000..0a72e5e --- /dev/null +++ b/src/calibration/calibraiton_helper.cpp @@ -0,0 +1,361 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include + +#include +#include + +#include +#include + +#include +#include +#include + +namespace basalt { + +template +bool estimateTransformation( + const CamT &cam_calib, const Eigen::vector &corners, + const std::vector &corner_ids, + const Eigen::vector &aprilgrid_corner_pos_3d, + Sophus::SE3d &T_target_camera, size_t &num_inliers) { + opengv::bearingVectors_t bearingVectors; + opengv::points_t points; + + for (size_t i = 0; i < corners.size(); i++) { + Eigen::Vector4d tmp; + cam_calib.unproject(corners[i], tmp); + Eigen::Vector3d bearing = tmp.head<3>(); + Eigen::Vector3d point = aprilgrid_corner_pos_3d[corner_ids[i]].head<3>(); + bearing.normalize(); + + bearingVectors.push_back(bearing); + points.push_back(point); + } + + opengv::absolute_pose::CentralAbsoluteAdapter adapter(bearingVectors, points); + + opengv::sac::Ransac< + opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> + ransac; + std::shared_ptr + absposeproblem_ptr( + new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem( + adapter, opengv::sac_problems::absolute_pose:: + AbsolutePoseSacProblem::KNEIP)); + ransac.sac_model_ = absposeproblem_ptr; + ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0) * 1 / cam_calib.getParam()[0])); + ransac.max_iterations_ = 50; + + ransac.computeModel(); + + T_target_camera = + Sophus::SE3d(ransac.model_coefficients_.topLeftCorner<3, 3>(), + ransac.model_coefficients_.topRightCorner<3, 1>()); + + num_inliers = ransac.inliers_.size(); + + return ransac.inliers_.size() > 8; +} + +void CalibHelper::detectCorners( + const VioDatasetPtr &vio_data, + tbb::concurrent_unordered_map &calib_corners, + tbb::concurrent_unordered_map + &calib_corners_rejected) { + calib_corners.clear(); + calib_corners_rejected.clear(); + + tbb::parallel_for( + tbb::blocked_range(0, vio_data->get_image_timestamps().size()), + [&](const tbb::blocked_range &r) { + ApriltagDetector ad; + + for (size_t j = r.begin(); j != r.end(); ++j) { + int64_t timestamp_ns = vio_data->get_image_timestamps()[j]; + const std::vector &img_vec = + vio_data->get_image_data(timestamp_ns); + + for (size_t i = 0; i < img_vec.size(); i++) { + if (img_vec[i].img.get()) { + CalibCornerData ccd_good; + CalibCornerData ccd_bad; + ad.detectTags(*img_vec[i].img, ccd_good.corners, + ccd_good.corner_ids, ccd_good.radii, + ccd_bad.corners, ccd_bad.corner_ids, ccd_bad.radii); + + // std::cout << "image (" << timestamp_ns << "," + // << i + // << ") detected " << + // ccd_good.corners.size() + // << "corners (" << + // ccd_bad.corners.size() + // << " rejected)" << std::endl; + + TimeCamId tcid = std::make_pair(timestamp_ns, i); + + calib_corners.emplace(tcid, ccd_good); + calib_corners_rejected.emplace(tcid, ccd_bad); + } + } + } + }); +} + +void CalibHelper::initCamPoses( + const Calibration::Ptr &calib, const VioDatasetPtr &vio_data, + const Eigen::vector &aprilgrid_corner_pos_3d, + tbb::concurrent_unordered_map &calib_corners, + tbb::concurrent_unordered_map + &calib_init_poses) { + calib_init_poses.clear(); + + std::vector corners; + corners.reserve(calib_corners.size()); + for (const auto &kv : calib_corners) { + corners.emplace_back(kv.first); + } + + tbb::parallel_for(tbb::blocked_range(0, corners.size()), + [&](const tbb::blocked_range &r) { + for (size_t j = r.begin(); j != r.end(); ++j) { + TimeCamId tcid = corners[j]; + const CalibCornerData &ccd = calib_corners.at(tcid); + + CalibInitPoseData cp; + + computeInitialPose(calib, tcid.second, + aprilgrid_corner_pos_3d, ccd, cp); + + calib_init_poses.emplace(tcid, cp); + } + }); +} + +bool CalibHelper::initializeIntrinsics( + const Eigen::vector &corners, + const std::vector &corner_ids, + const Eigen::vector &aprilgrid_corner_pos_3d, int cols, + int rows, Eigen::Vector4d &init_intr) { + // First, initialize the image center at the center of the image. + + Eigen::map id_to_corner; + for (size_t i = 0; i < corner_ids.size(); i++) { + id_to_corner[corner_ids[i]] = corners[i]; + } + + double _xi = 1.0; + double _cu = cols / 2.0 - 0.5; + double _cv = rows / 2.0 - 0.5; + + /// Initialize some temporaries needed. + double gamma0 = 0.0; + double minReprojErr = std::numeric_limits::max(); + + // Now we try to find a non-radial line to initialize the focal length + const size_t target_cols = 6; + const size_t target_rows = 6; + + bool success = false; + for (int tag_corner_offset = 0; tag_corner_offset < 2; tag_corner_offset++) + for (size_t r = 0; r < target_rows; ++r) { + // cv::Mat P(target.cols(); 4, CV_64F); + + Eigen::vector P; + + for (size_t c = 0; c < target_cols; ++c) { + int tag_offset = (r * target_cols + c) << 2; + + for (int i = 0; i < 2; i++) { + int corner_id = tag_offset + i + tag_corner_offset * 2; + + // std::cerr << corner_id << " "; + + if (id_to_corner.find(corner_id) != id_to_corner.end()) { + const Eigen::Vector2d imagePoint = id_to_corner[corner_id]; + + double u = imagePoint[0] - _cu; + double v = imagePoint[1] - _cv; + + P.emplace_back(u, v, 0.5, -0.5 * (square(u) + square(v))); + } + } + } + + // std::cerr << std::endl; + + const int MIN_CORNERS = 8; + // MIN_CORNERS is an arbitrary threshold for the number of corners + if (P.size() > MIN_CORNERS) { + // Resize P to fit with the count of valid points. + + Eigen::Map P_mat((double *)P.data(), 4, P.size()); + + // std::cerr << "P_mat\n" << P_mat.transpose() << std::endl; + + Eigen::MatrixXd P_mat_t = P_mat.transpose(); + + Eigen::JacobiSVD svd( + P_mat_t, Eigen::ComputeThinU | Eigen::ComputeThinV); + + // std::cerr << "U\n" << svd.matrixU() << std::endl; + // std::cerr << "V\n" << svd.matrixV() << std::endl; + // std::cerr << "singularValues\n" << svd.singularValues() << + // std::endl; + + Eigen::Vector4d C = svd.matrixV().col(3); + // std::cerr << "C\n" << C.transpose() << std::endl; + // std::cerr << "P*res\n" << P_mat.transpose() * C << std::endl; + + double t = square(C(0)) + square(C(1)) + C(2) * C(3); + if (t < 0) { + continue; + } + + // check that line image is not radial + double d = sqrt(1.0 / t); + double nx = C(0) * d; + double ny = C(1) * d; + if (hypot(nx, ny) > 0.95) { + // std::cerr << "hypot(nx, ny) " << hypot(nx, ny) << std::endl; + continue; + } + + double nz = sqrt(1.0 - square(nx) - square(ny)); + double gamma = fabs(C(2) * d / nz); + + Eigen::Matrix calib; + calib << 0.5 * gamma, 0.5 * gamma, _cu, _cv, 0.5 * _xi; + // std::cerr << "gamma " << gamma << std::endl; + + UnifiedCamera cam_calib(calib); + + size_t num_inliers; + Sophus::SE3d T_target_camera; + if (!estimateTransformation(cam_calib, corners, corner_ids, + aprilgrid_corner_pos_3d, T_target_camera, + num_inliers)) { + continue; + } + + double reprojErr = 0.0; + size_t numReprojected = computeReprojectionError( + cam_calib, corners, corner_ids, aprilgrid_corner_pos_3d, + T_target_camera, reprojErr); + + // std::cerr << "numReprojected " << numReprojected << " reprojErr " + // << reprojErr / numReprojected << std::endl; + + if (numReprojected > MIN_CORNERS) { + double avgReprojErr = reprojErr / numReprojected; + + if (avgReprojErr < minReprojErr) { + minReprojErr = avgReprojErr; + gamma0 = gamma; + success = true; + } + } + + } // If this observation has enough valid corners + } // For each row in the image. + + if (success) init_intr << 0.5 * gamma0, 0.5 * gamma0, _cu, _cv; + + return success; +} + +void CalibHelper::computeInitialPose( + const Calibration::Ptr &calib, size_t cam_id, + const Eigen::vector &aprilgrid_corner_pos_3d, + const CalibCornerData &cd, CalibInitPoseData &cp) { + if (cd.corners.size() < 8) { + cp.num_inliers = 0; + return; + } + + bool success; + size_t num_inliers; + + std::visit( + [&](const auto &cam) { + Sophus::SE3d T_target_camera; + success = estimateTransformation(cam, cd.corners, cd.corner_ids, + aprilgrid_corner_pos_3d, cp.T_a_c, + num_inliers); + }, + calib->intrinsics[cam_id].variant); + + if (success) { + Eigen::Matrix4d T_c_a_init = cp.T_a_c.inverse().matrix(); + + std::vector proj_success; + calib->intrinsics[cam_id].project(aprilgrid_corner_pos_3d, T_c_a_init, + cp.reprojected_corners, proj_success); + + cp.num_inliers = num_inliers; + } else { + cp.num_inliers = 0; + } +} + +size_t CalibHelper::computeReprojectionError( + const UnifiedCamera &cam_calib, + const Eigen::vector &corners, + const std::vector &corner_ids, + const Eigen::vector &aprilgrid_corner_pos_3d, + const Sophus::SE3d &T_target_camera, double &error) { + size_t num_projected = 0; + error = 0; + + Eigen::Matrix4d T_camera_target = T_target_camera.inverse().matrix(); + + for (size_t i = 0; i < corners.size(); i++) { + Eigen::Vector4d p_cam = + T_camera_target * aprilgrid_corner_pos_3d[corner_ids[i]]; + Eigen::Vector2d res; + cam_calib.project(p_cam, res); + res -= corners[i]; + + num_projected++; + error += res.norm(); + } + + return num_projected; +} +} // namespace basalt diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp new file mode 100644 index 0000000..d852341 --- /dev/null +++ b/src/calibration/cam_calib.cpp @@ -0,0 +1,798 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include + +#include + +#include + +#include + +#include +namespace fs = std::experimental::filesystem; + +namespace basalt { + +CamCalib::CamCalib(const std::string &dataset_path, + const std::string &dataset_type, + const std::string &cache_path, + const std::string &cache_dataset_name, int skip_images, + const std::vector &cam_types, bool show_gui) + : dataset_path(dataset_path), + dataset_type(dataset_type), + cache_path(ensure_trailing_slash(cache_path)), + cache_dataset_name(cache_dataset_name), + skip_images(skip_images), + cam_types(cam_types), + show_gui(show_gui), + show_frame("ui.show_frame", 0, 0, 1500), + show_corners("ui.show_corners", true, false, true), + show_corners_rejected("ui.show_corners_rejected", false, false, true), + show_init_reproj("ui.show_init_reproj", false, false, true), + show_opt("ui.show_opt", true, false, true), + show_vign("ui.show_vign", false, false, true), + show_ids("ui.show_ids", false, false, true), + huber_thresh("ui.huber_thresh", 4.0, 0.1, 10.0), + opt_intr("ui.opt_intr", true, false, true) { + if (show_gui) initGui(); + + if (!fs::exists(cache_path)) { + fs::create_directory(cache_path); + } +} + +CamCalib::~CamCalib() { + if (processing_thread) { + processing_thread->join(); + } +} + +void CamCalib::initGui() { + pangolin::CreateWindowAndBind("Main", 1600, 1000); + + img_view_display = + &pangolin::CreateDisplay() + .SetBounds(0.5, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View &vign_plot_display = + pangolin::CreateDisplay().SetBounds(0.0, 0.5, 0.7, 1.0); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + vign_plotter.reset(new pangolin::Plotter(&vign_data_log, 0.0, 1000.0, 0.0, + 1.0, 0.01f, 0.01f)); + vign_plot_display.AddDisplay(*vign_plotter); + + pangolin::Var> load_dataset( + "ui.load_dataset", std::bind(&CamCalib::loadDataset, this)); + + pangolin::Var> detect_corners( + "ui.detect_corners", std::bind(&CamCalib::detectCorners, this)); + + pangolin::Var> init_cam_intrinsics( + "ui.init_cam_intr", std::bind(&CamCalib::initCamIntrinsics, this)); + + pangolin::Var> init_cam_poses( + "ui.init_cam_poses", std::bind(&CamCalib::initCamPoses, this)); + + pangolin::Var> init_cam_extrinsics( + "ui.init_cam_extr", std::bind(&CamCalib::initCamExtrinsics, this)); + + pangolin::Var> init_opt( + "ui.init_opt", std::bind(&CamCalib::initOptimization, this)); + + pangolin::Var> optimize( + "ui.optimize", std::bind(&CamCalib::optimize, this)); + + pangolin::Var> save_calib( + "ui.save_calib", std::bind(&CamCalib::saveCalib, this)); + + pangolin::Var> compute_vign( + "ui.compute_vign", std::bind(&CamCalib::computeVign, this)); + + setNumCameras(1); +} + +void CamCalib::computeVign() { + Eigen::vector optical_centers; + for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) { + optical_centers.emplace_back( + calib_opt->calib->intrinsics[i].getParam().segment<2>(2)); + } + + std::map> reprojected_vignette2; + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i]; + const std::vector img_vec = + vio_dataset->get_image_data(timestamp_ns); + + for (size_t j = 0; j < calib_opt->calib->intrinsics.size(); j++) { + TimeCamId tcid(timestamp_ns, j); + + auto it = reprojected_vignette.find(tcid); + + if (it != reprojected_vignette.end() && img_vec[j].img.get()) { + Eigen::vector rv; + rv.resize(it->second.size()); + + for (size_t k = 0; k < it->second.size(); k++) { + Eigen::Vector2d pos = it->second[k]; + + rv[k].head<2>() = pos; + + if (img_vec[j].img->InBounds(pos[0], pos[1], 1)) { + double val = img_vec[j].img->interp(pos); + val /= std::numeric_limits::max(); + rv[k][2] = val; + } else { + // invalid projection + rv[k][2] = -1; + } + } + + reprojected_vignette2.emplace(tcid, rv); + } + } + } + + VignetteEstimator ve(vio_dataset, optical_centers, reprojected_vignette2, + april_grid); + + ve.optimize(); + ve.compute_error(&reprojected_vignette_error); + ve.compute_data_log(vign_data_log); + + { + vign_plotter->ClearSeries(); + vign_plotter->ClearMarkers(); + + for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) { + vign_plotter->AddSeries("$i", "$" + std::to_string(2 * i), + pangolin::DrawingModeLine, + pangolin::Colour::Unspecified(), + "vignette camera " + std::to_string(i)); + } + + vign_plotter->ScaleViewSmooth(vign_data_log.Samples() / 1000.0f, 1.0f, 0.0f, + 0.5f); + } + + ve.save_vign_png(cache_path); + + calib_opt->setVignette(ve.get_vign_param()); +} + +void CamCalib::setNumCameras(size_t n) { + while (img_view.size() < n && show_gui) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display->AddDisplay(*iv); + iv->extern_draw_function = std::bind(&CamCalib::drawImageOverlay, this, + std::placeholders::_1, idx); + } +} + +void CamCalib::renderingLoop() { + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (vio_dataset.get()) { + if (show_frame.GuiChanged()) { + size_t frame_id = static_cast(show_frame); + int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id]; + + const std::vector &img_vec = + vio_dataset->get_image_data(timestamp); + + for (size_t cam_id = 0; cam_id < vio_dataset->get_num_cams(); cam_id++) + if (img_vec[cam_id].img.get()) { + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + img_view[cam_id]->SetImage( + img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, + img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt); + } else { + img_view[cam_id]->Clear(); + } + } + } + + pangolin::FinishFrame(); + } +} + +void CamCalib::computeProjections() { + reprojected_corners.clear(); + reprojected_vignette.clear(); + + if (!calib_opt.get() || !vio_dataset.get()) return; + + for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j]; + + for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) { + TimeCamId tcid = std::make_pair(timestamp_ns, i); + + Eigen::vector rc, rv; + + Sophus::SE3d T_c_w_ = + (calib_opt->getT_w_i(timestamp_ns) * calib_opt->calib->T_i_c[i]) + .inverse(); + + Eigen::Matrix4d T_c_w = T_c_w_.matrix(); + + rc.resize(april_grid.aprilgrid_corner_pos_3d.size()); + rv.resize(april_grid.aprilgrid_vignette_pos_3d.size()); + + std::vector rc_success, rv_success; + + calib_opt->calib->intrinsics[i].project( + april_grid.aprilgrid_corner_pos_3d, T_c_w, rc, rc_success); + + calib_opt->calib->intrinsics[i].project( + april_grid.aprilgrid_vignette_pos_3d, T_c_w, rv, rv_success); + + reprojected_corners.emplace(tcid, rc); + reprojected_vignette.emplace(tcid, rv); + } + } +} + +void CamCalib::detectCorners() { + if (processing_thread) { + processing_thread->join(); + processing_thread.reset(); + } + + processing_thread.reset(new std::thread([this]() { + std::cout << "Started detecting corners" << std::endl; + + CalibHelper::detectCorners(this->vio_dataset, this->calib_corners, + this->calib_corners_rejected); + + std::string path = + cache_path + cache_dataset_name + "_detected_corners.cereal"; + std::ofstream os(path, std::ios::binary); + cereal::BinaryOutputArchive archive(os); + + archive(this->calib_corners); + archive(this->calib_corners_rejected); + + std::cout << "Done detecting corners. Saved them here: " << path + << std::endl; + })); + + if (!show_gui) { + processing_thread->join(); + processing_thread.reset(); + } +} + +void CamCalib::initCamIntrinsics() { + if (calib_corners.empty()) { + std::cerr << "No corners detected. Press detect_corners to start corner " + "detection." + << std::endl; + return; + } + + std::cout << "Started camera intrinsics initialization" << std::endl; + + if (!calib_opt) calib_opt.reset(new PosesOptimization); + + calib_opt->resetCalib(vio_dataset->get_num_cams(), cam_types); + + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) { + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + const int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i]; + const std::vector &img_vec = + vio_dataset->get_image_data(timestamp_ns); + + TimeCamId tcid = std::make_pair(timestamp_ns, j); + + if (calib_corners.find(tcid) != calib_corners.end()) { + CalibCornerData cid = calib_corners.at(tcid); + + bool success = calib_opt->initializeIntrinsics( + j, cid.corners, cid.corner_ids, april_grid.aprilgrid_corner_pos_3d, + img_vec[j].img->w, img_vec[j].img->h); + if (success) break; + } + } + } + + std::cout << "Done camera intrinsics initialization:" << std::endl; + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) { + std::cout << "Cam " << j << ": " + << calib_opt->calib->intrinsics[j].getParam().transpose() + << std::endl; + } + + // set resolution + { + int64_t t_ns = vio_dataset->get_image_timestamps()[1]; + const auto img_data = vio_dataset->get_image_data(t_ns); + + Eigen::vector res; + + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + res.emplace_back(img_data[i].img->w, img_data[i].img->h); + } + + calib_opt->setResolution(res); + } +} + +void CamCalib::initCamPoses() { + if (calib_corners.empty()) { + std::cerr << "No corners detected. Press detect_corners to start corner " + "detection." + << std::endl; + return; + } + + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "No initial intrinsics. Press init_intrinsics initialize " + "intrinsics" + << std::endl; + return; + } + + if (processing_thread) { + processing_thread->join(); + processing_thread.reset(); + } + + std::cout << "Started initial camera pose computation " << std::endl; + + CalibHelper::initCamPoses(calib_opt->calib, this->vio_dataset, + april_grid.aprilgrid_corner_pos_3d, + this->calib_corners, this->calib_init_poses); + + std::string path = cache_path + cache_dataset_name + "_init_poses.cereal"; + std::ofstream os(path, std::ios::binary); + cereal::BinaryOutputArchive archive(os); + + archive(this->calib_init_poses); + + std::cout << "Done initial camera pose computation. Saved them here: " << path + << std::endl; +} + +void CamCalib::initCamExtrinsics() { + if (calib_init_poses.empty()) { + std::cerr << "No initial camera poses. Press init_cam_poses initialize " + "camera poses " + << std::endl; + return; + } + + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "No initial intrinsics. Press init_intrinsics initialize " + "intrinsics" + << std::endl; + return; + } + + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i]; + + TimeCamId tcid0 = std::make_pair(timestamp_ns, 0); + + if (calib_init_poses.find(tcid0) == calib_init_poses.end()) continue; + + Sophus::SE3d T_a_c0 = calib_init_poses.at(tcid0).T_a_c; + + bool success = true; + + for (size_t j = 1; j < vio_dataset->get_num_cams(); j++) { + TimeCamId tcid = std::make_pair(timestamp_ns, j); + + auto cd = calib_init_poses.find(tcid); + if (cd != calib_init_poses.end() && cd->second.num_inliers > 0) { + calib_opt->calib->T_i_c[j] = T_a_c0.inverse() * cd->second.T_a_c; + } else { + success = false; + } + } + + if (success) break; + } + + std::cout << "Done camera extrinsics initialization:" << std::endl; + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) { + std::cout << "T_c0_c" << j << ":\n" + << calib_opt->calib->T_i_c[j].matrix() << std::endl; + } +} + +void CamCalib::initOptimization() { + if (!calib_opt) { + std::cerr << "Calibration is not initialized. Initialize calibration first!" + << std::endl; + return; + } + + if (calib_init_poses.empty()) { + std::cerr << "No initial camera poses. Press init_cam_poses initialize " + "camera poses " + << std::endl; + return; + } + + calib_opt->setAprilgridCorners3d(april_grid.aprilgrid_corner_pos_3d); + + std::set invalid_timestamps; + for (const auto &kv : calib_corners) { + if (kv.second.corner_ids.size() < MIN_CORNERS) + invalid_timestamps.insert(kv.first.first); + } + + for (const auto &kv : calib_corners) { + if (invalid_timestamps.find(kv.first.first) == invalid_timestamps.end()) + calib_opt->addAprilgridMeasurement(kv.first.first, kv.first.second, + kv.second.corners, + kv.second.corner_ids); + } + + for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j]; + + TimeCamId tcid = std::make_pair(timestamp_ns, 0); + const CalibInitPoseData &cp = calib_init_poses.at(tcid); + + calib_opt->addPoseMeasurement( + timestamp_ns, cp.T_a_c * calib_opt->calib->T_i_c[0].inverse()); + } + + calib_opt->init(); + computeProjections(); + + std::cout << "Initialized optimization." << std::endl; +} + +void CamCalib::loadDataset() { + basalt::DatasetIoInterfacePtr dataset_io = + basalt::DatasetIoFactory::getDatasetIo(dataset_type); + + dataset_io->read(dataset_path); + + vio_dataset = dataset_io->get_data(); + setNumCameras(vio_dataset->get_num_cams()); + + if (skip_images > 1) { + std::vector new_image_timestamps; + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + if (i % skip_images == 0) + new_image_timestamps.push_back(vio_dataset->get_image_timestamps()[i]); + } + + vio_dataset->get_image_timestamps() = new_image_timestamps; + } + + // load detected corners if they exist + { + std::string path = + cache_path + cache_dataset_name + "_detected_corners.cereal"; + + std::ifstream is(path, std::ios::binary); + + if (is.good()) { + cereal::BinaryInputArchive archive(is); + + calib_corners.clear(); + calib_corners_rejected.clear(); + archive(calib_corners); + archive(calib_corners_rejected); + + std::cout << "Loaded detected corners from: " << path << std::endl; + } else { + std::cout << "No pre-processed detected corners found" << std::endl; + } + } + + // load initial poses if they exist + { + std::string path = cache_path + cache_dataset_name + "_init_poses.cereal"; + + std::ifstream is(path, std::ios::binary); + + if (is.good()) { + cereal::BinaryInputArchive archive(is); + + calib_init_poses.clear(); + archive(calib_init_poses); + + std::cout << "Loaded initial poses from: " << path << std::endl; + } else { + std::cout << "No pre-processed initial poses found" << std::endl; + } + } + + // load calibration if exist + { + if (!calib_opt) calib_opt.reset(new PosesOptimization); + + calib_opt->loadCalib(cache_path); + } + + reprojected_corners.clear(); + reprojected_vignette.clear(); + + if (show_gui) { + show_frame = 0; + + show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1; + show_frame.Meta().gui_changed = true; + } +} + +void CamCalib::optimize() { optimizeWithParam(true); } + +void CamCalib::optimizeWithParam(bool print_info, + std::map *stats) { + if (calib_init_poses.empty()) { + std::cerr << "No initial camera poses. Press init_cam_poses initialize " + "camera poses " + << std::endl; + return; + } + + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "No initial intrinsics. Press init_intrinsics initialize " + "intrinsics" + << std::endl; + return; + } + + if (calib_opt) { + // calib_opt->compute_projections(); + double error; + double reprojection_error; + int num_points; + + auto start = std::chrono::high_resolution_clock::now(); + + calib_opt->optimize(opt_intr, huber_thresh, error, num_points, + reprojection_error); + + auto finish = std::chrono::high_resolution_clock::now(); + + if (stats) { + stats->clear(); + + stats->emplace("energy_error", error); + stats->emplace("num_points", num_points); + stats->emplace("mean_energy_error", error / num_points); + stats->emplace("reprojection_error", reprojection_error); + stats->emplace("mean_reprojection_error", + reprojection_error / num_points); + } + + if (print_info) { + std::cout << "==================================" << std::endl; + + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + std::cout << "intrinsics " << i << ": " + << calib_opt->calib->intrinsics[i].getParam().transpose() + << std::endl; + std::cout << "T_i_c" << i << ":\n" + << calib_opt->calib->T_i_c[i].matrix() << std::endl; + } + + std::cout << "Current error: " << error << " num_points " << num_points + << " mean_error " << error / num_points + << " reprojection_error " << reprojection_error + << " mean reprojection " << reprojection_error / num_points + << " opt_time " + << std::chrono::duration_cast( + finish - start) + .count() + << "ms." << std::endl; + + std::cout << "==================================" << std::endl; + } + + if (show_gui) { + computeProjections(); + } + } +} + +void CamCalib::saveCalib() { + if (calib_opt) { + calib_opt->saveCalib(cache_path, vio_dataset->get_mocap_to_imu_offset_ns()); + + std::cout << "Saved calibration in " << cache_path << "/calibration.json" + << std::endl; + } +} + +void CamCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) { + UNUSED(v); + + size_t frame_id = show_frame; + + if (vio_dataset && frame_id < vio_dataset->get_image_timestamps().size()) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[frame_id]; + TimeCamId tcid = std::make_pair(timestamp_ns, cam_id); + + if (show_corners) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (calib_corners.find(tcid) != calib_corners.end()) { + const CalibCornerData &cr = calib_corners.at(tcid); + const CalibCornerData &cr_rej = calib_corners_rejected.at(tcid); + + for (size_t i = 0; i < cr.corners.size(); i++) { + // the radius is the threshold used for maximum displacement. The + // search region is slightly larger. + const float radius = static_cast(cr.radii[i]); + const Eigen::Vector2f c = cr.corners[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", cr.corner_ids[i]).Draw(c[0], c[1]); + } + + pangolin::GlFont::I() + .Text("Detected %d corners (%d rejected)", cr.corners.size(), + cr_rej.corners.size()) + .Draw(5, 50); + + if (show_corners_rejected) { + glColor3f(1.0, 0.5, 0.0); + + for (size_t i = 0; i < cr_rej.corners.size(); i++) { + // the radius is the threshold used for maximum displacement. The + // search region is slightly larger. + const float radius = static_cast(cr_rej.radii[i]); + const Eigen::Vector2f c = cr_rej.corners[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I() + .Text("%d", cr_rej.corner_ids[i]) + .Draw(c[0], c[1]); + } + } + + } else { + glLineWidth(1.0); + + pangolin::GlFont::I().Text("Corners not processed").Draw(5, 50); + } + } + + if (show_init_reproj) { + glLineWidth(1.0); + glColor3f(1.0, 1.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (calib_init_poses.find(tcid) != calib_init_poses.end()) { + const CalibInitPoseData &cr = calib_init_poses.at(tcid); + + for (size_t i = 0; i < cr.reprojected_corners.size(); i++) { + Eigen::Vector2d c = cr.reprojected_corners[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + if (show_ids) pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + + pangolin::GlFont::I() + .Text("Initial pose with %d inliers", cr.num_inliers) + .Draw(5, 100); + + } else { + pangolin::GlFont::I().Text("Initial pose not processed").Draw(5, 100); + } + } + + if (show_opt) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 1.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (reprojected_corners.find(tcid) != reprojected_corners.end()) { + if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { + const auto &rc = reprojected_corners.at(tcid); + + for (size_t i = 0; i < rc.size(); i++) { + Eigen::Vector2d c = rc[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + if (show_ids) pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + } else { + pangolin::GlFont::I().Text("Too few corners detected.").Draw(5, 150); + } + } + } + + if (show_vign) { + glLineWidth(1.0); + glColor3f(1.0, 1.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (reprojected_vignette.find(tcid) != reprojected_vignette.end()) { + if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { + const auto &rc = reprojected_vignette.at(tcid); + + bool has_errors = false; + auto it = reprojected_vignette_error.find(tcid); + if (it != reprojected_vignette_error.end()) has_errors = true; + + for (size_t i = 0; i < rc.size(); i++) { + Eigen::Vector2d c = rc[i].head<2>(); + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + if (show_ids) { + if (has_errors) { + pangolin::GlFont::I() + .Text("%d(%f)", i, it->second[i]) + .Draw(c[0], c[1]); + } else { + pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + } + } + } else { + pangolin::GlFont::I().Text("Too few corners detected.").Draw(5, 200); + } + } + } + } +} + +bool CamCalib::hasCorners() const { return !calib_corners.empty(); } + +} // namespace basalt diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp new file mode 100644 index 0000000..1aa36ad --- /dev/null +++ b/src/calibration/cam_imu_calib.cpp @@ -0,0 +1,1054 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include + +#include + +#include + +namespace basalt { + +CamImuCalib::CamImuCalib(const std::string &dataset_path, + const std::string &dataset_type, + const std::string &cache_path, + const std::string &cache_dataset_name, int skip_images, + const std::vector &imu_noise, bool show_gui) + : dataset_path(dataset_path), + dataset_type(dataset_type), + cache_path(ensure_trailing_slash(cache_path)), + cache_dataset_name(cache_dataset_name), + skip_images(skip_images), + show_gui(show_gui), + imu_noise(imu_noise), + show_frame("ui.show_frame", 0, 0, 1500), + show_corners("ui.show_corners", true, false, true), + show_corners_rejected("ui.show_corners_rejected", false, false, true), + show_init_reproj("ui.show_init_reproj", false, false, true), + show_opt("ui.show_opt", true, false, true), + show_ids("ui.show_ids", false, false, true), + show_accel("ui.show_accel", true, false, true), + show_gyro("ui.show_gyro", false, false, true), + show_pos("ui.show_pos", false, false, true), + show_rot_error("ui.show_rot_error", false, false, true), + show_mocap("ui.show_mocap", false, false, true), + show_mocap_rot_error("ui.show_mocap_rot_error", false, false, true), + show_mocap_rot_vel("ui.show_mocap_rot_vel", false, false, true), + show_spline("ui.show_spline", true, false, true), + show_data("ui.show_data", true, false, true), + opt_intr("ui.opt_intr", false, false, true), + opt_poses("ui.opt_poses", false, false, true), + opt_corners("ui.opt_corners", true, false, true), + opt_cam_time_offset("ui.opt_cam_time_offset", false, false, true), + opt_imu_scale("ui.opt_imu_scale", false, false, true), + opt_mocap("ui.opt_mocap", false, false, true), + huber_thresh("ui.huber_thresh", 4.0, 0.1, 10.0) { + if (show_gui) initGui(); +} + +CamImuCalib::~CamImuCalib() { + if (processing_thread) { + processing_thread->join(); + } +} + +void CamImuCalib::initGui() { + pangolin::CreateWindowAndBind("Main", 1600, 1000); + + img_view_display = + &pangolin::CreateDisplay() + .SetBounds(0.5, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View &plot_display = pangolin::CreateDisplay().SetBounds( + 0.0, 0.5, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + plotter = new pangolin::Plotter(&imu_data_log, 0.0, 100.0, -10.0, 10.0, 0.01f, + 0.01f); + plot_display.AddDisplay(*plotter); + + pangolin::Var> load_dataset( + "ui.load_dataset", std::bind(&CamImuCalib::loadDataset, this)); + + pangolin::Var> detect_corners( + "ui.detect_corners", std::bind(&CamImuCalib::detectCorners, this)); + + pangolin::Var> init_cam_poses( + "ui.init_cam_poses", std::bind(&CamImuCalib::initCamPoses, this)); + + pangolin::Var> init_cam_imu( + "ui.init_cam_imu", std::bind(&CamImuCalib::initCamImuTransform, this)); + + pangolin::Var> init_opt( + "ui.init_opt", std::bind(&CamImuCalib::initOptimization, this)); + + pangolin::Var> optimize( + "ui.optimize", std::bind(&CamImuCalib::optimize, this)); + + pangolin::Var> init_mocap( + "ui.init_mocap", std::bind(&CamImuCalib::initMocap, this)); + + pangolin::Var> save_calib( + "ui.save_calib", std::bind(&CamImuCalib::saveCalib, this)); + + pangolin::Var> save_mocap_calib( + "ui.save_mocap_calib", std::bind(&CamImuCalib::saveMocapCalib, this)); + + setNumCameras(1); +} + +void CamImuCalib::setNumCameras(size_t n) { + while (img_view.size() < n && show_gui) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display->AddDisplay(*iv); + iv->extern_draw_function = std::bind(&CamImuCalib::drawImageOverlay, this, + std::placeholders::_1, idx); + } +} + +void CamImuCalib::renderingLoop() { + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (vio_dataset.get()) { + if (show_frame.GuiChanged()) { + size_t frame_id = static_cast(show_frame); + int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id]; + + const std::vector &img_vec = + vio_dataset->get_image_data(timestamp); + + for (size_t cam_id = 0; cam_id < vio_dataset->get_num_cams(); cam_id++) + if (img_vec[cam_id].img.get()) { + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + img_view[cam_id]->SetImage( + img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, + img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt); + } else { + img_view[cam_id]->Clear(); + } + drawPlots(); + } + + if (show_accel.GuiChanged() || show_gyro.GuiChanged() || + show_data.GuiChanged() || show_spline.GuiChanged() || + show_pos.GuiChanged() || show_rot_error.GuiChanged() || + show_mocap.GuiChanged() || show_mocap_rot_error.GuiChanged() || + show_mocap_rot_vel.GuiChanged()) { + drawPlots(); + } + } + + pangolin::FinishFrame(); + } +} + +void CamImuCalib::computeProjections() { + reprojected_corners.clear(); + + if (!calib_opt.get() || !vio_dataset.get()) return; + + for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j]; + + int64_t timestamp_corrected_ns = + timestamp_ns + calib_opt->getCamTimeOffsetNs(); + + if (timestamp_corrected_ns < calib_opt->getMinTimeNs() || + timestamp_corrected_ns >= calib_opt->getMaxTimeNs()) + continue; + + for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) { + TimeCamId tcid = std::make_pair(timestamp_ns, i); + + Eigen::vector rc; + + Sophus::SE3d T_c_w_ = (calib_opt->getT_w_i(timestamp_corrected_ns) * + calib_opt->getCamT_i_c(i)) + .inverse(); + + Eigen::Matrix4d T_c_w = T_c_w_.matrix(); + + rc.resize(april_grid.aprilgrid_corner_pos_3d.size()); + + std::vector proj_success; + calib_opt->calib->intrinsics[i].project( + april_grid.aprilgrid_corner_pos_3d, T_c_w, rc, proj_success); + + reprojected_corners.emplace(tcid, rc); + } + } +} + +void CamImuCalib::detectCorners() { + if (processing_thread) { + processing_thread->join(); + processing_thread.reset(); + } + + processing_thread.reset(new std::thread([this]() { + std::cout << "Started detecting corners" << std::endl; + + CalibHelper::detectCorners(this->vio_dataset, this->calib_corners, + this->calib_corners_rejected); + + std::string path = + cache_path + cache_dataset_name + "_detected_corners.cereal"; + std::ofstream os(path, std::ios::binary); + cereal::BinaryOutputArchive archive(os); + + archive(this->calib_corners); + archive(this->calib_corners_rejected); + + std::cout << "Done detecting corners. Saved them here: " << path + << std::endl; + })); + + if (!show_gui) { + processing_thread->join(); + processing_thread.reset(); + } +} + +void CamImuCalib::initCamPoses() { + if (calib_corners.empty()) { + std::cerr << "No corners detected. Press detect_corners to start corner " + "detection." + << std::endl; + return; + } + + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "No camera intrinsics. Calibrate camera using " + "basalt_calibrate first!" + << std::endl; + return; + } + + if (processing_thread) { + processing_thread->join(); + processing_thread.reset(); + } + { + std::cout << "Started initial camera pose computation " << std::endl; + + CalibHelper::initCamPoses(calib_opt->calib, this->vio_dataset, + april_grid.aprilgrid_corner_pos_3d, + this->calib_corners, this->calib_init_poses); + + std::string path = cache_path + cache_dataset_name + "_init_poses.cereal"; + std::ofstream os(path, std::ios::binary); + cereal::BinaryOutputArchive archive(os); + + archive(this->calib_init_poses); + + std::cout << "Done initial camera pose computation. Saved them here: " + << path << std::endl; + }; +} + +void CamImuCalib::initCamImuTransform() { + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "No camera intrinsics. Calibrate camera using " + "basalt_calibrate first!" + << std::endl; + return; + } + + if (calib_init_poses.empty()) { + std::cerr << "Initialize camera poses first!" << std::endl; + return; + } + + std::vector timestamps_cam; + Eigen::vector rot_vel_cam; + Eigen::vector rot_vel_imu; + + Sophus::SO3d R_i_c0_init = calib_opt->getCamT_i_c(0).so3(); + + for (size_t i = 1; i < vio_dataset->get_image_timestamps().size(); i++) { + int64_t timestamp0_ns = vio_dataset->get_image_timestamps()[i - 1]; + int64_t timestamp1_ns = vio_dataset->get_image_timestamps()[i]; + + TimeCamId tcid0 = std::make_pair(timestamp0_ns, 0); + TimeCamId tcid1 = std::make_pair(timestamp1_ns, 0); + + if (calib_init_poses.find(tcid0) == calib_init_poses.end()) continue; + if (calib_init_poses.find(tcid1) == calib_init_poses.end()) continue; + + Sophus::SE3d T_a_c0 = calib_init_poses.at(tcid0).T_a_c; + Sophus::SE3d T_a_c1 = calib_init_poses.at(tcid1).T_a_c; + + double dt = (timestamp1_ns - timestamp0_ns) * 1e-9; + + Eigen::Vector3d rot_vel_c0 = + R_i_c0_init * (T_a_c0.so3().inverse() * T_a_c1.so3()).log() / dt; + + timestamps_cam.push_back(timestamp0_ns); + rot_vel_cam.push_back(rot_vel_c0); + } + + for (size_t j = 0; j < timestamps_cam.size(); j++) { + int idx = -1; + int64_t min_dist = std::numeric_limits::max(); + + for (size_t i = 1; i < vio_dataset->get_gyro_data().size(); i++) { + int64_t dist = + vio_dataset->get_gyro_data()[i].timestamp_ns - timestamps_cam[j]; + if (std::abs(dist) < min_dist) { + min_dist = std::abs(dist); + idx = i; + } + } + + rot_vel_imu.push_back(vio_dataset->get_gyro_data()[idx].data); + } + + BASALT_ASSERT_STREAM(rot_vel_cam.size() == rot_vel_imu.size(), + "rot_vel_cam.size() " << rot_vel_cam.size() + << " rot_vel_imu.size() " + << rot_vel_imu.size()); + + // R_i_c * rot_vel_cam = rot_vel_imu + // R_i_c * rot_vel_cam * rot_vel_cam.T = rot_vel_imu * rot_vel_cam.T + // R_i_c = rot_vel_imu * rot_vel_cam.T * (rot_vel_cam * rot_vel_cam.T)^-1; + + Eigen::Matrix rot_vel_cam_m(3, rot_vel_cam.size()), + rot_vel_imu_m(3, rot_vel_imu.size()); + + for (size_t i = 0; i < rot_vel_cam.size(); i++) { + rot_vel_cam_m.col(i) = rot_vel_cam[i]; + rot_vel_imu_m.col(i) = rot_vel_imu[i]; + } + + Eigen::Matrix3d R_i_c0 = + rot_vel_imu_m * rot_vel_cam_m.transpose() * + (rot_vel_cam_m * rot_vel_cam_m.transpose()).inverse(); + + // std::cout << "raw R_i_c0\n" << R_i_c0 << std::endl; + + Eigen::AngleAxisd aa(R_i_c0); // RotationMatrix to AxisAngle + R_i_c0 = aa.toRotationMatrix(); + + // std::cout << "R_i_c0\n" << R_i_c0 << std::endl; + + Sophus::SE3d T_i_c0(R_i_c0, Eigen::Vector3d::Zero()); + + std::cout << "T_i_c0\n" << T_i_c0.matrix() << std::endl; + + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + calib_opt->getCamT_i_c(i) = T_i_c0 * calib_opt->getCamT_i_c(i); + } + + std::cout << "Done Camera-IMU extrinsics initialization:" << std::endl; + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) { + std::cout << "T_i_c" << j << ":\n" + << calib_opt->getCamT_i_c(j).matrix() << std::endl; + } +} + +void CamImuCalib::initOptimization() { + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "No camera intrinsics. Calibrate camera using " + "basalt_calibrate first!" + << std::endl; + return; + } + + calib_opt->setAprilgridCorners3d(april_grid.aprilgrid_corner_pos_3d); + + for (size_t i = 0; i < vio_dataset->get_accel_data().size(); i++) { + const basalt::AccelData &ad = vio_dataset->get_accel_data()[i]; + const basalt::GyroData &gd = vio_dataset->get_gyro_data()[i]; + + calib_opt->addAccelMeasurement(ad.timestamp_ns, ad.data); + calib_opt->addGyroMeasurement(gd.timestamp_ns, gd.data); + } + + std::set invalid_timestamps; + for (const auto &kv : calib_corners) { + if (kv.second.corner_ids.size() < MIN_CORNERS) + invalid_timestamps.insert(kv.first.first); + } + + for (const auto &kv : calib_corners) { + if (invalid_timestamps.find(kv.first.first) == invalid_timestamps.end()) + calib_opt->addAprilgridMeasurement(kv.first.first, kv.first.second, + kv.second.corners, + kv.second.corner_ids); + } + + for (size_t i = 0; i < vio_dataset->get_gt_timestamps().size(); i++) { + calib_opt->addMocapMeasurement(vio_dataset->get_gt_timestamps()[i], + vio_dataset->get_gt_pose_data()[i]); + } + + bool g_initialized = false; + Eigen::Vector3d g_a_init; + + for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j]; + + TimeCamId tcid = std::make_pair(timestamp_ns, 0); + const CalibInitPoseData &cp = calib_init_poses.at(tcid); + + calib_opt->addPoseMeasurement( + timestamp_ns, cp.T_a_c * calib_opt->getCamT_i_c(0).inverse()); + + if (!g_initialized) { + for (size_t i = 0; + i < vio_dataset->get_accel_data().size() && !g_initialized; i++) { + const basalt::AccelData &ad = vio_dataset->get_accel_data()[i]; + if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) { + g_a_init = cp.T_a_c.so3() * ad.data; + g_initialized = true; + std::cout << "g_a initialized with " << g_a_init.transpose() + << std::endl; + } + } + } + } + + const int num_samples = 100; + double dt = 0.0; + for (int i = 0; i < num_samples; i++) { + dt += 1e-9 * (vio_dataset->get_gyro_data()[i + 1].timestamp_ns - + vio_dataset->get_gyro_data()[i].timestamp_ns); + } + dt /= num_samples; + + std::cout << "IMU dt: " << dt << " freq: " << 1.0 / dt << std::endl; + + calib_opt->calib->imu_update_rate = 1.0 / dt; + + calib_opt->setG(g_a_init); + calib_opt->init(); + computeProjections(); + recomputeDataLog(); + + std::cout << "Initialized optimization." << std::endl; +} + +void CamImuCalib::initMocap() { + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "Initalize optimization first!" << std::endl; + return; + } + + Sophus::SE3d T_w_moc; + + // TODO: check for failure cases.. + for (size_t i = vio_dataset->get_gt_timestamps().size() / 2; + i < vio_dataset->get_gt_timestamps().size(); i++) { + int64_t timestamp_ns = vio_dataset->get_gt_timestamps()[i]; + T_w_moc = calib_opt->getT_w_i(timestamp_ns) * + vio_dataset->get_gt_pose_data()[i].inverse(); + + std::cout << "Initialized T_w_moc:\n" << T_w_moc.matrix() << std::endl; + break; + } + + calib_opt->setT_w_moc(T_w_moc); + recomputeDataLog(); +} + +void CamImuCalib::loadDataset() { + basalt::DatasetIoInterfacePtr dataset_io = + basalt::DatasetIoFactory::getDatasetIo(dataset_type); + + dataset_io->read(dataset_path); + + vio_dataset = dataset_io->get_data(); + setNumCameras(vio_dataset->get_num_cams()); + + if (skip_images > 1) { + std::vector new_image_timestamps; + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + if (i % skip_images == 0) + new_image_timestamps.push_back(vio_dataset->get_image_timestamps()[i]); + } + + vio_dataset->get_image_timestamps() = new_image_timestamps; + } + + // load detected corners if they exist + { + std::string path = + cache_path + cache_dataset_name + "_detected_corners.cereal"; + + std::ifstream is(path, std::ios::binary); + + if (is.good()) { + cereal::BinaryInputArchive archive(is); + + calib_corners.clear(); + calib_corners_rejected.clear(); + archive(calib_corners); + archive(calib_corners_rejected); + + std::cout << "Loaded detected corners from: " << path << std::endl; + } else { + std::cout << "No pre-processed detected corners found" << std::endl; + } + } + + // load initial poses if they exist + { + std::string path = cache_path + cache_dataset_name + "_init_poses.cereal"; + + std::ifstream is(path, std::ios::binary); + + if (is.good()) { + cereal::BinaryInputArchive archive(is); + + calib_init_poses.clear(); + archive(calib_init_poses); + + std::cout << "Loaded initial poses from: " << path << std::endl; + } else { + std::cout << "No pre-processed initial poses found" << std::endl; + } + } + + // load calibration if exist + { + if (!calib_opt) calib_opt.reset(new SplineOptimization<5, double>); + + calib_opt->loadCalib(cache_path); + + calib_opt->calib->accel_noise_std = imu_noise[0]; + calib_opt->calib->gyro_noise_std = imu_noise[1]; + calib_opt->calib->accel_bias_std = imu_noise[2]; + calib_opt->calib->gyro_bias_std = imu_noise[3]; + } + calib_opt->resetMocapCalib(); + + reprojected_corners.clear(); + + if (show_gui) { + show_frame = 0; + + show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1; + show_frame.Meta().gui_changed = true; + + plotter->ClearSeries(); + recomputeDataLog(); + drawPlots(); + } +} + +void CamImuCalib::optimize() { optimizeWithParam(true); } + +void CamImuCalib::optimizeWithParam(bool print_info, + std::map *stats) { + if (!calib_opt.get() || !calib_opt->calibInitialized()) { + std::cerr << "Initalize optimization first!" << std::endl; + return; + } + + if (calib_opt) { + // calib_opt->compute_projections(); + double error; + double reprojection_error; + int num_points; + + auto start = std::chrono::high_resolution_clock::now(); + + calib_opt->optimize(opt_intr, opt_poses, opt_corners, opt_cam_time_offset, + opt_imu_scale, opt_mocap, huber_thresh, error, + num_points, reprojection_error); + + auto finish = std::chrono::high_resolution_clock::now(); + + if (stats) { + stats->clear(); + + stats->emplace("energy_error", error); + stats->emplace("num_points", num_points); + stats->emplace("mean_energy_error", error / num_points); + stats->emplace("reprojection_error", reprojection_error); + stats->emplace("mean_reprojection_error", + reprojection_error / num_points); + } + + if (print_info) { + std::cout << "==================================" << std::endl; + + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + std::cout << "intrinsics " << i << ": " + << calib_opt->getIntrinsics(i).transpose() << std::endl; + std::cout << "T_i_c" << i << ":\n" + << calib_opt->getCamT_i_c(i).matrix() << std::endl; + } + + std::cout << "T_w_moc:\n" + << calib_opt->getT_w_moc().matrix() << std::endl; + + std::cout << "T_mark_i:\n" + << calib_opt->getT_mark_i().matrix() << std::endl; + + std::cout << "cam_time_offset_ns: " << calib_opt->getCamTimeOffsetNs() + << std::endl; + + std::cout << "mocap_time_offset_ns: " << calib_opt->getMocapTimeOffsetNs() + << std::endl; + { + Eigen::Vector3d accel_bias; + Eigen::Matrix3d accel_scale; + calib_opt->getAccelBias().getBiasAndScale(accel_bias, accel_scale); + + std::cout << "accel_bias: " << accel_bias.transpose() + << "\naccel_scale:\n" + << Eigen::Matrix3d::Identity() + accel_scale << std::endl; + + Eigen::Vector3d gyro_bias; + Eigen::Matrix3d gyro_scale; + calib_opt->getGyroBias().getBiasAndScale(gyro_bias, gyro_scale); + + std::cout << "gyro_bias: " << gyro_bias.transpose() << "\ngyro_scale:\n" + << Eigen::Matrix3d::Identity() + gyro_scale << std::endl; + } + + std::cout << " g " << calib_opt->getG().transpose() + << " norm: " << calib_opt->getG().norm() << " g_mocap: " + << (calib_opt->getT_w_moc().inverse().so3() * calib_opt->getG()) + .transpose() + << std::endl; + + std::cout << "Current error: " << error << " num_points " << num_points + << " mean_error " << error / num_points + << " reprojection_error " << reprojection_error + << " mean reprojection " << reprojection_error / num_points + << " opt_time " + << std::chrono::duration_cast( + finish - start) + .count() + << "ms." << std::endl; + + std::cout << "==================================" << std::endl; + } + + // calib_opt->compute_error(error, num_points); + // std::cerr << "after opt error: " << error << " num_points " << + // num_points << std::endl; + + if (show_gui) { + computeProjections(); + recomputeDataLog(); + drawPlots(); + } + } +} + +void CamImuCalib::saveCalib() { + if (calib_opt) { + calib_opt->saveCalib(cache_path); + + std::cout << "Saved calibration in " << cache_path << "/calibration.json" + << std::endl; + } +} + +void CamImuCalib::saveMocapCalib() { + if (calib_opt) { + calib_opt->saveMocapCalib(cache_path, + vio_dataset->get_mocap_to_imu_offset_ns()); + + std::cout << "Saved Mocap calibration in " << cache_path + << "/mocap_calibration.json" << std::endl; + } +} + +void CamImuCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) { + UNUSED(v); + + size_t frame_id = show_frame; + + if (vio_dataset && frame_id < vio_dataset->get_image_timestamps().size()) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[frame_id]; + TimeCamId tcid = std::make_pair(timestamp_ns, cam_id); + + if (show_corners) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (calib_corners.find(tcid) != calib_corners.end()) { + const CalibCornerData &cr = calib_corners.at(tcid); + const CalibCornerData &cr_rej = calib_corners_rejected.at(tcid); + + for (size_t i = 0; i < cr.corners.size(); i++) { + // the radius is the threshold used for maximum displacement. The + // search region is slightly larger. + const float radius = static_cast(cr.radii[i]); + const Eigen::Vector2f c = cr.corners[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", cr.corner_ids[i]).Draw(c[0], c[1]); + } + + pangolin::GlFont::I() + .Text("Detected %d corners (%d rejected)", cr.corners.size(), + cr_rej.corners.size()) + .Draw(5, 50); + + if (show_corners_rejected) { + glColor3f(1.0, 0.5, 0.0); + + for (size_t i = 0; i < cr_rej.corners.size(); i++) { + // the radius is the threshold used for maximum displacement. The + // search region is slightly larger. + const float radius = static_cast(cr_rej.radii[i]); + const Eigen::Vector2f c = cr_rej.corners[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I() + .Text("%d", cr_rej.corner_ids[i]) + .Draw(c[0], c[1]); + } + } + + } else { + glLineWidth(1.0); + + pangolin::GlFont::I().Text("Corners not processed").Draw(5, 50); + } + } + + if (show_init_reproj) { + glLineWidth(1.0); + glColor3f(1.0, 1.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (calib_init_poses.find(tcid) != calib_init_poses.end()) { + const CalibInitPoseData &cr = calib_init_poses.at(tcid); + + for (size_t i = 0; i < cr.reprojected_corners.size(); i++) { + Eigen::Vector2d c = cr.reprojected_corners[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + if (show_ids) pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + + pangolin::GlFont::I() + .Text("Initial pose with %d inliers", cr.num_inliers) + .Draw(5, 100); + + } else { + pangolin::GlFont::I().Text("Initial pose not processed").Draw(5, 100); + } + } + + if (show_opt) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 1.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (reprojected_corners.find(tcid) != reprojected_corners.end()) { + if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { + const auto &rc = reprojected_corners.at(tcid); + + for (size_t i = 0; i < rc.size(); i++) { + Eigen::Vector2d c = rc[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + if (show_ids) pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + } else { + pangolin::GlFont::I().Text("Too few corners detected.").Draw(5, 150); + } + } + } + } +} + +void CamImuCalib::recomputeDataLog() { + imu_data_log.Clear(); + pose_data_log.Clear(); + mocap_data_log.Clear(); + + if (!vio_dataset || vio_dataset->get_accel_data().empty()) return; + + double min_time = vio_dataset->get_accel_data()[0].timestamp_ns * 1e-9; + + for (size_t i = 0; i < vio_dataset->get_accel_data().size(); i++) { + const basalt::AccelData &ad = vio_dataset->get_accel_data()[i]; + const basalt::GyroData &gd = vio_dataset->get_gyro_data()[i]; + + Eigen::Vector3d a_sp(0, 0, 0), g_sp(0, 0, 0); + + if (calib_opt && calib_opt->calibInitialized() && + calib_opt->initialized()) { + Sophus::SE3d pose_sp = calib_opt->getT_w_i(ad.timestamp_ns); + Eigen::Vector3d a_sp_w = calib_opt->getTransAccelWorld(ad.timestamp_ns); + + a_sp = calib_opt->getAccelBias().invertCalibration( + pose_sp.so3().inverse() * (a_sp_w + calib_opt->getG())); + + g_sp = calib_opt->getGyroBias().invertCalibration( + calib_opt->getRotVelBody(ad.timestamp_ns)); + } + + std::vector vals; + double t = ad.timestamp_ns * 1e-9 - min_time; + vals.push_back(t); + + for (int k = 0; k < 3; k++) vals.push_back(ad.data[k]); + for (int k = 0; k < 3; k++) vals.push_back(a_sp[k]); + for (int k = 0; k < 3; k++) vals.push_back(gd.data[k]); + for (int k = 0; k < 3; k++) vals.push_back(g_sp[k]); + + imu_data_log.Log(vals); + } + + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i]; + + TimeCamId tcid = std::make_pair(timestamp_ns, 0); + const auto &it = calib_init_poses.find(tcid); + + double t = timestamp_ns * 1e-9 - min_time; + + Sophus::SE3d pose_sp, pose_meas; + if (calib_opt && calib_opt->initialized()) + pose_sp = calib_opt->getT_w_i(timestamp_ns); + + if (it != calib_init_poses.end() && calib_opt && + calib_opt->calibInitialized()) + pose_meas = it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse(); + + Eigen::Vector3d p_sp = pose_sp.translation(); + Eigen::Vector3d p_meas = pose_meas.translation(); + + double angle = + pose_sp.unit_quaternion().angularDistance(pose_meas.unit_quaternion()) * + 180 / M_PI; + + pose_data_log.Log(t, p_meas[0], p_meas[1], p_meas[2], p_sp[0], p_sp[1], + p_sp[2], angle); + } + + for (size_t i = 0; i < vio_dataset->get_gt_timestamps().size(); i++) { + int64_t timestamp_ns = vio_dataset->get_gt_timestamps()[i]; + + if (calib_opt && calib_opt->calibInitialized()) + timestamp_ns += calib_opt->getMocapTimeOffsetNs(); + + double t = timestamp_ns * 1e-9 - min_time; + + Sophus::SE3d pose_sp, pose_mocap; + if (calib_opt && calib_opt->calibInitialized()) { + if (timestamp_ns < calib_opt->getMinTimeNs() || + timestamp_ns > calib_opt->getMaxTimeNs()) + continue; + + pose_sp = calib_opt->getT_w_i(timestamp_ns); + pose_mocap = calib_opt->getT_w_moc() * + vio_dataset->get_gt_pose_data()[i] * + calib_opt->getT_mark_i(); + } + + Eigen::Vector3d p_sp = pose_sp.translation(); + Eigen::Vector3d p_mocap = pose_mocap.translation(); + + double angle = pose_sp.unit_quaternion().angularDistance( + pose_mocap.unit_quaternion()) * + 180 / M_PI; + + Eigen::Vector3d rot_vel(0, 0, 0); + if (i > 0) { + double dt = (vio_dataset->get_gt_timestamps()[i] - + vio_dataset->get_gt_timestamps()[i - 1]) * + 1e-9; + + rot_vel = (vio_dataset->get_gt_pose_data()[i - 1].so3().inverse() * + vio_dataset->get_gt_pose_data()[i].so3()) + .log() / + dt; + } + + std::vector valsd = {t, p_mocap[0], p_mocap[1], p_mocap[2], + p_sp[0], p_sp[1], p_sp[2], angle, + rot_vel[0], rot_vel[1], rot_vel[2]}; + + std::vector vals(valsd.begin(), valsd.end()); + + mocap_data_log.Log(vals); + } +} + +void CamImuCalib::drawPlots() { + plotter->ClearSeries(); + plotter->ClearMarkers(); + + if (show_accel) { + if (show_data) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "a x"); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "a y"); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "a z"); + } + + if (show_spline) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "a x Spline"); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "a y Spline"); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "a z Spline"); + } + } + + if (show_gyro) { + if (show_data) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "g x"); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "g y"); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "g z"); + } + + if (show_spline) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "g x Spline"); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "g y Spline"); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "g z Spline"); + } + } + + if (show_pos) { + if (show_data) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "p x", &pose_data_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "p y", &pose_data_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "p z", &pose_data_log); + } + + if (show_spline) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "p x Spline", &pose_data_log); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "p y Spline", + &pose_data_log); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "p z Spline", + &pose_data_log); + } + } + + if (show_rot_error) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine, + pangolin::Colour::White(), "rot error", &pose_data_log); + } + + if (show_mocap) { + if (show_data) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "p x", &mocap_data_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "p y", &mocap_data_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "p z", &mocap_data_log); + } + + if (show_spline) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "p x Spline", + &mocap_data_log); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "p y Spline", + &mocap_data_log); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "p z Spline", + &mocap_data_log); + } + } + + if (show_mocap_rot_error) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine, + pangolin::Colour::White(), "rot error", &mocap_data_log); + } + + if (show_mocap_rot_vel) { + plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine, + pangolin::Colour(1, 1, 0), "rot vel x", &mocap_data_log); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine, + pangolin::Colour(1, 0, 1), "rot vel y", &mocap_data_log); + plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine, + pangolin::Colour(0, 1, 1), "rot vel z", &mocap_data_log); + } + + size_t frame_id = show_frame; + double min_time = vio_dataset->get_accel_data().empty() + ? vio_dataset->get_image_timestamps()[0] * 1e-9 + : vio_dataset->get_accel_data()[0].timestamp_ns * 1e-9; + + int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id]; + if (calib_opt && calib_opt->calibInitialized()) + timestamp += calib_opt->getCamTimeOffsetNs(); + + double t = timestamp * 1e-9 - min_time; + plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal, + pangolin::Colour::White()); +} + +bool CamImuCalib::hasCorners() const { return !calib_corners.empty(); } + +} // namespace basalt diff --git a/src/calibration/vignette.cpp b/src/calibration/vignette.cpp new file mode 100644 index 0000000..0a8619a --- /dev/null +++ b/src/calibration/vignette.cpp @@ -0,0 +1,306 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include + +namespace basalt { + +VignetteEstimator::VignetteEstimator( + const VioDatasetPtr &vio_dataset, + const Eigen::vector &optical_centers, + const std::map> + &reprojected_vignette, + const AprilGrid &april_grid) + : vio_dataset(vio_dataset), + optical_centers(optical_centers), + reprojected_vignette(reprojected_vignette), + april_grid(april_grid), + vign_param(vio_dataset->get_num_cams(), + RdSpline<1, SPLINE_N>(knot_spacing)) { + vign_size = 0; + + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + Eigen::Vector2d oc = optical_centers[i]; + + size_t new_size = oc.norm() * 1.1; + vign_size = std::max(vign_size, new_size); + } + + // std::cerr << vign_size << std::endl; + + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + while (vign_param[i].maxTimeNs() < + int64_t(vign_size) * int64_t(1e9 * 0.7)) { + vign_param[i].knots_push_back(Eigen::Matrix(1)); + } + + while (vign_param[i].maxTimeNs() < int64_t(vign_size) * int64_t(1e9)) { + vign_param[i].knots_push_back(Eigen::Matrix(0.01)); + } + } + + irradiance.resize(april_grid.aprilgrid_vignette_pos_3d.size()); + std::fill(irradiance.begin(), irradiance.end(), 1.0); +} + +void VignetteEstimator::compute_error( + std::map> *reprojected_vignette_error) { + double error = 0; + double mean_residual = 0; + double max_residual = 0; + int num_residuals = 0; + + TimeCamId tcid_max; + // int point_id = 0; + + if (reprojected_vignette_error) reprojected_vignette_error->clear(); + + for (const auto &kv : reprojected_vignette) { + const TimeCamId &tcid = kv.first; + const auto &points_2d_val = kv.second; + + Eigen::Vector2d oc = optical_centers[tcid.second]; + + BASALT_ASSERT(points_2d_val.size() == + april_grid.aprilgrid_vignette_pos_3d.size()); + + std::vector ve(april_grid.aprilgrid_vignette_pos_3d.size()); + + for (size_t i = 0; i < points_2d_val.size(); i++) { + if (points_2d_val[i][2] >= 0) { + double val = points_2d_val[i][2]; + int64_t loc = + (points_2d_val[i].head<2>() - oc).norm() * 1e9; // in pixels * 1e9 + double e = + irradiance[i] * vign_param[tcid.second].evaluate(loc)[0] - val; + ve[i] = e; + error += e * e; + mean_residual += std::abs(e); + max_residual = std::max(max_residual, std::abs(e)); + if (max_residual == std::abs(e)) { + tcid_max = tcid; + // point_id = i; + } + num_residuals++; + } + } + + if (reprojected_vignette_error) + reprojected_vignette_error->emplace(tcid, ve); + } + + // std::cerr << "error " << error << std::endl; + // std::cerr << "mean_residual " << mean_residual / num_residuals << + // std::endl; + // std::cerr << "max_residual " << max_residual << std::endl; + + // int frame_id = 0; + // for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + // if (tcid_max.first == vio_dataset->get_image_timestamps()[i]) { + // frame_id = i; + // } + // } + + // std::cerr << "tcid_max " << frame_id << " " << tcid_max.second << " point + // id " + // << point_id << std::endl + // << std::endl; +} + +void VignetteEstimator::opt_irradience() { + std::vector new_irradiance(irradiance.size(), 0); + std::vector new_irradiance_count(irradiance.size(), 0); + + for (const auto &kv : reprojected_vignette) { + const TimeCamId &tcid = kv.first; + const auto &points_2d_val = kv.second; + + Eigen::Vector2d oc = optical_centers[tcid.second]; + + BASALT_ASSERT(points_2d_val.size() == + april_grid.aprilgrid_vignette_pos_3d.size()); + + for (size_t i = 0; i < points_2d_val.size(); i++) { + if (points_2d_val[i][2] >= 0) { + double val = points_2d_val[i][2]; + int64_t loc = + (points_2d_val[i].head<2>() - oc).norm() * 1e9; // in pixels * 1e9 + + new_irradiance[i] += val / vign_param[tcid.second].evaluate(loc)[0]; + new_irradiance_count[i] += 1; + } + } + } + + for (size_t i = 0; i < irradiance.size(); i++) { + if (new_irradiance_count[i] > 0) + irradiance[i] = new_irradiance[i] / new_irradiance_count[i]; + } +} + +void VignetteEstimator::opt_vign() { + size_t num_knots = vign_param[0].getKnots().size(); + + std::vector> new_vign_param( + vio_dataset->get_num_cams(), std::vector(num_knots, 0)); + std::vector> new_vign_param_count( + vio_dataset->get_num_cams(), std::vector(num_knots, 0)); + + for (const auto &kv : reprojected_vignette) { + const TimeCamId &tcid = kv.first; + const auto &points_2d_val = kv.second; + + // Sophus::SE3d T_w_cam = + // calib_opt->getT_w_i(tcid.first) * + // calib_opt->getCamT_i_c(tcid.second); + // Eigen::Vector3d opt_axis_w = T_w_cam.so3() * + // Eigen::Vector3d::UnitZ(); + // if (-opt_axis_w[2] < angle_threshold) continue; + + Eigen::Vector2d oc = optical_centers[tcid.second]; + + BASALT_ASSERT(points_2d_val.size() == + april_grid.aprilgrid_vignette_pos_3d.size()); + + for (size_t i = 0; i < points_2d_val.size(); i++) { + if (points_2d_val[i][2] >= 0) { + double val = points_2d_val[i][2]; + int64_t loc = (points_2d_val[i].head<2>() - oc).norm() * 1e9; + + RdSpline<1, SPLINE_N>::JacobianStruct J; + vign_param[tcid.second].evaluate(loc, &J); + + for (size_t k = 0; k < J.d_val_d_knot.size(); k++) { + new_vign_param[tcid.second][J.start_idx + k] += + J.d_val_d_knot[k] * val / irradiance[i]; + new_vign_param_count[tcid.second][J.start_idx + k] += + J.d_val_d_knot[k]; + } + } + } + } + + double max_val = 0; + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) + for (size_t i = 0; i < num_knots; i++) { + if (new_vign_param_count[j][i] > 0) { + // std::cerr << "update " << i << " " << j << std::endl; + double val = new_vign_param[j][i] / new_vign_param_count[j][i]; + max_val = std::max(max_val, val); + vign_param[j].getKnot(i)[0] = val; + } + } + + // normalize vignette + double max_val_inv = 1.0 / max_val; + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) + for (size_t i = 0; i < num_knots; i++) { + if (new_vign_param_count[j][i] > 0) { + vign_param[j].getKnot(i)[0] *= max_val_inv; + } + } +} + +void VignetteEstimator::optimize() { + compute_error(); + for (int i = 0; i < 10; i++) { + opt_irradience(); + compute_error(); + opt_vign(); + compute_error(); + } +} + +void VignetteEstimator::compute_data_log(pangolin::DataLog &vign_data_log) { + std::vector> num_proj_points( + 2, std::vector(vign_size, 0)); + + for (const auto &kv : reprojected_vignette) { + const TimeCamId &tcid = kv.first; + const auto &points_2d = kv.second; + + Eigen::Vector2d oc = optical_centers[tcid.second]; + + BASALT_ASSERT(points_2d.size() == + april_grid.aprilgrid_vignette_pos_3d.size()); + + for (size_t i = 0; i < points_2d.size(); i++) { + if (points_2d[i][2] >= 0) { + size_t loc = (points_2d[i].head<2>() - oc).norm(); + num_proj_points[tcid.second][loc] += 1.; + } + } + } + + vign_data_log.Clear(); + for (size_t i = 0; i < vign_size; i++) { + std::vector log_data; + for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) { + int64_t loc = i * 1e9; + log_data.push_back(vign_param[j].evaluate(loc)[0]); + log_data.push_back(num_proj_points[j][i]); + } + vign_data_log.Log(log_data); + } +} + +void VignetteEstimator::save_vign_png(const std::string &path) { + for (size_t k = 0; k < vio_dataset->get_num_cams(); k++) { + auto img = + vio_dataset->get_image_data(vio_dataset->get_image_timestamps()[0])[0]; + pangolin::ManagedImage vign_img(img.img->w, img.img->h); + vign_img.Fill(0); + + Eigen::Vector2d oc = optical_centers[k]; + + for (size_t x = 0; x < vign_img.w; x++) { + for (size_t y = 0; y < vign_img.h; y++) { + int64_t loc = (Eigen::Vector2d(x, y) - oc).norm() * 1e9; + double val = vign_param[k].evaluate(loc)[0]; + if (val < 0.5) continue; + uint16_t val_int = + val >= 1.0 ? std::numeric_limits::max() + : uint16_t(val * std::numeric_limits::max()); + vign_img(x, y) = val_int; + } + } + + pangolin::SaveImage(vign_img.UnsafeReinterpret(), + pangolin::PixelFormatFromString("GRAY16LE"), + path + "/vingette_" + std::to_string(k) + ".png"); + } +} +} // namespace basalt diff --git a/src/io/dataset_io.cpp b/src/io/dataset_io.cpp new file mode 100644 index 0000000..ac1a044 --- /dev/null +++ b/src/io/dataset_io.cpp @@ -0,0 +1,57 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include +#include +#include + +namespace basalt { + +DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo( + const std::string &dataset_type, bool with_images) { + if (dataset_type == "euroc") { + // return DatasetIoInterfacePtr(); + return DatasetIoInterfacePtr(new EurocIO); + } else if (dataset_type == "bag") { + return DatasetIoInterfacePtr(new RosbagIO(with_images)); + } else { + std::cerr << "Dataset type " << dataset_type << " is not supported" + << std::endl; + std::abort(); + } +} + +} // namespace basalt diff --git a/src/io/marg_data_io.cpp b/src/io/marg_data_io.cpp new file mode 100644 index 0000000..91a410c --- /dev/null +++ b/src/io/marg_data_io.cpp @@ -0,0 +1,228 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include + +#include + +#include + +#include +namespace fs = std::experimental::filesystem; + +namespace basalt { + +MargDataSaver::MargDataSaver(const std::string& path) { + fs::remove_all(path); + fs::create_directory(path); + + save_image_queue.set_capacity(300); + + std::string img_path = path + "/images/"; + fs::create_directory(img_path); + + in_marg_queue.set_capacity(1000); + + auto save_func = [&, path]() { + basalt::MargData::Ptr data; + + std::unordered_set processed_opt_flow; + + while (true) { + in_marg_queue.pop(data); + + if (data.get()) { + int64_t kf_id = *data->kfs_to_marg.begin(); + + std::string p = path + "/" + std::to_string(kf_id) + ".cereal"; + std::ofstream os(p, std::ios::binary); + + { + cereal::BinaryOutputArchive archive(os); + archive(*data); + } + os.close(); + + for (const auto& d : data->opt_flow_res) { + if (processed_opt_flow.count(d->t_ns) == 0) { + save_image_queue.push(d); + processed_opt_flow.emplace(d->t_ns); + } + } + + } else { + save_image_queue.push(nullptr); + break; + } + } + + std::cout << "Finished MargDataSaver" << std::endl; + }; + + auto save_image_func = [&, img_path]() { + basalt::OpticalFlowResult::Ptr data; + + while (true) { + save_image_queue.pop(data); + + if (data.get()) { + std::string p = img_path + "/" + std::to_string(data->t_ns) + ".cereal"; + std::ofstream os(p, std::ios::binary); + + { + cereal::BinaryOutputArchive archive(os); + archive(data); + } + os.close(); + } else { + break; + } + } + + std::cout << "Finished image MargDataSaver" << std::endl; + }; + + saving_thread.reset(new std::thread(save_func)); + saving_img_thread.reset(new std::thread(save_image_func)); +} // namespace basalt + +MargDataLoader::MargDataLoader() : out_marg_queue(nullptr) {} + +void MargDataLoader::start(const std::string& path) { + auto func = [&, path]() { + std::string img_path = path + "/images/"; + + std::unordered_set saved_images; + + std::map opt_flow_res; + + for (const auto& entry : fs::directory_iterator(img_path)) { + OpticalFlowResult::Ptr data; + // std::cout << entry.path() << std::endl; + std::ifstream is(entry.path(), std::ios::binary); + { + cereal::BinaryInputArchive archive(is); + archive(data); + } + is.close(); + opt_flow_res[data->t_ns] = data; + } + + std::map filenames; + + for (auto& p : fs::directory_iterator(path)) { + std::string filename = p.path().filename(); + if (!std::isdigit(filename[0])) continue; + + size_t lastindex = filename.find_last_of("."); + std::string rawname = filename.substr(0, lastindex); + + int64_t t_ns = std::stol(rawname); + + filenames.emplace(t_ns, filename); + } + + for (const auto& kv : filenames) { + basalt::MargData::Ptr data(new basalt::MargData); + + std::string p = path + "/" + kv.second; + std::ifstream is(p, std::ios::binary); + + { + cereal::BinaryInputArchive archive(is); + archive(*data); + } + is.close(); + + for (const auto& d : data->kfs_all) { + data->opt_flow_res.emplace_back(opt_flow_res.at(d)); + } + + out_marg_queue->push(data); + } + + out_marg_queue->push(nullptr); + + std::cout << "Finished MargDataLoader" << std::endl; + }; + + processing_thread.reset(new std::thread(func)); +} +} // namespace basalt + +namespace cereal { + +template +void save(Archive& ar, const basalt::ManagedImage& m) { + ar(m.w); + ar(m.h); + ar(cereal::binary_data(m.ptr, sizeof(T) * m.w * m.h)); +} + +template +void load(Archive& ar, basalt::ManagedImage& m) { + size_t w; + size_t h; + ar(w); + ar(h); + m.Reinitialise(w, h); + ar(cereal::binary_data(m.ptr, sizeof(T) * m.w * m.h)); +} + +template +void serialize(Archive& ar, basalt::OpticalFlowResult& m) { + ar(m.t_ns); + ar(m.observations); + ar(m.input_images); +} + +template +void serialize(Archive& ar, basalt::OpticalFlowInput& m) { + ar(m.t_ns); + ar(m.img_data); +} + +template +void serialize(Archive& ar, basalt::ImageData& m) { + ar(m.exposure); + ar(m.img); +} + +template +void serialize(Archive& ar, Eigen::AffineCompact2f& m) { + ar(m.matrix()); +} +} // namespace cereal diff --git a/src/mapper.cpp b/src/mapper.cpp new file mode 100644 index 0000000..428245a --- /dev/null +++ b/src/mapper.cpp @@ -0,0 +1,647 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace fs = std::experimental::filesystem; + +using basalt::POSE_SIZE; +using basalt::POSE_VEL_BIAS_SIZE; + +Eigen::Vector3d g(0, 0, -9.81); + +const Eigen::vector image_resolutions = {{752, 480}, + {752, 480}}; + +basalt::VioConfig vio_config; +basalt::NfrMapper::Ptr nrf_mapper; + +Eigen::vector gt_frame_t_w_i; +std::vector gt_frame_t_ns, image_t_ns; + +Eigen::vector mapper_points; +std::vector mapper_point_ids; + +std::map marg_data; + +Eigen::vector edges_vis; +Eigen::vector roll_pitch_vis; +Eigen::vector rel_edges_vis; + +void draw_image_overlay(pangolin::View& v, size_t cam_id); +void draw_scene(); +void load_data(const std::string& calib_path, + const std::string& marg_data_path); +void processMargData(basalt::MargData& m); +void extractNonlinearFactors(basalt::MargData& m); +void computeEdgeVis(); +void optimize(); +void randomInc(); +void randomYawInc(); +void compute_error(); +double alignButton(); +void detect(); +void match(); +void tracks(); +void optimize(); + +constexpr int UI_WIDTH = 200; + +basalt::Calibration calib; + +pangolin::Var show_frame1("ui.show_frame1", 0, 0, 1); +pangolin::Var show_cam1("ui.show_cam1", 0, 0, 0); +pangolin::Var show_frame2("ui.show_frame2", 0, 0, 1); +pangolin::Var show_cam2("ui.show_cam2", 0, 0, 0); +pangolin::Var lock_frames("ui.lock_frames", true, false, true); +pangolin::Var show_detected("ui.show_detected", true, false, true); +pangolin::Var show_matches("ui.show_matches", true, false, true); +pangolin::Var show_inliers("ui.show_inliers", true, false, true); +pangolin::Var show_ids("ui.show_ids", false, false, true); + +pangolin::Var num_opt_iter("ui.num_opt_iter", 10, 0, 20); + +pangolin::Var show_gt("ui.show_gt", true, false, true); +pangolin::Var show_edges("ui.show_edges", true, false, true); +pangolin::Var show_points("ui.show_points", true, false, true); + +using Button = pangolin::Var>; + +Button detect_btn("ui.detect", &detect); +Button match_btn("ui.match", &match); +Button tracks_btn("ui.tracks", &tracks); +Button optimize_btn("ui.optimize", &optimize); +Button align_btn("ui.aling_svd", &alignButton); + +pangolin::OpenGlRenderState camera; + +std::string marg_data_path; +std::string vocabulary; + +int main(int argc, char** argv) { + bool show_gui = true; + std::string cam_calib_path; + std::string result_path; + std::string config_path; + + CLI::App app{"App description"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, + "Ground-truth camera calibration used for simulation.") + ->required(); + + app.add_option("--marg-data", marg_data_path, "Path to cache folder.") + ->required(); + + app.add_option("--config-path", config_path, "Path to config file."); + + app.add_option("--vocabulary", vocabulary, "Path to vocabulary.")->required(); + + app.add_option("--result-path", result_path, "Path to config file."); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError& e) { + return app.exit(e); + } + + if (!config_path.empty()) { + vio_config.load(config_path); + } + + load_data(cam_calib_path, marg_data_path); + + for (auto& kv : marg_data) { + nrf_mapper->addMargData(kv.second); + } + + computeEdgeVis(); + + { + std::cout << "Loaded " << nrf_mapper->img_data.size() << " images." + << std::endl; + + show_frame1.Meta().range[1] = nrf_mapper->img_data.size() - 1; + show_frame2.Meta().range[1] = nrf_mapper->img_data.size() - 1; + show_frame1.Meta().gui_changed = true; + show_frame2.Meta().gui_changed = true; + + show_cam1.Meta().range[1] = calib.intrinsics.size() - 1; + show_cam2.Meta().range[1] = calib.intrinsics.size() - 1; + if (calib.intrinsics.size() > 1) show_cam2 = 1; + + for (const auto& kv : nrf_mapper->img_data) { + image_t_ns.emplace_back(kv.first); + } + + std::sort(image_t_ns.begin(), image_t_ns.end()); + } + + if (show_gui) { + pangolin::CreateWindowAndBind("Main", 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::View& img_view_display = + pangolin::CreateDisplay() + .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.4) + .SetLayout(pangolin::LayoutEqual); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < calib.intrinsics.size()) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display.AddDisplay(*iv); + iv->extern_draw_function = + std::bind(&draw_image_overlay, std::placeholders::_1, idx); + } + + camera = pangolin::OpenGlRenderState( + pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), + pangolin::ModelViewLookAt(-3.4, -3.7, -8.3, 2.1, 0.6, 0.2, + pangolin::AxisNegY)); + + // pangolin::OpenGlRenderState camera( + // pangolin::ProjectionMatrixOrthographic(-30, 30, -30, 30, -30, 30), + // pangolin::ModelViewLookAt(-3.4, -3.7, -8.3, 2.1, 0.6, 0.2, + // pangolin::AxisNegY)); + + pangolin::View& display3D = + pangolin::CreateDisplay() + .SetAspect(-640 / 480.0) + .SetBounds(0.0, 1.0, 0.4, 1.0) + .SetHandler(new pangolin::Handler3D(camera)); + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (lock_frames) { + // in case of locking frames, chaning one should change the other + if (show_frame1.GuiChanged()) { + show_frame2 = show_frame1; + show_frame2.Meta().gui_changed = true; + show_frame1.Meta().gui_changed = true; + } else if (show_frame2.GuiChanged()) { + show_frame1 = show_frame2; + show_frame1.Meta().gui_changed = true; + show_frame2.Meta().gui_changed = true; + } + } + + display3D.Activate(camera); + glClearColor(1.f, 1.f, 1.f, 1.0f); + + draw_scene(); + + if (show_frame1.GuiChanged() || show_cam1.GuiChanged()) { + size_t frame_id = static_cast(show_frame1); + int64_t timestamp = image_t_ns[frame_id]; + size_t cam_id = show_cam1; + + if (nrf_mapper->img_data.count(timestamp) > 0 && + nrf_mapper->img_data.at(timestamp).get()) { + const std::vector& img_vec = + nrf_mapper->img_data.at(timestamp)->img_data; + + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + if (img_vec[cam_id].img.get()) { + img_view[0]->SetImage( + img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, + img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt); + } else { + img_view[0]->Clear(); + } + } else { + img_view[0]->Clear(); + } + } + + if (show_frame2.GuiChanged() || show_cam2.GuiChanged()) { + size_t frame_id = static_cast(show_frame2); + int64_t timestamp = image_t_ns[frame_id]; + size_t cam_id = show_cam2; + + if (nrf_mapper->img_data.count(timestamp) > 0 && + nrf_mapper->img_data.at(timestamp).get()) { + const std::vector& img_vec = + nrf_mapper->img_data.at(timestamp)->img_data; + + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + if (img_vec[cam_id].img.get()) { + img_view[1]->SetImage( + img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, + img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt); + } else { + img_view[1]->Clear(); + } + } else { + img_view[1]->Clear(); + } + } + + pangolin::FinishFrame(); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } else { + optimize(); + detect(); + match(); + tracks(); + optimize(); + + if (!result_path.empty()) { + double error = alignButton(); + + std::ofstream os(result_path); + os << error << std::endl; + os.close(); + } + } + + return 0; +} + +void draw_image_overlay(pangolin::View& v, size_t view_id) { + size_t frame_id = (view_id == 0) ? show_frame1 : show_frame2; + size_t cam_id = (view_id == 0) ? show_cam1 : show_cam2; + + basalt::TimeCamId tcid = std::make_pair(image_t_ns[frame_id], cam_id); + + float text_row = 20; + + if (show_detected) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); // red + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (nrf_mapper->feature_corners.find(tcid) != + nrf_mapper->feature_corners.end()) { + const basalt::KeypointsData& cr = nrf_mapper->feature_corners.at(tcid); + + for (size_t i = 0; i < cr.corners.size(); i++) { + Eigen::Vector2d c = cr.corners[i]; + double angle = cr.corner_angles[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + Eigen::Vector2d r(3, 0); + Eigen::Rotation2Dd rot(angle); + r = rot * r; + + pangolin::glDrawLine(c, c + r); + } + + pangolin::GlFont::I() + .Text("Detected %d corners", cr.corners.size()) + .Draw(5, 20); + + } else { + glLineWidth(1.0); + + pangolin::GlFont::I().Text("Corners not processed").Draw(5, text_row); + } + text_row += 20; + } + + if (show_matches || show_inliers) { + glLineWidth(1.0); + glColor3f(0.0, 0.0, 1.0); // blue + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + size_t o_frame_id = (view_id == 0 ? show_frame2 : show_frame1); + size_t o_cam_id = (view_id == 0 ? show_cam2 : show_cam1); + + basalt::TimeCamId o_tcid = std::make_pair(image_t_ns[o_frame_id], o_cam_id); + + int idx = -1; + + auto it = nrf_mapper->feature_matches.find(std::make_pair(tcid, o_tcid)); + + if (it != nrf_mapper->feature_matches.end()) { + idx = 0; + } else { + it = nrf_mapper->feature_matches.find(std::make_pair(o_tcid, tcid)); + if (it != nrf_mapper->feature_matches.end()) { + idx = 1; + } + } + + if (idx >= 0 && show_matches) { + if (nrf_mapper->feature_corners.find(tcid) != + nrf_mapper->feature_corners.end()) { + const basalt::KeypointsData& cr = nrf_mapper->feature_corners.at(tcid); + + for (size_t i = 0; i < it->second.matches.size(); i++) { + size_t c_idx = idx == 0 ? it->second.matches[i].first + : it->second.matches[i].second; + + Eigen::Vector2d c = cr.corners[c_idx]; + double angle = cr.corner_angles[c_idx]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + Eigen::Vector2d r(3, 0); + Eigen::Rotation2Dd rot(angle); + r = rot * r; + + pangolin::glDrawLine(c, c + r); + + if (show_ids) { + pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + } + + pangolin::GlFont::I() + .Text("Detected %d matches", it->second.matches.size()) + .Draw(5, text_row); + text_row += 20; + } + } + + glColor3f(0.0, 1.0, 0.0); // green + + if (idx >= 0 && show_inliers) { + if (nrf_mapper->feature_corners.find(tcid) != + nrf_mapper->feature_corners.end()) { + const basalt::KeypointsData& cr = nrf_mapper->feature_corners.at(tcid); + + for (size_t i = 0; i < it->second.inliers.size(); i++) { + size_t c_idx = idx == 0 ? it->second.inliers[i].first + : it->second.inliers[i].second; + + Eigen::Vector2d c = cr.corners[c_idx]; + double angle = cr.corner_angles[c_idx]; + pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0); + + Eigen::Vector2d r(3, 0); + Eigen::Rotation2Dd rot(angle); + r = rot * r; + + pangolin::glDrawLine(c, c + r); + + if (show_ids) { + pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]); + } + } + + pangolin::GlFont::I() + .Text("Detected %d inliers", it->second.inliers.size()) + .Draw(5, text_row); + text_row += 20; + } + } + } +} + +void draw_scene() { + glPointSize(3); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glColor3ubv(pose_color); + if (show_points) pangolin::glDrawPoints(mapper_points); + + glColor3ubv(gt_color); + if (show_gt) pangolin::glDrawLineStrip(gt_frame_t_w_i); + + glColor3f(0.0, 1.0, 0.0); + if (show_edges) pangolin::glDrawLines(edges_vis); + + glLineWidth(2); + glColor3f(1.0, 0.0, 1.0); + if (show_edges) pangolin::glDrawLines(roll_pitch_vis); + glLineWidth(1); + + glColor3f(1.0, 0.0, 0.0); + if (show_edges) pangolin::glDrawLines(rel_edges_vis); + + for (const auto& kv : nrf_mapper->getFramePoses()) { + pangolin::glDrawAxis(kv.second.getPose().matrix(), 0.1); + } + + pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); +} + +void load_data(const std::string& calib_path, const std::string& cache_path) { + { + std::ifstream os(calib_path, std::ios::binary); + + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + std::cout << "Loaded camera with " << calib.intrinsics.size() + << " cameras" << std::endl; + + } else { + std::cerr << "could not load camera calibration " << calib_path + << std::endl; + std::abort(); + } + } + + { + // Load gt. + { + std::string p = cache_path + "/gt.cereal"; + std::ifstream is(p, std::ios::binary); + + { + cereal::BinaryInputArchive archive(is); + archive(gt_frame_t_ns); + archive(gt_frame_t_w_i); + } + is.close(); + std::cout << "Loaded " << gt_frame_t_ns.size() << " timestamps and " + << gt_frame_t_w_i.size() << " poses" << std::endl; + } + } + + nrf_mapper.reset(new basalt::NfrMapper(calib, vio_config, vocabulary)); + + basalt::MargDataLoader mdl; + tbb::concurrent_bounded_queue marg_queue; + mdl.out_marg_queue = &marg_queue; + + mdl.start(cache_path); + + while (true) { + basalt::MargData::Ptr data; + marg_queue.pop(data); + + if (data.get()) { + int64_t t_ns = *data->kfs_to_marg.begin(); + marg_data[t_ns] = data; + + } else { + break; + } + } + + std::cout << "Loaded " << marg_data.size() << " marg data." << std::endl; +} + +void computeEdgeVis() { + edges_vis.clear(); + for (const auto& kv1 : nrf_mapper->obs) { + for (const auto& kv2 : kv1.second) { + Eigen::Vector3d p1 = nrf_mapper->getFramePoses() + .at(kv1.first.first) + .getPose() + .translation(); + Eigen::Vector3d p2 = nrf_mapper->getFramePoses() + .at(kv2.first.first) + .getPose() + .translation(); + + edges_vis.emplace_back(p1); + edges_vis.emplace_back(p2); + } + } + + roll_pitch_vis.clear(); + for (const auto& v : nrf_mapper->roll_pitch_factors) { + const Sophus::SE3d& T_w_i = + nrf_mapper->getFramePoses().at(v.t_ns).getPose(); + + Eigen::Vector3d p = T_w_i.translation(); + Eigen::Vector3d d = + v.R_w_i_meas * T_w_i.so3().inverse() * (-Eigen::Vector3d::UnitZ()); + + roll_pitch_vis.emplace_back(p); + roll_pitch_vis.emplace_back(p + 0.1 * d); + } + + rel_edges_vis.clear(); + for (const auto& v : nrf_mapper->rel_pose_factors) { + Eigen::Vector3d p1 = + nrf_mapper->getFramePoses().at(v.t_i_ns).getPose().translation(); + Eigen::Vector3d p2 = + nrf_mapper->getFramePoses().at(v.t_j_ns).getPose().translation(); + + rel_edges_vis.emplace_back(p1); + rel_edges_vis.emplace_back(p2); + } +} + +void optimize() { + nrf_mapper->optimize(num_opt_iter); + nrf_mapper->get_current_points(mapper_points, mapper_point_ids); + + computeEdgeVis(); +} + +double alignButton() { + Eigen::vector filter_t_w_i; + std::vector filter_t_ns; + + for (const auto& kv : nrf_mapper->getFramePoses()) { + filter_t_ns.emplace_back(kv.first); + filter_t_w_i.emplace_back(kv.second.getPose().translation()); + } + + return basalt::alignSVD(filter_t_ns, filter_t_w_i, gt_frame_t_ns, + gt_frame_t_w_i); +} + +void detect() { + nrf_mapper->feature_corners.clear(); + nrf_mapper->feature_matches.clear(); + nrf_mapper->detect_keypoints(); +} + +void match() { + nrf_mapper->feature_matches.clear(); + nrf_mapper->match_stereo(); + nrf_mapper->match_all(); +} + +void tracks() { + nrf_mapper->build_tracks(); + nrf_mapper->setup_opt(); + nrf_mapper->get_current_points(mapper_points, mapper_point_ids); + // nrf_mapper->get_current_points_with_color(mapper_points, + // mapper_points_color, + // mapper_point_ids); + computeEdgeVis(); +} diff --git a/src/mapper_sim.cpp b/src/mapper_sim.cpp new file mode 100644 index 0000000..860a99f --- /dev/null +++ b/src/mapper_sim.cpp @@ -0,0 +1,595 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace fs = std::experimental::filesystem; + +using basalt::POSE_SIZE; +using basalt::POSE_VEL_BIAS_SIZE; + +Eigen::Vector3d g(0, 0, -9.81); + +std::shared_ptr> gt_spline; + +Eigen::vector gt_frame_T_w_i; +Eigen::vector gt_frame_t_w_i, vio_t_w_i; +std::vector gt_frame_t_ns; + +Eigen::vector gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, + noisy_accel, noisy_gyro, gt_vel; + +std::vector gt_imu_t_ns; + +Eigen::vector filter_points; +std::vector filter_point_ids; + +std::map marg_data; + +Eigen::vector roll_pitch_factors; +Eigen::vector rel_pose_factors; + +Eigen::vector edges_vis; +Eigen::vector roll_pitch_vis; +Eigen::vector rel_edges_vis; + +Eigen::vector mapper_points; +std::vector mapper_point_ids; + +basalt::NfrMapper::Ptr nrf_mapper; + +std::map gt_observations; +std::map noisy_observations; + +void draw_scene(); +void load_data(const std::string& calib_path, + const std::string& marg_data_path); +void processMargData(basalt::MargData& m); +void extractNonlinearFactors(basalt::MargData& m); +void computeEdgeVis(); +void optimize(); +void randomInc(); +void randomYawInc(); +double alignButton(); +void setup_points(); + +constexpr int UI_WIDTH = 200; +constexpr int NUM_FRAMES = 500; + +basalt::Calibration calib; + +pangolin::Var show_edges("ui.show_edges", true, false, true); +pangolin::Var show_points("ui.show_points", true, false, true); + +using Button = pangolin::Var>; + +Button optimize_btn("ui.optimize", &optimize); +Button rand_inc_btn("ui.rand_inc", &randomInc); +Button rand_yaw_inc_btn("ui.rand_yaw", &randomYawInc); +Button setup_points_btn("ui.setup_points", &setup_points); +Button align_svd_btn("ui.align_svd", &alignButton); + +std::string marg_data_path; + +int main(int argc, char** argv) { + bool show_gui = true; + std::string cam_calib_path; + + CLI::App app{"App description"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, + "Ground-truth camera calibration used for simulation.") + ->required(); + + app.add_option("--marg-data", marg_data_path, "Path to cache folder.") + ->required(); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError& e) { + return app.exit(e); + } + + load_data(cam_calib_path, marg_data_path); + + basalt::VioConfig config; + + nrf_mapper.reset(new basalt::NfrMapper(calib, config)); + + for (auto& kv : marg_data) { + nrf_mapper->addMargData(kv.second); + } + + computeEdgeVis(); + + std::cout << "roll_pitch_factors.size() " << roll_pitch_factors.size() + << std::endl; + std::cout << "rel_pose_factors.size() " << rel_pose_factors.size() + << std::endl; + + if (show_gui) { + pangolin::CreateWindowAndBind("Main", 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + pangolin::OpenGlRenderState camera( + pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), + pangolin::ModelViewLookAt(-8.4, -8.7, -8.3, 2.1, 0.6, 0.2, + pangolin::AxisNegY)); + + pangolin::View& display3D = + pangolin::CreateDisplay() + .SetAspect(-640 / 480.0) + .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) + .SetHandler(new pangolin::Handler3D(camera)); + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + display3D.Activate(camera); + glClearColor(1.f, 1.f, 1.f, 1.0f); + + draw_scene(); + + pangolin::FinishFrame(); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + + return 0; +} + +void draw_scene() { + glPointSize(3); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glColor3ubv(pose_color); + if (show_points) pangolin::glDrawPoints(mapper_points); + + glColor3ubv(gt_color); + pangolin::glDrawLineStrip(gt_frame_t_w_i); + + glColor3ubv(cam_color); + pangolin::glDrawLineStrip(vio_t_w_i); + + glColor3f(0.0, 1.0, 0.0); + if (show_edges) pangolin::glDrawLines(edges_vis); + + glColor3f(1.0, 0.0, 1.0); + if (show_edges) pangolin::glDrawLines(roll_pitch_vis); + + glColor3f(1.0, 0.0, 0.0); + if (show_edges) pangolin::glDrawLines(rel_edges_vis); + + for (const auto& kv : nrf_mapper->getFramePoses()) { + pangolin::glDrawAxis(kv.second.getPose().matrix(), 0.1); + } + + pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); +} + +void load_data(const std::string& calib_path, const std::string& cache_path) { + { + std::ifstream os(calib_path, std::ios::binary); + + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + std::cout << "Loaded camera with " << calib.intrinsics.size() + << " cameras" << std::endl; + + } else { + std::cerr << "could not load camera calibration " << calib_path + << std::endl; + std::abort(); + } + } + + { + std::string path = cache_path + "/gt_spline.cereal"; + std::cout << "path " << path << std::endl; + + std::ifstream is(path, std::ios::binary); + + if (is.is_open()) { + cereal::JSONInputArchive archive(is); + + int64_t t_ns; + Eigen::vector knots; + + archive(cereal::make_nvp("t_ns", t_ns)); + archive(cereal::make_nvp("knots", knots)); + + archive(cereal::make_nvp("gt_observations", gt_observations)); + archive(cereal::make_nvp("noisy_observations", noisy_observations)); + + std::cout << "path " << path << std::endl; + std::cout << "t_ns " << t_ns << std::endl; + std::cout << "knots " << knots.size() << std::endl; + + gt_spline.reset(new basalt::Se3Spline<5>(t_ns)); + + for (size_t i = 0; i < knots.size(); i++) { + gt_spline->knots_push_back(knots[i]); + } + + is.close(); + } else { + std::cerr << "could not open " << path << std::endl; + std::abort(); + } + } + + { + int64_t dt_ns = int64_t(1e9) / 50; + + for (int64_t t_ns = 0; t_ns < gt_spline->maxTimeNs(); t_ns += dt_ns) { + gt_frame_t_w_i.emplace_back(gt_spline->pose(t_ns).translation()); + gt_frame_t_ns.emplace_back(t_ns); + } + } + + basalt::MargDataLoader mdl; + tbb::concurrent_bounded_queue marg_queue; + mdl.out_marg_queue = &marg_queue; + + mdl.start(cache_path); + + while (true) { + basalt::MargData::Ptr data; + marg_queue.pop(data); + + if (data.get()) { + int64_t t_ns = *data->kfs_to_marg.begin(); + marg_data[t_ns] = data; + } else { + break; + } + } + + std::cout << "Loaded " << marg_data.size() << " marg data." << std::endl; +} + +void processMargData(basalt::MargData& m) { + BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols())); + + std::cout << "rank " << m.abs_H.fullPivLu().rank() << " size " + << m.abs_H.cols() << std::endl; + + basalt::AbsOrderMap aom_new; + std::set idx_to_keep; + std::set idx_to_marg; + + for (const auto& kv : m.aom.abs_order_map) { + if (kv.second.second == POSE_SIZE) { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_keep.emplace(kv.second.first + i); + aom_new.abs_order_map.emplace(kv); + aom_new.total_size += POSE_SIZE; + } else if (kv.second.second == POSE_VEL_BIAS_SIZE) { + if (m.kfs_all.count(kv.first) > 0) { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_keep.emplace(kv.second.first + i); + for (size_t i = POSE_SIZE; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_marg.emplace(kv.second.first + i); + + aom_new.abs_order_map[kv.first] = + std::make_pair(aom_new.total_size, POSE_SIZE); + aom_new.total_size += POSE_SIZE; + + basalt::PoseStateWithLin p = m.frame_states.at(kv.first); + m.frame_poses[kv.first] = p; + m.frame_states.erase(kv.first); + } else { + for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_marg.emplace(kv.second.first + i); + m.frame_states.erase(kv.first); + } + } else { + std::cerr << "Unknown size" << std::endl; + std::abort(); + } + + std::cout << kv.first << " " << kv.second.first << " " << kv.second.second + << std::endl; + } + + Eigen::MatrixXd marg_H_new; + Eigen::VectorXd marg_b_new; + basalt::KeypointVioEstimator::marginalizeHelper( + m.abs_H, m.abs_b, idx_to_keep, idx_to_marg, marg_H_new, marg_b_new); + + std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size " + << marg_H_new.cols() << std::endl; + + m.abs_H = marg_H_new; + m.abs_b = marg_b_new; + m.aom = aom_new; + + BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols())); +} + +void extractNonlinearFactors(basalt::MargData& m) { + size_t asize = m.aom.total_size; + std::cout << "asize " << asize << std::endl; + + Eigen::MatrixXd cov_old; + cov_old.setIdentity(asize, asize); + m.abs_H.ldlt().solveInPlace(cov_old); + + int64_t kf_id = *m.kfs_to_marg.cbegin(); + int kf_start_idx = m.aom.abs_order_map.at(kf_id).first; + + auto state_kf = m.frame_poses.at(kf_id); + + Sophus::SE3d T_w_i_kf = state_kf.getPose(); + + Eigen::Vector3d pos = T_w_i_kf.translation(); + Eigen::Vector3d yaw_dir_body = + T_w_i_kf.so3().inverse() * Eigen::Vector3d::UnitX(); + + Sophus::Matrix d_pos_d_T_w_i; + Sophus::Matrix d_yaw_d_T_w_i; + Sophus::Matrix d_rp_d_T_w_i; + + basalt::absPositionError(T_w_i_kf, pos, &d_pos_d_T_w_i); + basalt::yawError(T_w_i_kf, yaw_dir_body, &d_yaw_d_T_w_i); + basalt::rollPitchError(T_w_i_kf, T_w_i_kf.so3(), &d_rp_d_T_w_i); + + { + Eigen::MatrixXd J; + J.setZero(POSE_SIZE, asize); + J.block<3, POSE_SIZE>(0, kf_start_idx) = d_pos_d_T_w_i; + J.block<1, POSE_SIZE>(3, kf_start_idx) = d_yaw_d_T_w_i; + J.block<2, POSE_SIZE>(4, kf_start_idx) = d_rp_d_T_w_i; + + Sophus::Matrix6d cov_new = J * cov_old * J.transpose(); + + // std::cout << "cov_new\n" << cov_new << std::endl; + + basalt::RollPitchFactor rpf; + rpf.t_ns = kf_id; + rpf.R_w_i_meas = T_w_i_kf.so3(); + rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse(); + + roll_pitch_factors.emplace_back(rpf); + } + + for (int64_t other_id : m.kfs_all) { + if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) { + continue; + } + + auto state_o = m.frame_poses.at(other_id); + + Sophus::SE3d T_w_i_o = state_o.getPose(); + Sophus::SE3d T_kf_o = T_w_i_kf.inverse() * T_w_i_o; + + int o_start_idx = m.aom.abs_order_map.at(other_id).first; + + Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j; + basalt::relPoseError(T_kf_o, T_w_i_kf, T_w_i_o, &d_res_d_T_w_i, + &d_res_d_T_w_j); + + Eigen::MatrixXd J; + J.setZero(POSE_SIZE, asize); + J.block(0, kf_start_idx) = d_res_d_T_w_i; + J.block(0, o_start_idx) = d_res_d_T_w_j; + + Sophus::Matrix6d cov_new = J * cov_old * J.transpose(); + basalt::RelPoseFactor rpf; + rpf.t_i_ns = kf_id; + rpf.t_j_ns = other_id; + rpf.T_i_j = T_kf_o; + rpf.cov_inv.setIdentity(); + cov_new.ldlt().solveInPlace(rpf.cov_inv); + + // std::cout << "rpf.cov_inv\n" << rpf.cov_inv << std::endl; + + rel_pose_factors.emplace_back(rpf); + } +} + +void computeEdgeVis() { + edges_vis.clear(); + for (const auto& kv1 : nrf_mapper->obs) { + for (const auto& kv2 : kv1.second) { + Eigen::Vector3d p1 = nrf_mapper->getFramePoses() + .at(kv1.first.first) + .getPose() + .translation(); + Eigen::Vector3d p2 = nrf_mapper->getFramePoses() + .at(kv2.first.first) + .getPose() + .translation(); + + edges_vis.emplace_back(p1); + edges_vis.emplace_back(p2); + } + } + + roll_pitch_vis.clear(); + for (const auto& v : nrf_mapper->roll_pitch_factors) { + const Sophus::SE3d& T_w_i = + nrf_mapper->getFramePoses().at(v.t_ns).getPose(); + + Eigen::Vector3d p = T_w_i.translation(); + Eigen::Vector3d d = + v.R_w_i_meas * T_w_i.so3().inverse() * (-Eigen::Vector3d::UnitZ()); + + roll_pitch_vis.emplace_back(p); + roll_pitch_vis.emplace_back(p + 0.1 * d); + } + + rel_edges_vis.clear(); + for (const auto& v : nrf_mapper->rel_pose_factors) { + Eigen::Vector3d p1 = + nrf_mapper->getFramePoses().at(v.t_i_ns).getPose().translation(); + Eigen::Vector3d p2 = + nrf_mapper->getFramePoses().at(v.t_j_ns).getPose().translation(); + + rel_edges_vis.emplace_back(p1); + rel_edges_vis.emplace_back(p2); + } +} + +void optimize() { + nrf_mapper->optimize(); + nrf_mapper->get_current_points(mapper_points, mapper_point_ids); + // nrf_mapper->get_current_points_with_color(mapper_points, + // mapper_points_color, + // mapper_point_ids); + + computeEdgeVis(); +} + +void randomInc() { + Sophus::Vector6d rnd = Sophus::Vector6d::Random().array().abs(); + Sophus::SE3d random_inc = Sophus::expd(rnd / 10); + + for (auto& kv : nrf_mapper->getFramePoses()) { + Sophus::SE3d pose = random_inc * kv.second.getPose(); + basalt::PoseStateWithLin p(kv.first, pose); + kv.second = p; + } + + computeEdgeVis(); +} + +void randomYawInc() { + Sophus::Vector6d rnd; + rnd.setZero(); + rnd[5] = std::abs(Eigen::Vector2d::Random()[0]); + + Sophus::SE3d random_inc = Sophus::expd(rnd); + + std::cout << "random_inc\n" << random_inc.matrix() << std::endl; + + for (auto& kv : nrf_mapper->getFramePoses()) { + Sophus::SE3d pose = random_inc * kv.second.getPose(); + basalt::PoseStateWithLin p(kv.first, pose); + kv.second = p; + } + + computeEdgeVis(); +} + +double alignButton() { + Eigen::vector filter_t_w_i; + std::vector filter_t_ns; + + for (const auto& kv : nrf_mapper->getFramePoses()) { + filter_t_ns.emplace_back(kv.first); + filter_t_w_i.emplace_back(kv.second.getPose().translation()); + } + + return basalt::alignSVD(filter_t_ns, filter_t_w_i, gt_frame_t_ns, + gt_frame_t_w_i); +} + +void setup_points() { + for (auto& kv : nrf_mapper->getFramePoses()) { + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + basalt::TimeCamId tcid(kv.first, i); + auto obs = noisy_observations.at(tcid); + + basalt::KeypointsData kd; + kd.corners = obs.pos; + + nrf_mapper->feature_corners[tcid] = kd; + + for (size_t j = 0; j < kd.corners.size(); j++) { + nrf_mapper->feature_tracks[obs.id[j]][tcid] = j; + } + } + } + + for (auto it = nrf_mapper->feature_tracks.cbegin(); + it != nrf_mapper->feature_tracks.cend();) { + if (it->second.size() < 5) { + it = nrf_mapper->feature_tracks.erase(it); + } else { + ++it; + } + } + + std::cerr << "nrf_mapper->feature_tracks.size() " + << nrf_mapper->feature_tracks.size() << std::endl; + + nrf_mapper->setup_opt(); + + nrf_mapper->get_current_points(mapper_points, mapper_point_ids); +} diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp new file mode 100644 index 0000000..6635197 --- /dev/null +++ b/src/mapper_sim_naive.cpp @@ -0,0 +1,751 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include + +// GUI functions +void draw_image_overlay(pangolin::View& v, size_t cam_id); +void draw_scene(); +void load_data(const std::string& calib_path); +void gen_data(); +void compute_projections(); +void setup_vio(); +void draw_plots(); +bool next_step(); +void alignButton(); + +static const int knot_time = 3; +static const double obs_std_dev = 0.5; +static const double accel_std_dev = 0.23; +static const double gyro_std_dev = 0.0027; + +static const double accel_bias_std_dev = 0.00123; +static const double gyro_bias_std_dev = 0.000234; + +Eigen::Vector3d g(0, 0, -9.81); + +// std::random_device rd{}; +// std::mt19937 gen{rd()}; +std::mt19937 gen{1}; + +std::normal_distribution<> obs_noise_dist{0, obs_std_dev}; +std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev}; +std::normal_distribution<> accel_noise_dist{0, accel_std_dev}; + +std::normal_distribution<> gyro_bias_dist{0, gyro_bias_std_dev}; +std::normal_distribution<> accel_bias_dist{0, accel_bias_std_dev}; + +// Simulated data + +basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9)); + +Eigen::vector gt_points; +Eigen::vector gt_frame_T_w_i; +Eigen::vector gt_frame_t_w_i, vio_t_w_i; +std::vector gt_frame_t_ns, kf_t_ns; +Eigen::vector gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, + noisy_accel, noisy_gyro, gt_vel; +std::vector gt_imu_t_ns; + +std::map gt_observations; +std::map noisy_observations; + +std::string marg_data_path; + +// VIO vars +basalt::Calibration calib; +basalt::KeypointVioEstimator::Ptr vio; + +// Visualization vars +std::unordered_map vis_map; +tbb::concurrent_bounded_queue out_vis_queue; +tbb::concurrent_bounded_queue out_state_queue; + +std::vector images; + +// Pangolin vars +constexpr int UI_WIDTH = 200; +pangolin::DataLog imu_data_log, vio_data_log, error_data_log; +pangolin::Plotter* plotter; + +pangolin::Var show_frame("ui.show_frame", 0, 0, 1000); + +pangolin::Var show_obs("ui.show_obs", true, false, true); +pangolin::Var show_obs_noisy("ui.show_obs_noisy", true, false, true); +pangolin::Var show_obs_vio("ui.show_obs_vio", true, false, true); + +pangolin::Var show_ids("ui.show_ids", false, false, true); + +pangolin::Var show_accel("ui.show_accel", false, false, true); +pangolin::Var show_gyro("ui.show_gyro", false, false, true); +pangolin::Var show_gt_vel("ui.show_gt_vel", false, false, true); +pangolin::Var show_gt_pos("ui.show_gt_pos", true, false, true); +pangolin::Var show_gt_bg("ui.show_gt_bg", false, false, true); +pangolin::Var show_gt_ba("ui.show_gt_ba", false, false, true); + +pangolin::Var show_est_vel("ui.show_est_vel", false, false, true); +pangolin::Var show_est_pos("ui.show_est_pos", true, false, true); +pangolin::Var show_est_bg("ui.show_est_bg", false, false, true); +pangolin::Var show_est_ba("ui.show_est_ba", false, false, true); + +using Button = pangolin::Var>; + +Button next_step_btn("ui.next_step", &next_step); + +pangolin::Var continue_btn("ui.continue_btn", true, false, true); + +Button align_step_btn("ui.align_button", &alignButton); + +int main(int argc, char** argv) { + srand(1); + + bool show_gui = true; + std::string cam_calib_path; + + CLI::App app{"App description"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, + "Ground-truth camera calibration used for simulation.") + ->required(); + + app.add_option("--marg-data", marg_data_path, + "Folder to store marginalization data.") + ->required(); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError& e) { + return app.exit(e); + } + + load_data(cam_calib_path); + gen_data(); + + setup_vio(); + + vio->out_vis_queue = &out_vis_queue; + vio->out_state_queue = &out_state_queue; + + std::thread t0([&]() { + for (size_t i = 0; i < gt_imu_t_ns.size(); i++) { + basalt::ImuData::Ptr data(new basalt::ImuData); + data->t_ns = gt_imu_t_ns[i]; + + data->accel = noisy_accel[i]; + data->gyro = noisy_gyro[i]; + + data->accel_cov.setConstant(accel_std_dev * accel_std_dev); + data->gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + + vio->addIMUToQueue(data); + } + + vio->addIMUToQueue(nullptr); + + std::cout << "Finished t0" << std::endl; + }); + + std::thread t1([&]() { + for (const auto& t_ns : kf_t_ns) { + basalt::OpticalFlowResult::Ptr data(new basalt::OpticalFlowResult); + data->t_ns = t_ns; + + for (size_t j = 0; j < calib.T_i_c.size(); j++) { + data->observations.emplace_back(); + basalt::TimeCamId tcid(data->t_ns, j); + const basalt::SimObservations& obs = noisy_observations.at(tcid); + for (size_t k = 0; k < obs.pos.size(); k++) { + Eigen::AffineCompact2f t; + t.setIdentity(); + t.translation() = obs.pos[k].cast(); + data->observations.back()[obs.id[k]] = t; + } + } + + vio->addVisionToQueue(data); + } + + vio->addVisionToQueue(nullptr); + + std::cout << "Finished t1" << std::endl; + }); + + std::thread t2([&]() { + basalt::VioVisualizationData::Ptr data; + + while (true) { + out_vis_queue.pop(data); + + if (data.get()) { + vis_map[data->t_ns] = data; + } else { + break; + } + } + + std::cout << "Finished t2" << std::endl; + }); + + std::thread t3([&]() { + basalt::PoseVelBiasState::Ptr data; + + while (true) { + out_state_queue.pop(data); + + if (!data.get()) break; + + int64_t t_ns = data->t_ns; + + // std::cerr << "t_ns " << t_ns << std::endl; + Sophus::SE3d T_w_i = data->T_w_i; + Eigen::Vector3d vel_w_i = data->vel_w_i; + Eigen::Vector3d bg = data->bias_gyro; + Eigen::Vector3d ba = data->bias_accel; + + vio_t_w_i.emplace_back(T_w_i.translation()); + + { + std::vector vals; + vals.push_back(t_ns * 1e-9); + + for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]); + for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]); + for (int i = 0; i < 3; i++) vals.push_back(bg[i]); + for (int i = 0; i < 3; i++) vals.push_back(ba[i]); + + vio_data_log.Log(vals); + } + } + + std::cout << "Finished t3" << std::endl; + }); + + if (show_gui) { + pangolin::CreateWindowAndBind("Main", 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::View& img_view_display = + pangolin::CreateDisplay() + .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.5) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds( + 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + plotter = new pangolin::Plotter(&imu_data_log, 0.0, kf_t_ns.back() * 1e-9, + -10.0, 10.0, 0.01f, 0.01f); + plot_display.AddDisplay(*plotter); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < calib.intrinsics.size()) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display.AddDisplay(*iv); + iv->extern_draw_function = + std::bind(&draw_image_overlay, std::placeholders::_1, idx); + } + + pangolin::OpenGlRenderState camera( + pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), + pangolin::ModelViewLookAt(15, 3, 15, 0, 0, 0, pangolin::AxisZ)); + + pangolin::View& display3D = + pangolin::CreateDisplay() + .SetAspect(-640 / 480.0) + .SetBounds(0.4, 1.0, 0.5, 1.0) + .SetHandler(new pangolin::Handler3D(camera)); + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + display3D.Activate(camera); + glClearColor(0.95f, 0.95f, 0.95f, 1.0f); + + draw_scene(); + + img_view_display.Activate(); + + if (show_frame.GuiChanged()) { + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + // img_view[i]->SetImage(images[i]); + } + draw_plots(); + } + + if (show_accel.GuiChanged() || show_gyro.GuiChanged() || + show_gt_vel.GuiChanged() || show_gt_pos.GuiChanged() || + show_gt_ba.GuiChanged() || show_gt_bg.GuiChanged() || + show_est_vel.GuiChanged() || show_est_pos.GuiChanged() || + show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) { + draw_plots(); + } + + pangolin::FinishFrame(); + + if (continue_btn) { + if (!next_step()) continue_btn = false; + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + } + + t0.join(); + t1.join(); + t2.join(); + t3.join(); + // t4.join(); + + return 0; +} + +void draw_image_overlay(pangolin::View& v, size_t cam_id) { + size_t frame_id = show_frame; + basalt::TimeCamId tcid = std::make_pair(kf_t_ns[frame_id], cam_id); + + if (show_obs) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (gt_observations.find(tcid) != gt_observations.end()) { + const basalt::SimObservations& cr = gt_observations.at(tcid); + + for (size_t i = 0; i < cr.pos.size(); i++) { + const float radius = 2; + const Eigen::Vector2f c = cr.pos[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]); + } + + pangolin::GlFont::I().Text("%d gt points", cr.pos.size()).Draw(5, 20); + } + } + + if (show_obs_noisy) { + glLineWidth(1.0); + glColor3f(1.0, 1.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (noisy_observations.find(tcid) != noisy_observations.end()) { + const basalt::SimObservations& cr = noisy_observations.at(tcid); + + for (size_t i = 0; i < cr.pos.size(); i++) { + const float radius = 2; + const Eigen::Vector2f c = cr.pos[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]); + } + + pangolin::GlFont::I().Text("%d noisy points", cr.pos.size()).Draw(5, 40); + } + } + + if (show_obs_vio) { + glLineWidth(1.0); + glColor3f(0.0, 0.0, 1.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + auto it = vis_map.find(gt_frame_t_ns[frame_id]); + + if (it != vis_map.end() && cam_id < it->second->projections.size()) { + const auto& points = it->second->projections[cam_id]; + + if (points.size() > 0) { + double min_id = points[0][2], max_id = points[0][2]; + for (size_t i = 0; i < points.size(); i++) { + min_id = std::min(min_id, points[i][2]); + max_id = std::max(max_id, points[i][2]); + } + + for (size_t i = 0; i < points.size(); i++) { + const float radius = 2; + const Eigen::Vector4d c = points[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]); + } + } + + glColor3f(0.0, 0.0, 1.0); + pangolin::GlFont::I().Text("%d vio points", points.size()).Draw(5, 60); + } + } +} + +void draw_scene() { + glPointSize(3); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glColor3ubv(gt_color); + pangolin::glDrawPoints(gt_points); + pangolin::glDrawLineStrip(gt_frame_t_w_i); + + glColor3ubv(cam_color); + pangolin::glDrawLineStrip(vio_t_w_i); + + size_t frame_id = show_frame; + + auto it = vis_map.find(kf_t_ns[frame_id]); + + if (it != vis_map.end()) { + for (const auto& p : it->second->states) + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, cam_color, 0.1f); + + for (const auto& p : it->second->frames) + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); + + glColor3ubv(pose_color); + pangolin::glDrawPoints(it->second->points); + } + + // pangolin::glDrawAxis(gt_frame_T_w_i[frame_id].matrix(), 0.1); + + pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); +} + +void load_data(const std::string& calib_path) { + std::ifstream os(calib_path, std::ios::binary); + + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + std::cout << "Loaded camera with " << calib.intrinsics.size() + << " cameras" << std::endl; + + } else { + std::cerr << "could not load camera calibration " << calib_path + << std::endl; + std::abort(); + } +} + +void gen_data() { + // Save spline data + { + std::string path = marg_data_path + "/gt_spline.cereal"; + + std::cout << "Loading gt_spline " << path << std::endl; + + std::ifstream is(path, std::ios::binary); + { + cereal::JSONInputArchive archive(is); + + int64_t t_ns; + Eigen::vector knots; + + archive(cereal::make_nvp("t_ns", t_ns)); + archive(cereal::make_nvp("knots", knots)); + + gt_spline = basalt::Se3Spline<5>(t_ns); + + for (size_t i = 0; i < knots.size(); i++) { + gt_spline.knots_push_back(knots[i]); + } + + archive(cereal::make_nvp("noisy_accel", noisy_accel)); + archive(cereal::make_nvp("noisy_gyro", noisy_gyro)); + archive(cereal::make_nvp("noisy_accel", gt_accel)); + archive(cereal::make_nvp("gt_gyro", gt_gyro)); + archive(cereal::make_nvp("gt_accel_bias", gt_accel_bias)); + archive(cereal::make_nvp("gt_gyro_bias", gt_gyro_bias)); + + archive(cereal::make_nvp("gt_points", gt_points)); + + archive(cereal::make_nvp("gt_observations", gt_observations)); + archive(cereal::make_nvp("noisy_observations", noisy_observations)); + + archive(cereal::make_nvp("gt_points", gt_points)); + + archive(cereal::make_nvp("gt_frame_t_ns", gt_frame_t_ns)); + archive(cereal::make_nvp("gt_imu_t_ns", gt_imu_t_ns)); + } + + gt_frame_t_w_i.clear(); + for (int64_t t_ns : gt_frame_t_ns) { + gt_frame_t_w_i.emplace_back(gt_spline.pose(t_ns).translation()); + } + + is.close(); + } + + basalt::MargDataLoader mdl; + tbb::concurrent_bounded_queue marg_queue; + mdl.out_marg_queue = &marg_queue; + + mdl.start(marg_data_path); + + Eigen::map tmp_poses; + + while (true) { + basalt::MargData::Ptr data; + marg_queue.pop(data); + + if (data.get()) { + for (const auto& kv : data->frame_poses) { + tmp_poses[kv.first] = kv.second.getPose(); + } + + for (const auto& kv : data->frame_states) { + if (data->kfs_all.count(kv.first) > 0) { + tmp_poses[kv.first] = kv.second.getState().T_w_i; + } + } + + } else { + break; + } + } + + for (const auto& kv : tmp_poses) { + kf_t_ns.emplace_back(kv.first); + } + + show_frame.Meta().range[1] = kf_t_ns.size() - 1; +} + +void draw_plots() { + plotter->ClearSeries(); + plotter->ClearMarkers(); + + if (show_accel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "accel measurements x"); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "accel measurements y"); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "accel measurements z"); + } + + if (show_gyro) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "gyro measurements x"); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "gyro measurements y"); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "gyro measurements z"); + } + + if (show_gt_vel) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth velocity x"); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth velocity y"); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth velocity z"); + } + + if (show_gt_pos) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth position x"); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth position y"); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth position z"); + } + + if (show_gt_bg) { + plotter->AddSeries("$0", "$13", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth gyro bias x"); + plotter->AddSeries("$0", "$14", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth gyro bias y"); + plotter->AddSeries("$0", "$15", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth gyro bias z"); + } + + if (show_gt_ba) { + plotter->AddSeries("$0", "$16", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth accel bias x"); + plotter->AddSeries("$0", "$17", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth accel bias y"); + plotter->AddSeries("$0", "$18", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth accel bias z"); + } + + if (show_est_vel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated velocity x", + &vio_data_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated velocity y", + &vio_data_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated velocity z", + &vio_data_log); + } + + if (show_est_pos) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated position x", + &vio_data_log); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated position y", + &vio_data_log); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated position z", + &vio_data_log); + } + + if (show_est_bg) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated gyro bias x", + &vio_data_log); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated gyro bias y", + &vio_data_log); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated gyro bias z", + &vio_data_log); + } + + if (show_est_ba) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated accel bias x", + &vio_data_log); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated accel bias y", + &vio_data_log); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated accel bias z", + &vio_data_log); + } + + double t = kf_t_ns[show_frame] * 1e-9; + plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal, + pangolin::Colour::White()); +} + +void setup_vio() { + int64_t t_init_ns = kf_t_ns.front(); + Sophus::SE3d T_w_i_init = gt_spline.pose(t_init_ns); + Eigen::Vector3d vel_w_i_init = gt_spline.transVelWorld(t_init_ns); + + std::cout << "Setting up filter: t_ns " << t_init_ns << std::endl; + std::cout << "T_w_i\n" << T_w_i_init.matrix() << std::endl; + std::cout << "vel_w_i " << vel_w_i_init.transpose() << std::endl; + + basalt::VioConfig config; + config.vio_debug = true; + + vio.reset(new basalt::KeypointVioEstimator( + t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(), + gt_accel_bias.front(), 0.0001, g, calib, config)); + + vio->setMaxStates(10000); + vio->setMaxKfs(10000); + + // int iteration = 0; + vio_data_log.Clear(); + error_data_log.Clear(); + vio_t_w_i.clear(); +} + +bool next_step() { + if (show_frame < int(kf_t_ns.size()) - 1) { + show_frame = show_frame + 1; + show_frame.Meta().gui_changed = true; + return true; + } else { + return false; + } +} + +void alignButton() { + Eigen::vector vio_t_w_i; + + auto it = vis_map.find(kf_t_ns.back()); + + if (it != vis_map.end()) { + for (const auto& t : it->second->states) + vio_t_w_i.emplace_back(t.translation()); + + } else { + std::cerr << "Could not find results!!" << std::endl; + } + + BASALT_ASSERT(kf_t_ns.size() == vio_t_w_i.size()); + + basalt::alignSVD(kf_t_ns, vio_t_w_i, gt_frame_t_ns, gt_frame_t_w_i); +} diff --git a/src/opt_flow.cpp b/src/opt_flow.cpp new file mode 100644 index 0000000..233bf6f --- /dev/null +++ b/src/opt_flow.cpp @@ -0,0 +1,342 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include + +#include + +constexpr int UI_WIDTH = 200; + +void draw_image_overlay(pangolin::View& v, size_t cam_id); +void load_data(const std::string& calib_path); +bool next_step(); + +pangolin::Var show_frame("ui.show_frame", 0, 0, 1500); +pangolin::Var show_obs("ui.show_obs", true, false, true); +pangolin::Var show_ids("ui.show_ids", false, false, true); + +using Button = pangolin::Var>; +Button next_step_btn("ui.next_step", &next_step); +pangolin::Var continue_btn("ui.continue_btn", true, false, true); + +// Opt flow variables +basalt::VioDatasetPtr vio_dataset; + +basalt::VioConfig vio_config; +basalt::OpticalFlowBase::Ptr opt_flow_ptr; + +tbb::concurrent_unordered_map + observations; +tbb::concurrent_bounded_queue + observations_queue; + +basalt::Calibration calib; + +std::unordered_map keypoint_stats; + +void feed_images() { + std::cout << "Started input_data thread " << std::endl; + + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + basalt::OpticalFlowInput::Ptr data(new basalt::OpticalFlowInput); + + data->t_ns = vio_dataset->get_image_timestamps()[i]; + data->img_data = vio_dataset->get_image_data(data->t_ns); + + opt_flow_ptr->input_queue.push(data); + } + + // Indicate the end of the sequence + basalt::OpticalFlowInput::Ptr data; + opt_flow_ptr->input_queue.push(data); + + std::cout << "Finished input_data thread " << std::endl; +} + +void read_result() { + std::cout << "Started read_result thread " << std::endl; + + basalt::OpticalFlowResult::Ptr res; + + while (true) { + observations_queue.pop(res); + if (!res.get()) break; + + res->input_images.reset(); + + observations.emplace(res->t_ns, res); + + for (size_t i = 0; i < res->observations.size(); i++) + for (const auto& kv : res->observations.at(i)) { + if (keypoint_stats.count(kv.first) == 0) { + keypoint_stats[kv.first] = 1; + } else { + keypoint_stats[kv.first]++; + } + } + } + + std::cout << "Finished read_result thread " << std::endl; + + double sum = 0; + + for (const auto& kv : keypoint_stats) { + sum += kv.second; + } + + std::cout << "Mean track length: " << sum / keypoint_stats.size() + << " num_points: " << keypoint_stats.size() << std::endl; +} + +int main(int argc, char** argv) { + bool show_gui = true; + std::string cam_calib_path; + std::string dataset_path; + std::string dataset_type; + std::string config_path; + + CLI::App app{"App description"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, + "Ground-truth camera calibration used for simulation.") + ->required(); + + app.add_option("--dataset-path", dataset_path, "Path to dataset.") + ->required(); + + app.add_option("--dataset-type", dataset_type, "Type of dataset.") + ->required(); + + app.add_option("--config-path", config_path, "Path to config file."); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError& e) { + return app.exit(e); + } + + if (!config_path.empty()) { + vio_config.load(config_path); + } + + load_data(cam_calib_path); + + { + basalt::DatasetIoInterfacePtr dataset_io = + basalt::DatasetIoFactory::getDatasetIo(dataset_type); + + dataset_io->read(dataset_path); + + vio_dataset = dataset_io->get_data(); + vio_dataset->get_image_timestamps().erase( + vio_dataset->get_image_timestamps().begin()); + + show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1; + show_frame.Meta().gui_changed = true; + + opt_flow_ptr = + basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib); + if (show_gui) opt_flow_ptr->output_queue = &observations_queue; + observations_queue.set_capacity(100); + + keypoint_stats.reserve(50000); + } + + std::thread t1(&feed_images); + + if (show_gui) { + std::thread t2(&read_result); + + pangolin::CreateWindowAndBind("Main", 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::View& img_view_display = + pangolin::CreateDisplay() + .SetBounds(0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) + .SetLayout(pangolin::LayoutEqual); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < calib.intrinsics.size()) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display.AddDisplay(*iv); + iv->extern_draw_function = + std::bind(&draw_image_overlay, std::placeholders::_1, idx); + } + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + glClearColor(0.95f, 0.95f, 0.95f, 1.0f); + + img_view_display.Activate(); + + if (show_frame.GuiChanged()) { + size_t frame_id = static_cast(show_frame); + int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id]; + + const std::vector& img_vec = + vio_dataset->get_image_data(timestamp); + + for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) { + if (img_vec[cam_id].img.get()) { + auto img = img_vec[cam_id].img->toPangoImage(); + + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + img_view[cam_id]->SetImage(img.ptr, img.w, img.h, img.pitch, fmt); + } else { + img_view[cam_id]->Clear(); + } + } + } + + pangolin::FinishFrame(); + + if (continue_btn) { + if (!next_step()) { + continue_btn = false; + } + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + + t2.join(); + } + + t1.join(); + + return 0; +} + +void draw_image_overlay(pangolin::View& v, size_t cam_id) { + size_t frame_id = static_cast(show_frame); + int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; + + if (show_obs) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (observations.count(t_ns) > 0) { + const Eigen::map& kp_map = + observations.at(t_ns)->observations[cam_id]; + + for (const auto& kv : kp_map) { + Eigen::MatrixXf transformed_patch = + kv.second.linear() * opt_flow_ptr->patch_coord; + transformed_patch.colwise() += kv.second.translation(); + + for (int i = 0; i < transformed_patch.cols(); i++) { + const Eigen::Vector2f c = transformed_patch.col(i); + pangolin::glDrawCirclePerimeter(c[0], c[1], 0.5f); + } + + const Eigen::Vector2f c = kv.second.translation(); + + if (show_ids) + pangolin::GlFont::I().Text("%d", kv.first).Draw(5 + c[0], 5 + c[1]); + } + + pangolin::GlFont::I() + .Text("Tracked %d keypoints", kp_map.size()) + .Draw(5, 20); + } + } +} + +void load_data(const std::string& calib_path) { + std::ifstream os(calib_path, std::ios::binary); + + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + std::cout << "Loaded camera with " << calib.intrinsics.size() + << " cameras" << std::endl; + + } else { + std::cerr << "could not load camera calibration " << calib_path + << std::endl; + std::abort(); + } +} + +bool next_step() { + if (show_frame < int(vio_dataset->get_image_timestamps().size()) - 1) { + show_frame = show_frame + 1; + show_frame.Meta().gui_changed = true; + return true; + } else { + return false; + } +} diff --git a/src/optical_flow/optical_flow.cpp b/src/optical_flow/optical_flow.cpp new file mode 100644 index 0000000..7861fae --- /dev/null +++ b/src/optical_flow/optical_flow.cpp @@ -0,0 +1,101 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include + +#include +#include + +namespace basalt { + +OpticalFlowBase::Ptr OpticalFlowFactory::getOpticalFlow( + const VioConfig& config, const Calibration& cam) { + OpticalFlowBase::Ptr res; + + if (config.optical_flow_type == "patch") { + switch (config.optical_flow_pattern) { + case 24: + res.reset(new PatchOpticalFlow(config, cam)); + break; + + case 52: + res.reset(new PatchOpticalFlow(config, cam)); + break; + + case 51: + res.reset(new PatchOpticalFlow(config, cam)); + break; + + case 50: + res.reset(new PatchOpticalFlow(config, cam)); + break; + + default: + std::cerr << "config.optical_flow_pattern " + << config.optical_flow_pattern << " is not supported." + << std::endl; + std::abort(); + } + } + + if (config.optical_flow_type == "frame_to_frame") { + switch (config.optical_flow_pattern) { + case 24: + res.reset(new FrameToFrameOpticalFlow(config, cam)); + break; + + case 52: + res.reset(new FrameToFrameOpticalFlow(config, cam)); + break; + + case 51: + res.reset(new FrameToFrameOpticalFlow(config, cam)); + break; + + case 50: + res.reset(new FrameToFrameOpticalFlow(config, cam)); + break; + + default: + std::cerr << "config.optical_flow_pattern " + << config.optical_flow_pattern << " is not supported." + << std::endl; + std::abort(); + } + } + return res; +} +} // namespace basalt diff --git a/src/utils/keypoints.cpp b/src/utils/keypoints.cpp new file mode 100644 index 0000000..2f162fb --- /dev/null +++ b/src/utils/keypoints.cpp @@ -0,0 +1,420 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include +#include + +#include +#include +#include +#include + +namespace basalt { + +// const int PATCH_SIZE = 31; +const int HALF_PATCH_SIZE = 15; +const int EDGE_THRESHOLD = 19; + +const static char pattern_31_x_a[256] = { + 8, 4, -11, 7, 2, 1, -2, -13, -13, 10, -13, -11, 7, -4, -13, + -9, 12, -3, -6, 11, 4, 5, 3, -8, -2, -13, -7, -4, -10, 5, + 5, 1, 9, 4, 2, -4, -8, 4, 0, -13, -3, -6, 8, 0, 7, + -13, 10, -6, 10, -13, -13, 3, 5, -1, 3, 2, -13, -13, -13, -7, + 6, -9, -2, -12, 3, -7, -3, 2, -11, -1, 5, -4, -9, -12, 10, + 7, -7, -4, 7, -7, -13, -3, 7, -13, 1, 2, -4, -1, 7, 1, + 9, -1, -13, 7, 12, 6, 5, 2, 3, 2, 9, -8, -11, 1, 6, + 2, 6, 3, 7, -11, -10, -5, -10, 8, 4, -10, 4, -2, -5, 7, + -9, -5, 8, -9, 1, 7, -2, 11, -12, 3, 5, 0, -9, 0, -1, + 5, 3, -13, -5, -4, 6, -7, -13, 1, 4, -2, 2, -2, 4, -6, + -3, 7, 4, -13, 7, 7, -7, -8, -13, 2, 10, -6, 8, 2, -11, + -12, -11, 5, -2, -1, -13, -10, -3, 2, -9, -4, -4, -6, 6, -13, + 11, 7, -1, -4, -7, -13, -7, -8, -5, -13, 1, 1, 9, 5, -1, + -9, -1, -13, 8, 2, 7, -10, -10, 4, 3, -4, 5, 4, -9, 0, + -12, 3, -10, 8, -8, 2, 10, 6, -7, -3, -1, -3, -8, 4, 2, + 6, 3, 11, -3, 4, 2, -10, -13, -13, 6, 0, -13, -9, -13, 5, + 2, -1, 9, 11, 3, -1, 3, -13, 5, 8, 7, -10, 7, 9, 7, + -1}; + +const static char pattern_31_y_a[256] = { + -3, 2, 9, -12, -13, -7, -10, -13, -3, 4, -8, 7, 7, -5, 2, + 0, -6, 6, -13, -13, 7, -3, -7, -7, 11, 12, 3, 2, -12, -12, + -6, 0, 11, 7, -1, -12, -5, 11, -8, -2, -2, 9, 12, 9, -5, + -6, 7, -3, -9, 8, 0, 3, 7, 7, -10, -4, 0, -7, 3, 12, + -10, -1, -5, 5, -10, -7, -2, 9, -13, 6, -3, -13, -6, -10, 2, + 12, -13, 9, -1, 6, 11, 7, -8, -7, -3, -6, 3, -13, 1, -1, + 1, -9, -13, 7, -5, 3, -13, -12, 8, 6, -12, 4, 12, 12, -9, + 3, 3, -3, 8, -5, 11, -8, 5, -1, -6, 12, -2, 0, -8, -6, + -13, -13, -8, -11, -8, -4, 1, -6, -9, 7, 5, -4, 12, 7, 2, + 11, 5, -4, 9, -7, 5, 6, 6, -10, 1, -2, -12, -13, 1, -10, + -13, 5, -2, 9, 1, -8, -4, 11, 6, 4, -5, -5, -3, -12, -2, + -13, 0, -3, -13, -8, -11, -2, 9, -3, -13, 6, 12, -11, -3, 11, + 11, -5, 12, -8, 1, -12, -2, 5, -1, 7, 5, 0, 12, -8, 11, + -3, -10, 1, -11, -13, -13, -10, -8, -6, 12, 2, -13, -13, 9, 3, + 1, 2, -10, -13, -12, 2, 6, 8, 10, -9, -13, -7, -2, 2, -5, + -9, -1, -1, 0, -11, -4, -6, 7, 12, 0, -1, 3, 8, -6, -9, + 7, -6, 5, -3, 0, 4, -6, 0, 8, 9, -4, 4, 3, -7, 0, + -6}; + +const static char pattern_31_x_b[256] = { + 9, 7, -8, 12, 2, 1, -2, -11, -12, 11, -8, -9, 12, -3, -12, -7, + 12, -2, -4, 12, 5, 10, 6, -6, -1, -8, -5, -3, -6, 6, 7, 4, + 11, 4, 4, -2, -7, 9, 1, -8, -2, -4, 10, 1, 11, -11, 12, -6, + 12, -8, -8, 7, 10, 1, 5, 3, -13, -12, -11, -4, 12, -7, 0, -7, + 8, -4, -1, 5, -5, 0, 5, -4, -9, -8, 12, 12, -6, -3, 12, -5, + -12, -2, 12, -11, 12, 3, -2, 1, 8, 3, 12, -1, -10, 10, 12, 7, + 6, 2, 4, 12, 10, -7, -4, 2, 7, 3, 11, 8, 9, -6, -5, -3, + -9, 12, 6, -8, 6, -2, -5, 10, -8, -5, 9, -9, 1, 9, -1, 12, + -6, 7, 10, 2, -5, 2, 1, 7, 6, -8, -3, -3, 8, -6, -5, 3, + 8, 2, 12, 0, 9, -3, -1, 12, 5, -9, 8, 7, -7, -7, -12, 3, + 12, -6, 9, 2, -10, -7, -10, 11, -1, 0, -12, -10, -2, 3, -4, -3, + -2, -4, 6, -5, 12, 12, 0, -3, -6, -8, -6, -6, -4, -8, 5, 10, + 10, 10, 1, -6, 1, -8, 10, 3, 12, -5, -8, 8, 8, -3, 10, 5, + -4, 3, -6, 4, -10, 12, -6, 3, 11, 8, -6, -3, -1, -3, -8, 12, + 3, 11, 7, 12, -3, 4, 2, -8, -11, -11, 11, 1, -9, -6, -8, 8, + 3, -1, 11, 12, 3, 0, 4, -10, 12, 9, 8, -10, 12, 10, 12, 0}; + +const static char pattern_31_y_b[256] = { + 5, -12, 2, -13, 12, 6, -4, -8, -9, 9, -9, 12, 6, 0, -3, + 5, -1, 12, -8, -8, 1, -3, 12, -2, -10, 10, -3, 7, 11, -7, + -1, -5, -13, 12, 4, 7, -10, 12, -13, 2, 3, -9, 7, 3, -10, + 0, 1, 12, -4, -12, -4, 8, -7, -12, 6, -10, 5, 12, 8, 7, + 8, -6, 12, 5, -13, 5, -7, -11, -13, -1, 2, 12, 6, -4, -3, + 12, 5, 4, 2, 1, 5, -6, -7, -12, 12, 0, -13, 9, -6, 12, + 6, 3, 5, 12, 9, 11, 10, 3, -6, -13, 3, 9, -6, -8, -4, + -2, 0, -8, 3, -4, 10, 12, 0, -6, -11, 7, 7, 12, 2, 12, + -8, -2, -13, 0, -2, 1, -4, -11, 4, 12, 8, 8, -13, 12, 7, + -9, -8, 9, -3, -12, 0, 12, -2, 10, -4, -13, 12, -6, 3, -5, + 1, -11, -7, -5, 6, 6, 1, -8, -8, 9, 3, 7, -8, 8, 3, + -9, -5, 8, 12, 9, -5, 11, -13, 2, 0, -10, -7, 9, 11, 5, + 6, -2, 7, -2, 7, -13, -8, -9, 5, 10, -13, -13, -1, -9, -13, + 2, 12, -10, -6, -6, -9, -7, -13, 5, -13, -3, -12, -1, 3, -9, + 1, -8, 9, 12, -5, 7, -8, -12, 5, 9, 5, 4, 3, 12, 11, + -13, 12, 4, 6, 12, 1, 1, 1, -13, -13, 4, -2, -3, -2, 10, + -9, -1, -2, -8, 5, 10, 5, 5, 11, -6, -12, 9, 4, -2, -2, + -11}; + +void detectKeypointsMapping(const basalt::Image& img_raw, + KeypointsData& kd, int num_features) { + cv::Mat image(img_raw.h, img_raw.w, CV_8U); + + uint8_t* dst = image.ptr(); + const uint16_t* src = img_raw.ptr; + + for (size_t i = 0; i < img_raw.size(); i++) { + dst[i] = (src[i] >> 8); + } + + std::vector points; + goodFeaturesToTrack(image, points, num_features, 0.01, 8); + + kd.corners.clear(); + kd.corner_angles.clear(); + kd.corner_descriptors.clear(); + + for (size_t i = 0; i < points.size(); i++) { + if (img_raw.InBounds(points[i].x, points[i].y, EDGE_THRESHOLD)) { + kd.corners.emplace_back(points[i].x, points[i].y); + } + } +} + +void detectKeypoints(const basalt::Image& img_raw, + KeypointsData& kd, int PATCH_SIZE, int num_points_cell, + const Eigen::vector& current_points) { + kd.corners.clear(); + kd.corner_angles.clear(); + kd.corner_descriptors.clear(); + + const size_t x_start = (img_raw.w % PATCH_SIZE) / 2; + const size_t x_stop = x_start + img_raw.w - PATCH_SIZE; + + const size_t y_start = (img_raw.h % PATCH_SIZE) / 2; + const size_t y_stop = y_start + img_raw.h - PATCH_SIZE; + + // std::cerr << "x_start " << x_start << " x_stop " << x_stop << std::endl; + // std::cerr << "y_start " << y_start << " y_stop " << y_stop << std::endl; + + Eigen::MatrixXi cells; + cells.setZero(img_raw.h / PATCH_SIZE + 1, img_raw.w / PATCH_SIZE + 1); + + for (const Eigen::Vector2d& p : current_points) { + if (p[0] >= x_start && p[1] >= y_start) { + int x = (p[0] - x_start) / PATCH_SIZE; + int y = (p[1] - y_start) / PATCH_SIZE; + + cells(y, x) += 1; + } + } + + for (size_t x = x_start; x < x_stop; x += PATCH_SIZE) { + for (size_t y = y_start; y < y_stop; y += PATCH_SIZE) { + if (cells((y - y_start) / PATCH_SIZE, (x - x_start) / PATCH_SIZE) > 0) + continue; + + const basalt::Image sub_img_raw = + img_raw.SubImage(x, y, PATCH_SIZE, PATCH_SIZE); + + cv::Mat subImg(PATCH_SIZE, PATCH_SIZE, CV_8U); + + for (int y = 0; y < PATCH_SIZE; y++) { + uchar* sub_ptr = subImg.ptr(y); + for (int x = 0; x < PATCH_SIZE; x++) { + sub_ptr[x] = (sub_img_raw(x, y) >> 8); + } + } + + int points_added = 0; + int threshold = 40; + + while (points_added < num_points_cell && threshold >= 10) { + std::vector points; + cv::FAST(subImg, points, threshold); + + std::sort(points.begin(), points.end(), + [](const cv::KeyPoint& a, const cv::KeyPoint& b) -> bool { + return a.response > b.response; + }); + + // std::cout << "Detected " << points.size() << " points. + // Threshold " + // << threshold << std::endl; + + for (size_t i = 0; i < points.size() && points_added < num_points_cell; + i++) + if (img_raw.InBounds(x + points[i].pt.x, y + points[i].pt.y, + EDGE_THRESHOLD)) { + kd.corners.emplace_back(x + points[i].pt.x, y + points[i].pt.y); + points_added++; + } + + threshold /= 2; + } + } + } + + // std::cout << "Total points: " << kd.corners.size() << std::endl; + + // cv::TermCriteria criteria = + // cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001); + // cv::Size winSize = cv::Size(5, 5); + // cv::Size zeroZone = cv::Size(-1, -1); + // cv::cornerSubPix(image, points, winSize, zeroZone, criteria); + + // for (size_t i = 0; i < points.size(); i++) { + // if (img_raw.InBounds(points[i].pt.x, points[i].pt.y, EDGE_THRESHOLD)) { + // kd.corners.emplace_back(points[i].pt.x, points[i].pt.y); + // } + // } +} + +void computeAngles(const basalt::Image& img_raw, + KeypointsData& kd, bool rotate_features) { + kd.corner_angles.resize(kd.corners.size()); + + for (size_t i = 0; i < kd.corners.size(); i++) { + const Eigen::Vector2d& p = kd.corners[i]; + + const int cx = p[0]; + const int cy = p[1]; + + double angle = 0; + + if (rotate_features) { + double m01 = 0, m10 = 0; + for (int x = -HALF_PATCH_SIZE; x <= HALF_PATCH_SIZE; x++) { + for (int y = -HALF_PATCH_SIZE; y <= HALF_PATCH_SIZE; y++) { + if (x * x + y * y <= HALF_PATCH_SIZE * HALF_PATCH_SIZE) { + double val = img_raw(cx + x, cy + y); + m01 += y * val; + m10 += x * val; + } + } + } + + angle = atan2(m01, m10); + } + + kd.corner_angles[i] = angle; + } +} + +void computeDescriptors(const basalt::Image& img_raw, + KeypointsData& kd) { + kd.corner_descriptors.resize(kd.corners.size()); + + for (size_t i = 0; i < kd.corners.size(); i++) { + std::bitset<256> descriptor; + + const Eigen::Vector2d& p = kd.corners[i]; + double angle = kd.corner_angles[i]; + + int cx = p[0]; + int cy = p[1]; + + Eigen::Rotation2Dd rot(angle); + Eigen::Matrix2d mat_rot = rot.matrix(); + + for (int i = 0; i < 256; i++) { + Eigen::Vector2d va(pattern_31_x_a[i], pattern_31_y_a[i]), + vb(pattern_31_x_b[i], pattern_31_y_b[i]); + + Eigen::Vector2i vva = (mat_rot * va).array().round().cast(); + Eigen::Vector2i vvb = (mat_rot * vb).array().round().cast(); + + descriptor[i] = + img_raw(cx + vva[0], cy + vva[1]) < img_raw(cx + vvb[0], cy + vvb[1]); + } + + kd.corner_descriptors[i] = descriptor; + } +} + +void matchFastHelper(const std::vector>& corner_descriptors_1, + const std::vector>& corner_descriptors_2, + std::map& matches, int threshold, + double test_dist) { + matches.clear(); + + for (size_t i = 0; i < corner_descriptors_1.size(); i++) { + int best_idx = -1, best_dist = 500; + int best2_dist = 500; + + for (size_t j = 0; j < corner_descriptors_2.size(); j++) { + int dist = (corner_descriptors_1[i] ^ corner_descriptors_2[j]).count(); + + if (dist <= best_dist) { + best2_dist = best_dist; + + best_dist = dist; + best_idx = j; + } else if (dist < best2_dist) { + best2_dist = dist; + } + } + + if (best_dist < threshold && best_dist * test_dist <= best2_dist) { + matches.emplace(i, best_idx); + } + } +} + +void matchDescriptors(const std::vector>& corner_descriptors_1, + const std::vector>& corner_descriptors_2, + std::vector>& matches, int threshold, + double dist_2_best) { + matches.clear(); + + std::map matches_1_2, matches_2_1; + matchFastHelper(corner_descriptors_1, corner_descriptors_2, matches_1_2, + threshold, dist_2_best); + matchFastHelper(corner_descriptors_2, corner_descriptors_1, matches_2_1, + threshold, dist_2_best); + + for (const auto& kv : matches_1_2) { + if (matches_2_1[kv.second] == kv.first) { + matches.emplace_back(kv.first, kv.second); + } + } +} + +void findInliersRansac(const KeypointsData& kd1, const KeypointsData& kd2, + const double ransac_thresh, const int ransac_min_inliers, + MatchData& md) { + md.inliers.clear(); + + opengv::bearingVectors_t bearingVectors1, bearingVectors2; + + for (size_t i = 0; i < md.matches.size(); i++) { + bearingVectors1.push_back(kd1.corners_3d[md.matches[i].first].head<3>()); + bearingVectors2.push_back(kd2.corners_3d[md.matches[i].second].head<3>()); + } + + // create the central relative adapter + opengv::relative_pose::CentralRelativeAdapter adapter(bearingVectors1, + bearingVectors2); + // create a RANSAC object + opengv::sac::Ransac< + opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem> + ransac; + // create a CentralRelativePoseSacProblem + // (set algorithm to STEWENIUS, NISTER, SEVENPT, or EIGHTPT) + std::shared_ptr< + opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem> + relposeproblem_ptr( + new opengv::sac_problems::relative_pose:: + CentralRelativePoseSacProblem( + adapter, opengv::sac_problems::relative_pose:: + CentralRelativePoseSacProblem::STEWENIUS)); + // run ransac + ransac.sac_model_ = relposeproblem_ptr; + ransac.threshold_ = ransac_thresh; + ransac.max_iterations_ = 25; + ransac.computeModel(); + + // do non-linear refinement and add more inliers + const size_t num_inliers_ransac = ransac.inliers_.size(); + + adapter.sett12(ransac.model_coefficients_.topRightCorner<3, 1>()); + adapter.setR12(ransac.model_coefficients_.topLeftCorner<3, 3>()); + + const opengv::transformation_t nonlinear_transformation = + opengv::relative_pose::optimize_nonlinear(adapter, ransac.inliers_); + + ransac.sac_model_->selectWithinDistance(nonlinear_transformation, + ransac.threshold_, ransac.inliers_); + + // Sanity check if the number of inliers decreased, but only warn if it is by + // 3 or more, since some small fluctuation is expected. + if (ransac.inliers_.size() + 2 < num_inliers_ransac) { + std::cout << "Warning: non-linear refinement reduced the relative pose " + "ransac inlier count from " + << num_inliers_ransac << " to " << ransac.inliers_.size() << "." + << std::endl; + } + + // get the result (normalize translation) + md.T_i_j = Sophus::SE3d( + nonlinear_transformation.topLeftCorner<3, 3>(), + nonlinear_transformation.topRightCorner<3, 1>().normalized()); + + if ((long)ransac.inliers_.size() >= ransac_min_inliers) { + for (size_t i = 0; i < ransac.inliers_.size(); i++) + md.inliers.emplace_back(md.matches[ransac.inliers_[i]]); + } +} + +} // namespace basalt diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp new file mode 100644 index 0000000..d0ce423 --- /dev/null +++ b/src/utils/vio_config.cpp @@ -0,0 +1,130 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include + +#include +#include + +namespace basalt { + +VioConfig::VioConfig() { + // optical_flow_type = "patch"; + optical_flow_type = "frame_to_frame"; + optical_flow_detection_grid_size = 50; + optical_flow_max_recovered_dist2 = 0.09f; + optical_flow_pattern = 51; + optical_flow_max_iterations = 5; + optical_flow_levels = 3; + optical_flow_epipolar_error = 0.005; + + vio_max_states = 3; + vio_max_kfs = 7; + vio_min_frames_after_kf = 5; + vio_new_kf_keypoints_thresh = 0.7; + vio_max_iterations = 5; + vio_debug = false; + vio_obs_std_dev = 0.5; + vio_obs_huber_thresh = 1.0; + + mapper_obs_std_dev = 0.25; + mapper_obs_huber_thresh = 1.5; + mapper_detection_num_points = 800; + mapper_num_frames_to_match = 30; + mapper_frames_to_match_threshold = 0.3; + mapper_min_matches = 20; + mapper_ransac_threshold = 5e-5; + mapper_min_track_length = 5; + mapper_max_hamming_distance = 70; + mapper_second_best_test_ratio = 1.2; +} + +void VioConfig::save(const std::string& filename) { + std::ofstream os(filename); + + { + cereal::JSONOutputArchive archive(os); + archive(*this); + } + os.close(); +} + +void VioConfig::load(const std::string& filename) { + std::ifstream is(filename); + + { + cereal::JSONInputArchive archive(is); + archive(*this); + } + is.close(); +} +} // namespace basalt + +namespace cereal { + +template +void serialize(Archive& ar, basalt::VioConfig& config) { + ar(CEREAL_NVP(config.optical_flow_type)); + ar(CEREAL_NVP(config.optical_flow_detection_grid_size)); + ar(CEREAL_NVP(config.optical_flow_max_recovered_dist2)); + ar(CEREAL_NVP(config.optical_flow_pattern)); + ar(CEREAL_NVP(config.optical_flow_max_iterations)); + ar(CEREAL_NVP(config.optical_flow_epipolar_error)); + ar(CEREAL_NVP(config.optical_flow_levels)); + + ar(CEREAL_NVP(config.vio_max_states)); + ar(CEREAL_NVP(config.vio_max_kfs)); + ar(CEREAL_NVP(config.vio_min_frames_after_kf)); + ar(CEREAL_NVP(config.vio_new_kf_keypoints_thresh)); + ar(CEREAL_NVP(config.vio_debug)); + ar(CEREAL_NVP(config.vio_max_iterations)); + + ar(CEREAL_NVP(config.vio_obs_std_dev)); + ar(CEREAL_NVP(config.vio_obs_huber_thresh)); + + ar(CEREAL_NVP(config.mapper_obs_std_dev)); + ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); + ar(CEREAL_NVP(config.mapper_detection_num_points)); + ar(CEREAL_NVP(config.mapper_num_frames_to_match)); + ar(CEREAL_NVP(config.mapper_frames_to_match_threshold)); + ar(CEREAL_NVP(config.mapper_min_matches)); + ar(CEREAL_NVP(config.mapper_ransac_threshold)); + ar(CEREAL_NVP(config.mapper_min_track_length)); + ar(CEREAL_NVP(config.mapper_max_hamming_distance)); + ar(CEREAL_NVP(config.mapper_second_best_test_ratio)); +} +} // namespace cereal diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp new file mode 100644 index 0000000..82b480d --- /dev/null +++ b/src/vi_estimator/ba_base.cpp @@ -0,0 +1,525 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + + +#include + +#include + +namespace basalt { + +Sophus::SE3d BundleAdjustmentBase::computeRelPose(const Sophus::SE3d& T_w_i_h, + const Sophus::SE3d& T_i_c_h, + const Sophus::SE3d& T_w_i_t, + const Sophus::SE3d& T_i_c_t, + Sophus::Matrix6d* d_rel_d_h, + Sophus::Matrix6d* d_rel_d_t) { + Sophus::SE3d tmp2 = (T_i_c_t).inverse(); + + Sophus::SE3d T_t_i_h_i; + T_t_i_h_i.so3() = T_w_i_t.so3().inverse() * T_w_i_h.so3(); + T_t_i_h_i.translation() = + T_w_i_t.so3().inverse() * (T_w_i_h.translation() - T_w_i_t.translation()); + + Sophus::SE3d tmp = tmp2 * T_t_i_h_i; + Sophus::SE3d res = tmp * T_i_c_h; + + if (d_rel_d_h) { + Eigen::Matrix3d R = T_w_i_h.so3().inverse().matrix(); + + Sophus::Matrix6d RR; + RR.setZero(); + RR.topLeftCorner<3, 3>() = R; + RR.bottomRightCorner<3, 3>() = R; + + *d_rel_d_h = tmp.Adj() * RR; + } + + if (d_rel_d_t) { + Eigen::Matrix3d R = T_w_i_t.so3().inverse().matrix(); + + Sophus::Matrix6d RR; + RR.setZero(); + RR.topLeftCorner<3, 3>() = R; + RR.bottomRightCorner<3, 3>() = R; + + *d_rel_d_t = -tmp2.Adj() * RR; + } + + return res; +} + +void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, + const RelLinData& rld, + const Eigen::VectorXd& inc) { + Eigen::VectorXd rel_inc; + rel_inc.setZero(rld.order.size() * POSE_SIZE); + for (size_t i = 0; i < rld.order.size(); i++) { + const TimeCamId& tcid_h = rld.order[i].first; + const TimeCamId& tcid_t = rld.order[i].second; + + if (tcid_h.first != tcid_t.first) { + int abs_h_idx = aom.abs_order_map.at(tcid_h.first).first; + int abs_t_idx = aom.abs_order_map.at(tcid_t.first).first; + + rel_inc.segment(i * POSE_SIZE) = + rld.d_rel_d_h[i] * inc.segment(abs_h_idx) + + rld.d_rel_d_t[i] * inc.segment(abs_t_idx); + } + } + + for (const auto& kv : rld.lm_to_obs) { + int lm_idx = kv.first; + const auto& other_obs = kv.second; + + Eigen::Vector3d H_l_p_x; + H_l_p_x.setZero(); + + for (size_t k = 0; k < other_obs.size(); k++) { + int rel_idx = other_obs[k].first; + const FrameRelLinData& frld_other = rld.Hpppl.at(rel_idx); + + Eigen::Matrix H_l_p_other = + frld_other.Hpl[other_obs[k].second].transpose(); + + H_l_p_x += H_l_p_other * rel_inc.segment(rel_idx * POSE_SIZE); + + // std::cerr << "inc_p " << inc_p.transpose() << std::endl; + } + + Eigen::Vector3d inc_p = rld.Hll.at(lm_idx) * (rld.bl.at(lm_idx) - H_l_p_x); + + KeypointPosition& kpt = kpts[lm_idx]; + kpt.dir -= inc_p.head<2>(); + kpt.id -= inc_p[2]; + + kpt.id = std::max(0., kpt.id); + } +} + +void BundleAdjustmentBase::computeError(double& error) const { + error = 0; + + for (const auto& kv : obs) { + const TimeCamId& tcid_h = kv.first; + + for (const auto& obs_kv : kv.second) { + const TimeCamId& tcid_t = obs_kv.first; + + if (tcid_h != tcid_t) { + PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.first); + PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first); + + Sophus::SE3d T_t_h_sophus = + computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.second], + state_t.getPose(), calib.T_i_c[tcid_t.second]); + + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = kpts.at(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); + + if (valid) { + double e = res.norm(); + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += + (2 - huber_weight) * obs_weight * res.transpose() * res; + } + } + }, + calib.intrinsics[tcid_t.second].variant); + + } else { + // target and host are the same + // residual does not depend on the pose + // it just depends on the point + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = kpts.at(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); + if (valid) { + double e = res.norm(); + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += + (2 - huber_weight) * obs_weight * res.transpose() * res; + } + } + }, + calib.intrinsics[tcid_t.second].variant); + } + } + } +} + +void BundleAdjustmentBase::linearizeHelper( + Eigen::vector& rld_vec, + const Eigen::map>>& + obs_to_lin, + double& error) const { + error = 0; + + rld_vec.clear(); + + std::vector obs_tcid_vec; + for (const auto& kv : obs_to_lin) { + obs_tcid_vec.emplace_back(kv.first); + rld_vec.emplace_back(kpts.size(), kv.second.size()); + } + + tbb::parallel_for( + tbb::blocked_range(0, obs_tcid_vec.size()), + [&](const tbb::blocked_range& range) { + for (size_t r = range.begin(); r != range.end(); ++r) { + auto kv = obs_to_lin.find(obs_tcid_vec[r]); + + RelLinData& rld = rld_vec[r]; + + rld.error = 0; + + const TimeCamId& tcid_h = kv->first; + + for (const auto& obs_kv : kv->second) { + const TimeCamId& tcid_t = obs_kv.first; + if (tcid_h != tcid_t) { + // target and host are not the same + rld.order.emplace_back(std::make_pair(tcid_h, tcid_t)); + + PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.first); + PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first); + + Sophus::Matrix6d d_rel_d_h, d_rel_d_t; + + Sophus::SE3d T_t_h_sophus = computeRelPose( + state_h.getPoseLin(), calib.T_i_c[tcid_h.second], + state_t.getPoseLin(), calib.T_i_c[tcid_t.second], &d_rel_d_h, + &d_rel_d_t); + + rld.d_rel_d_h.emplace_back(d_rel_d_h); + rld.d_rel_d_t.emplace_back(d_rel_d_t); + + if (state_h.isLinearized() || state_t.isLinearized()) { + T_t_h_sophus = computeRelPose( + state_h.getPose(), calib.T_i_c[tcid_h.second], + state_t.getPose(), calib.T_i_c[tcid_t.second]); + } + + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + FrameRelLinData frld; + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = kpts.at(kpt_obs.kpt_id); + + Eigen::Vector2d res; + Eigen::Matrix d_res_d_xi; + Eigen::Matrix d_res_d_p; + + bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, + res, &d_res_d_xi, &d_res_d_p); + + if (valid) { + double e = res.norm(); + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + rld.error += (2 - huber_weight) * obs_weight * + res.transpose() * res; + + if (rld.Hll.count(kpt_obs.kpt_id) == 0) { + rld.Hll[kpt_obs.kpt_id].setZero(); + rld.bl[kpt_obs.kpt_id].setZero(); + } + + rld.Hll[kpt_obs.kpt_id] += + obs_weight * d_res_d_p.transpose() * d_res_d_p; + rld.bl[kpt_obs.kpt_id] += + obs_weight * d_res_d_p.transpose() * res; + + frld.Hpp += + obs_weight * d_res_d_xi.transpose() * d_res_d_xi; + frld.bp += obs_weight * d_res_d_xi.transpose() * res; + frld.Hpl.emplace_back( + obs_weight * d_res_d_xi.transpose() * d_res_d_p); + frld.lm_id.emplace_back(kpt_obs.kpt_id); + + rld.lm_to_obs[kpt_obs.kpt_id].emplace_back( + rld.Hpppl.size(), frld.lm_id.size() - 1); + } + } + }, + calib.intrinsics[tcid_t.second].variant); + + rld.Hpppl.emplace_back(frld); + + } else { + // target and host are the same + // residual does not depend on the pose + // it just depends on the point + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = kpts.at(kpt_obs.kpt_id); + + Eigen::Vector2d res; + Eigen::Matrix d_res_d_p; + + bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res, + &d_res_d_p); + + if (valid) { + double e = res.norm(); + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + rld.error += (2 - huber_weight) * obs_weight * + res.transpose() * res; + + if (rld.Hll.count(kpt_obs.kpt_id) == 0) { + rld.Hll[kpt_obs.kpt_id].setZero(); + rld.bl[kpt_obs.kpt_id].setZero(); + } + + rld.Hll[kpt_obs.kpt_id] += + obs_weight * d_res_d_p.transpose() * d_res_d_p; + rld.bl[kpt_obs.kpt_id] += + obs_weight * d_res_d_p.transpose() * res; + } + } + }, + calib.intrinsics[tcid_t.second].variant); + } + } + } + + }); + + for (const auto& rld : rld_vec) error += rld.error; +} + +void BundleAdjustmentBase::linearizeRel(const RelLinData& rld, + Eigen::MatrixXd& H, + Eigen::VectorXd& b) { + // std::cout << "linearizeRel: KF " << frame_states.size() << " obs " + // << obs.size() << std::endl; + + // Do schur complement + size_t msize = rld.order.size(); + H.setZero(POSE_SIZE * msize, POSE_SIZE * msize); + b.setZero(POSE_SIZE * msize); + + for (size_t i = 0; i < rld.order.size(); i++) { + const FrameRelLinData& frld = rld.Hpppl.at(i); + + H.block(POSE_SIZE * i, POSE_SIZE * i) += frld.Hpp; + b.segment(POSE_SIZE * i) += frld.bp; + + for (size_t j = 0; j < frld.lm_id.size(); j++) { + Eigen::Matrix H_pl_H_ll_inv; + int lm_id = frld.lm_id[j]; + + H_pl_H_ll_inv = frld.Hpl[j] * rld.Hll.at(lm_id); + b.segment(POSE_SIZE * i) -= H_pl_H_ll_inv * rld.bl.at(lm_id); + + const auto& other_obs = rld.lm_to_obs.at(lm_id); + for (size_t k = 0; k < other_obs.size(); k++) { + const FrameRelLinData& frld_other = rld.Hpppl.at(other_obs[k].first); + int other_i = other_obs[k].first; + + Eigen::Matrix H_l_p_other = + frld_other.Hpl[other_obs[k].second].transpose(); + + H.block(POSE_SIZE * i, POSE_SIZE * other_i) -= + H_pl_H_ll_inv * H_l_p_other; + } + } + } +} + +void BundleAdjustmentBase::get_current_points( + Eigen::vector& points, std::vector& ids) const { + points.clear(); + ids.clear(); + + for (const auto& kv_kpt : kpts) { + Eigen::Vector3d pt_cam = + StereographicParam::unproject(kv_kpt.second.dir).head<3>(); + pt_cam /= kv_kpt.second.id; + + Sophus::SE3d T_w_i; + + int64_t id = kv_kpt.second.kf_id.first; + if (frame_states.count(id) > 0) { + PoseVelBiasStateWithLin state = frame_states.at(id); + T_w_i = state.getState().T_w_i; + } else if (frame_poses.count(id) > 0) { + PoseStateWithLin state = frame_poses.at(id); + + T_w_i = state.getPose(); + } else { + std::cout << "Unknown frame id: " << id << std::endl; + std::abort(); + } + + const Sophus::SE3d& T_i_c = calib.T_i_c[kv_kpt.second.kf_id.second]; + + // std::cerr << "T_w_i\n" << T_w_i.matrix() << std::endl; + + points.emplace_back(T_w_i * T_i_c * pt_cam); + + ids.emplace_back(kv_kpt.first); + } +} + +Eigen::Vector4d BundleAdjustmentBase::triangulate(const Eigen::Vector3d& p0_3d, + const Eigen::Vector3d& p1_3d, + const Sophus::SE3d& T_0_1) { + Eigen::Vector3d p1_3d_unrotated = T_0_1.so3() * p1_3d; + Eigen::Vector2d b; + b[0] = T_0_1.translation().dot(p0_3d); + b[1] = T_0_1.translation().dot(p1_3d_unrotated); + Eigen::Matrix2d A; + A(0, 0) = p0_3d.dot(p0_3d); + A(1, 0) = p0_3d.dot(p1_3d_unrotated); + A(0, 1) = -A(1, 0); + A(1, 1) = -p1_3d_unrotated.dot(p1_3d_unrotated); + Eigen::Vector2d lambda = A.inverse() * b; + Eigen::Vector3d xm = lambda[0] * p0_3d; + Eigen::Vector3d xn = T_0_1.translation() + lambda[1] * p1_3d_unrotated; + + Eigen::Vector4d p0_triangulated; + p0_triangulated.head<3>() = (xm + xn) / 2; + p0_triangulated[3] = 0; + + return p0_triangulated; +} + +void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, + Eigen::VectorXd& abs_b, + const std::set& idx_to_keep, + const std::set& idx_to_marg, + Eigen::MatrixXd& marg_H, + Eigen::VectorXd& marg_b) { + int keep_size = idx_to_keep.size(); + int marg_size = idx_to_marg.size(); + + BASALT_ASSERT(keep_size + marg_size == abs_H.cols()); + + // Fill permutation matrix + Eigen::Matrix indices(idx_to_keep.size() + + idx_to_marg.size()); + + { + auto it = idx_to_keep.begin(); + for (size_t i = 0; i < idx_to_keep.size(); i++) { + indices[i] = *it; + it++; + } + } + + { + auto it = idx_to_marg.begin(); + for (size_t i = 0; i < idx_to_marg.size(); i++) { + indices[idx_to_keep.size() + i] = *it; + it++; + } + } + + const Eigen::PermutationWrapper> p( + indices); + + const Eigen::PermutationMatrix pt = p.transpose(); + + abs_b.applyOnTheLeft(pt); + abs_H.applyOnTheLeft(pt); + abs_H.applyOnTheRight(p); + + Eigen::MatrixXd H_mm_inv; + // H_mm_inv.setIdentity(marg_size, marg_size); + // abs_H.bottomRightCorner(marg_size, + // marg_size).ldlt().solveInPlace(H_mm_inv); + + H_mm_inv = abs_H.bottomRightCorner(marg_size, marg_size) + .fullPivLu() + .solve(Eigen::MatrixXd::Identity(marg_size, marg_size)); + + // H_mm_inv = abs_H.bottomRightCorner(marg_size, marg_size) + // .fullPivHouseholderQr() + // .solve(Eigen::MatrixXd::Identity(marg_size, marg_size)); + + abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv; + + marg_H = abs_H.topLeftCorner(keep_size, keep_size); + marg_b = abs_b.head(keep_size); + + marg_H -= abs_H.topRightCorner(keep_size, marg_size) * + abs_H.bottomLeftCorner(marg_size, keep_size); + marg_b -= abs_H.topRightCorner(keep_size, marg_size) * abs_b.tail(marg_size); + + abs_H.resize(0, 0); + abs_b.resize(0); +} +} diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp new file mode 100644 index 0000000..19da9b0 --- /dev/null +++ b/src/vi_estimator/keypoint_vio.cpp @@ -0,0 +1,1054 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include +#include + +#include + +#include +#include +#include + +#include + +namespace basalt { + +KeypointVioEstimator::KeypointVioEstimator( + int64_t t_ns, const Sophus::SE3d& T_w_i, const Eigen::Vector3d& vel_w_i, + const Eigen::Vector3d& bg, const Eigen::Vector3d& ba, double int_std_dev, + const Eigen::Vector3d& g, const basalt::Calibration& calib, + const VioConfig& config) + : take_kf(true), frames_after_kf(0), g(g), config(config) { + this->obs_std_dev = config.vio_obs_std_dev; + this->huber_thresh = config.vio_obs_huber_thresh; + this->calib = calib; + + last_state_t_ns = t_ns; + + imu_meas[t_ns] = IntegratedImuMeasurement(t_ns, bg, ba); + frame_states[t_ns] = + PoseVelBiasStateWithLin(t_ns, T_w_i, vel_w_i, bg, ba, true); + + // Setup marginalization + marg_H.setZero(POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE); + marg_b.setZero(POSE_VEL_BIAS_SIZE); + + // prior on position + marg_H.diagonal().head<3>().setConstant(prior_weight); + // prior on yaw + marg_H(5, 5) = prior_weight; + + // small prior to avoid jumps in bias + marg_H.diagonal().segment<3>(9).array() = 1e1; + marg_H.diagonal().segment<3>(12).array() = 1e2; + + std::cout << "marg_H\n" << marg_H << std::endl; + + marg_order.abs_order_map[t_ns] = std::make_pair(0, POSE_VEL_BIAS_SIZE); + marg_order.total_size = POSE_VEL_BIAS_SIZE; + marg_order.items = 1; + + gyro_bias_weight.setConstant(1.0 / + (calib.gyro_bias_std * calib.gyro_bias_std)); + accel_bias_weight.setConstant(1.0 / + (calib.accel_bias_std * calib.accel_bias_std)); + + max_states = std::numeric_limits::max(); + max_kfs = std::numeric_limits::max(); + + opt_started = false; + + vision_data_queue.set_capacity(10); + imu_data_queue.set_capacity(300); + + auto proc_func = [&] { + OpticalFlowResult::Ptr prev_frame, curr_frame; + IntegratedImuMeasurement::Ptr meas; + + ImuData::Ptr data; + imu_data_queue.pop(data); + + while (true) { + vision_data_queue.pop(curr_frame); + + if (!curr_frame.get()) { + break; + } + + if (prev_frame) { + // preintegrate measurements + + auto last_state = frame_states.at(last_state_t_ns); + + meas.reset(new IntegratedImuMeasurement( + prev_frame->t_ns, last_state.getState().bias_gyro, + last_state.getState().bias_accel)); + + while (data->t_ns <= prev_frame->t_ns) { + imu_data_queue.pop(data); + + if (!data.get()) break; + } + + while (data->t_ns <= curr_frame->t_ns) { + meas->integrate(*data); + imu_data_queue.pop(data); + if (!data.get()) break; + } + + if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) { + int64_t tmp = data->t_ns; + data->t_ns = curr_frame->t_ns; + meas->integrate(*data); + data->t_ns = tmp; + } + } + + measure(curr_frame, meas); + prev_frame = curr_frame; + } + + if (out_vis_queue) out_vis_queue->push(nullptr); + if (out_marg_queue) out_marg_queue->push(nullptr); + if (out_state_queue) out_state_queue->push(nullptr); + + finished = true; + + std::cout << "Finished VIOFilter " << std::endl; + }; + + processing_thread.reset(new std::thread(proc_func)); +} + +void KeypointVioEstimator::addIMUToQueue(const ImuData::Ptr& data) { + imu_data_queue.emplace(data); +} + +void KeypointVioEstimator::addVisionToQueue( + const OpticalFlowResult::Ptr& data) { + vision_data_queue.push(data); +} + +bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, + const IntegratedImuMeasurement::Ptr& meas) { + if (meas.get()) { + BASALT_ASSERT(frame_states[last_state_t_ns].getState().t_ns == + meas->get_start_t_ns()); + BASALT_ASSERT(opt_flow_meas->t_ns == + meas->get_dt_ns() + meas->get_start_t_ns()); + + PoseVelBiasState next_state = frame_states.at(last_state_t_ns).getState(); + + meas->predictState(frame_states.at(last_state_t_ns).getState(), g, + next_state); + + last_state_t_ns = opt_flow_meas->t_ns; + next_state.t_ns = opt_flow_meas->t_ns; + + frame_states[last_state_t_ns] = next_state; + + imu_meas[meas->get_start_t_ns()] = *meas; + } + + // save results + prev_opt_flow_res[opt_flow_meas->t_ns] = opt_flow_meas; + + // Make new residual for existing keypoints + int connected0 = 0; + std::map num_points_connected; + std::unordered_set unconnected_obs0; + for (size_t i = 0; i < opt_flow_meas->observations.size(); i++) { + TimeCamId tcid_target(opt_flow_meas->t_ns, i); + + for (const auto& kv_obs : opt_flow_meas->observations[i]) { + int kpt_id = kv_obs.first; + + auto it = kpts.find(kpt_id); + if (it != kpts.end()) { + const TimeCamId& tcid_host = it->second.kf_id; + + KeypointObservation kobs; + kobs.kpt_id = kpt_id; + kobs.pos = kv_obs.second.translation().cast(); + + obs[tcid_host][tcid_target].push_back(kobs); + + if (num_points_connected.count(tcid_host.first) == 0) { + num_points_connected[tcid_host.first] = 0; + } + num_points_connected[tcid_host.first]++; + + if (i == 0) connected0++; + } else { + if (i == 0) { + unconnected_obs0.emplace(kpt_id); + } + } + } + } + + if (double(connected0) / (connected0 + unconnected_obs0.size()) < + config.vio_new_kf_keypoints_thresh && + frames_after_kf > config.vio_min_frames_after_kf) + take_kf = true; + + if (config.vio_debug) { + std::cout << "connected0 " << connected0 << " unconnected0 " + << unconnected_obs0.size() << std::endl; + } + + if (take_kf) { + // Triangulate new points from stereo and make keyframe for camera 0 + take_kf = false; + frames_after_kf = 0; + kf_ids.emplace(last_state_t_ns); + + TimeCamId tcidl(opt_flow_meas->t_ns, 0); + + int num_points_added = 0; + for (int lm_id : unconnected_obs0) { + // Find all observations + std::map kp_obs; + + for (const auto& kv : prev_opt_flow_res) { + for (size_t k = 0; k < kv.second->observations.size(); k++) { + auto it = kv.second->observations[k].find(lm_id); + if (it != kv.second->observations[k].end()) { + TimeCamId tcido(kv.first, k); + + KeypointObservation kobs; + kobs.kpt_id = lm_id; + kobs.pos = it->second.translation().cast(); + + // obs[tcidl][tcido].push_back(kobs); + kp_obs[tcido] = kobs; + } + } + } + + // triangulate + bool valid_kp = false; + for (const auto& kv_obs : kp_obs) { + if (valid_kp) break; + TimeCamId tcido = kv_obs.first; + + const Eigen::Vector2d p0 = opt_flow_meas->observations.at(0) + .at(lm_id) + .translation() + .cast(); + const Eigen::Vector2d p1 = prev_opt_flow_res[tcido.first] + ->observations[tcido.second] + .at(lm_id) + .translation() + .cast(); + + Eigen::Vector4d p0_3d, p1_3d; + bool valid1 = calib.intrinsics[0].unproject(p0, p0_3d); + bool valid2 = calib.intrinsics[tcido.second].unproject(p1, p1_3d); + if (!valid1 || !valid2) continue; + + Sophus::SE3d T_i0_i1 = + getPoseStateWithLin(tcidl.first).getPose().inverse() * + getPoseStateWithLin(tcido.first).getPose(); + Sophus::SE3d T_0_1 = + calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.second]; + + if (T_0_1.translation().squaredNorm() < 0.03 * 0.03) continue; + + Eigen::Vector4d p0_triangulated = + triangulate(p0_3d.head<3>(), p1_3d.head<3>(), T_0_1); + + if (p0_triangulated[2] > 0) { + KeypointPosition kpt_pos; + kpt_pos.kf_id = tcidl; + kpt_pos.dir = StereographicParam::project(p0_triangulated); + kpt_pos.id = 1.0 / p0_triangulated.norm(); + + if (kpt_pos.id > 0 && kpt_pos.id < 10) { + kpts[lm_id] = kpt_pos; + + num_points_added++; + valid_kp = true; + } + } + } + + if (valid_kp) { + for (const auto& kv_obs : kp_obs) { + obs[tcidl][kv_obs.first].push_back(kv_obs.second); + } + } + } + + num_points_kf[opt_flow_meas->t_ns] = num_points_added; + } else { + frames_after_kf++; + } + + optimize(); + marginalize(num_points_connected); + + if (out_state_queue) { + PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns); + + PoseVelBiasState::Ptr data(new PoseVelBiasState(p.getState())); + + out_state_queue->push(data); + } + + if (out_vis_queue) { + VioVisualizationData::Ptr data(new VioVisualizationData); + + data->t_ns = last_state_t_ns; + + for (const auto& kv : frame_states) { + data->states.emplace_back(kv.second.getState().T_w_i); + } + + for (const auto& kv : frame_poses) { + data->frames.emplace_back(kv.second.getPose()); + } + + get_current_points(data->points, data->point_ids); + + data->projections.resize(opt_flow_meas->observations.size()); + computeProjections(data->projections); + + out_vis_queue->push(data); + } + + last_processed_t_ns = last_state_t_ns; + + return true; +} + +Eigen::VectorXd KeypointVioEstimator::checkNullspace( + const Eigen::MatrixXd& H, const Eigen::VectorXd& b, + const AbsOrderMap& order, + const Eigen::map& frame_states, + const Eigen::map& frame_poses) { + BASALT_ASSERT(size_t(H.cols()) == order.total_size); + size_t marg_size = order.total_size; + + Eigen::VectorXd inc_x, inc_y, inc_z, inc_roll, inc_pitch, inc_yaw; + inc_x.setZero(marg_size); + inc_y.setZero(marg_size); + inc_z.setZero(marg_size); + inc_roll.setZero(marg_size); + inc_pitch.setZero(marg_size); + inc_yaw.setZero(marg_size); + + int num_trans = 0; + Eigen::Vector3d mean_trans; + mean_trans.setZero(); + + // Compute mean translation + for (const auto& kv : order.abs_order_map) { + Eigen::Vector3d trans; + if (kv.second.second == POSE_SIZE) { + mean_trans += frame_poses.at(kv.first).getPoseLin().translation(); + num_trans++; + } else if (kv.second.second == POSE_VEL_BIAS_SIZE) { + mean_trans += frame_states.at(kv.first).getStateLin().T_w_i.translation(); + num_trans++; + } else { + std::cerr << "Unknown size of the state: " << kv.second.second + << std::endl; + std::abort(); + } + } + mean_trans /= num_trans; + + double eps = 0.01; + + // Compute nullspace increments + for (const auto& kv : order.abs_order_map) { + inc_x(kv.second.first + 0) = eps; + inc_y(kv.second.first + 1) = eps; + inc_z(kv.second.first + 2) = eps; + inc_roll(kv.second.first + 3) = eps; + inc_pitch(kv.second.first + 4) = eps; + inc_yaw(kv.second.first + 5) = eps; + + Eigen::Vector3d trans; + if (kv.second.second == POSE_SIZE) { + trans = frame_poses.at(kv.first).getPoseLin().translation(); + } else if (kv.second.second == POSE_VEL_BIAS_SIZE) { + trans = frame_states.at(kv.first).getStateLin().T_w_i.translation(); + } else { + BASALT_ASSERT(false); + } + + trans -= mean_trans; + + Eigen::Matrix3d J = -Sophus::SO3d::hat(trans); + J *= eps; + + inc_roll.segment<3>(kv.second.first) = J.col(0); + inc_pitch.segment<3>(kv.second.first) = J.col(1); + inc_yaw.segment<3>(kv.second.first) = J.col(2); + + if (kv.second.second == POSE_VEL_BIAS_SIZE) { + Eigen::Vector3d vel = frame_states.at(kv.first).getStateLin().vel_w_i; + Eigen::Matrix3d J_vel = -Sophus::SO3d::hat(vel); + J_vel *= eps; + + inc_roll.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(0); + inc_pitch.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(1); + inc_yaw.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(2); + } + } + + inc_x.normalize(); + inc_y.normalize(); + inc_z.normalize(); + inc_roll.normalize(); + inc_pitch.normalize(); + inc_yaw.normalize(); + + // std::cout << "inc_x " << inc_x.transpose() << std::endl; + // std::cout << "inc_y " << inc_y.transpose() << std::endl; + // std::cout << "inc_z " << inc_z.transpose() << std::endl; + // std::cout << "inc_yaw " << inc_yaw.transpose() << std::endl; + + Eigen::VectorXd inc_random; + inc_random.setRandom(marg_size); + inc_random.normalize(); + + Eigen::VectorXd xHx(7), xb(7); + xHx[0] = 0.5 * inc_x.transpose() * H * inc_x; + xHx[1] = 0.5 * inc_y.transpose() * H * inc_y; + xHx[2] = 0.5 * inc_z.transpose() * H * inc_z; + xHx[3] = 0.5 * inc_roll.transpose() * H * inc_roll; + xHx[4] = 0.5 * inc_pitch.transpose() * H * inc_pitch; + xHx[5] = 0.5 * inc_yaw.transpose() * H * inc_yaw; + xHx[6] = 0.5 * inc_random.transpose() * H * inc_random; + + xb[0] = inc_x.transpose() * b; + xb[1] = inc_y.transpose() * b; + xb[2] = inc_z.transpose() * b; + xb[3] = inc_roll.transpose() * b; + xb[4] = inc_pitch.transpose() * b; + xb[5] = inc_yaw.transpose() * b; + xb[6] = inc_random.transpose() * b; + + std::cout << "nullspace x_trans: " << xHx[0] << " + " << xb[0] << std::endl; + std::cout << "nullspace y_trans: " << xHx[1] << " + " << xb[1] << std::endl; + std::cout << "nullspace z_trans: " << xHx[2] << " + " << xb[2] << std::endl; + std::cout << "nullspace roll : " << xHx[3] << " + " << xb[3] << std::endl; + std::cout << "nullspace pitch : " << xHx[4] << " + " << xb[4] << std::endl; + std::cout << "nullspace yaw : " << xHx[5] << " + " << xb[5] << std::endl; + std::cout << "nullspace random : " << xHx[6] << " + " << xb[6] << std::endl; + + return xHx + xb; +} + +void KeypointVioEstimator::checkMargNullspace() const { + checkNullspace(marg_H, marg_b, marg_order, frame_states, frame_poses); +} + +void KeypointVioEstimator::marginalize( + const std::map& num_points_connected) { + if (!opt_started) return; + + if (frame_poses.size() > max_kfs || frame_states.size() >= max_states) { + // Marginalize + + const int states_to_remove = frame_states.size() - max_states + 1; + + auto it = frame_states.cbegin(); + for (int i = 0; i < states_to_remove; i++) it++; + int64_t last_state_to_marg = it->first; + + AbsOrderMap aom; + + // remove all frame_poses that are not kfs + std::set poses_to_marg; + for (const auto& kv : frame_poses) { + aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE); + + if (kf_ids.count(kv.first) == 0) poses_to_marg.emplace(kv.first); + + // Check that we have the same order as marginalization + BASALT_ASSERT(marg_order.abs_order_map.at(kv.first) == + aom.abs_order_map.at(kv.first)); + + aom.total_size += POSE_SIZE; + aom.items++; + } + + std::set states_to_marg_vel_bias; + std::set states_to_marg_all; + for (const auto& kv : frame_states) { + if (kv.first > last_state_to_marg) break; + + if (kv.first != last_state_to_marg) { + if (kf_ids.count(kv.first) > 0) { + states_to_marg_vel_bias.emplace(kv.first); + } else { + states_to_marg_all.emplace(kv.first); + } + } + + aom.abs_order_map[kv.first] = + std::make_pair(aom.total_size, POSE_VEL_BIAS_SIZE); + + // Check that we have the same order as marginalization + if (aom.items < marg_order.abs_order_map.size()) + BASALT_ASSERT(marg_order.abs_order_map.at(kv.first) == + aom.abs_order_map.at(kv.first)); + + aom.total_size += POSE_VEL_BIAS_SIZE; + aom.items++; + } + + auto kf_ids_all = kf_ids; + std::set kfs_to_marg; + while (kf_ids.size() > max_kfs && !states_to_marg_vel_bias.empty()) { + int64_t id_to_marg = -1; + + { + std::vector ids; + for (int64_t id : kf_ids) { + ids.push_back(id); + } + + for (size_t i = 0; i < ids.size() - 2; i++) { + if (num_points_connected.count(ids[i]) == 0 || + (num_points_connected.at(ids[i]) / num_points_kf.at(ids[i]) < + 0.05)) { + id_to_marg = ids[i]; + break; + } + } + } + + if (id_to_marg < 0) { + std::vector ids; + for (int64_t id : kf_ids) { + ids.push_back(id); + } + + int64_t last_kf = *kf_ids.crbegin(); + double min_score = std::numeric_limits::max(); + int64_t min_score_id = -1; + + for (size_t i = 0; i < ids.size() - 2; i++) { + double denom = 0; + for (size_t j = 0; j < ids.size() - 2; j++) { + denom += 1 / ((frame_poses.at(ids[i]).getPose().translation() - + frame_poses.at(ids[j]).getPose().translation()) + .norm() + + 1e-5); + } + + double score = + std::sqrt( + (frame_poses.at(ids[i]).getPose().translation() - + frame_states.at(last_kf).getState().T_w_i.translation()) + .norm()) * + denom; + + if (score < min_score) { + min_score_id = ids[i]; + min_score = score; + } + } + + id_to_marg = min_score_id; + } + + kfs_to_marg.emplace(id_to_marg); + poses_to_marg.emplace(id_to_marg); + + kf_ids.erase(id_to_marg); + } + + // std::cout << "marg order" << std::endl; + // aom.print_order(); + + // std::cout << "marg prior order" << std::endl; + // marg_order.print_order(); + + if (config.vio_debug) { + std::cout << "states_to_remove " << states_to_remove << std::endl; + std::cout << "poses_to_marg.size() " << poses_to_marg.size() << std::endl; + std::cout << "states_to_marg.size() " << states_to_marg_all.size() + << std::endl; + std::cout << "state_to_marg_vel_bias.size() " + << states_to_marg_vel_bias.size() << std::endl; + std::cout << "kfs_to_marg.size() " << kfs_to_marg.size() << std::endl; + } + + size_t asize = aom.total_size; + + double marg_prior_error; + double imu_error, bg_error, ba_error; + + DenseAccumulator accum; + accum.reset(asize); + + { + // Linearize points + + Eigen::map>> + obs_to_lin; + + for (auto it = obs.cbegin(); it != obs.cend();) { + if (kfs_to_marg.count(it->first.first) > 0) { + for (auto it2 = it->second.cbegin(); it2 != it->second.cend(); + ++it2) { + if (it2->first.first <= last_state_to_marg) + obs_to_lin[it->first].emplace(*it2); + } + it = obs.erase(it); + } else { + ++it; + } + } + + double rld_error; + Eigen::vector rld_vec; + + linearizeHelper(rld_vec, obs_to_lin, rld_error); + + for (auto& rld : rld_vec) { + rld.invert_keypoint_hessians(); + + Eigen::MatrixXd rel_H; + Eigen::VectorXd rel_b; + linearizeRel(rld, rel_H, rel_b); + + linearizeAbs(rel_H, rel_b, rld, aom, accum); + } + + // remove points + for (auto it = kpts.cbegin(); it != kpts.cend();) { + if (kfs_to_marg.count(it->second.kf_id.first) > 0) { + it = kpts.erase(it); + } else { + ++it; + } + } + } + + linearizeAbsIMU(aom, accum.getH(), accum.getB(), imu_error, bg_error, + ba_error, frame_states, imu_meas, gyro_bias_weight, + accel_bias_weight, g); + linearizeMargPrior(aom, accum.getH(), accum.getB(), marg_prior_error); + + // Save marginalization prior + if (out_marg_queue && !kfs_to_marg.empty()) { + // int64_t kf_id = *kfs_to_marg.begin(); + + { + MargData::Ptr m(new MargData); + m->aom = aom; + m->abs_H = accum.getH(); + m->abs_b = accum.getB(); + m->frame_poses = frame_poses; + m->frame_states = frame_states; + m->kfs_all = kf_ids_all; + m->kfs_to_marg = kfs_to_marg; + + for (int64_t t : m->kfs_all) { + m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t)); + } + + out_marg_queue->push(m); + } + } + + std::set idx_to_keep, idx_to_marg; + for (const auto& kv : aom.abs_order_map) { + if (kv.second.second == POSE_SIZE) { + int start_idx = kv.second.first; + if (poses_to_marg.count(kv.first) == 0) { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_keep.emplace(start_idx + i); + } else { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_marg.emplace(start_idx + i); + } + } else { + BASALT_ASSERT(kv.second.second == POSE_VEL_BIAS_SIZE); + // state + int start_idx = kv.second.first; + if (states_to_marg_all.count(kv.first) > 0) { + for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_marg.emplace(start_idx + i); + } else if (states_to_marg_vel_bias.count(kv.first) > 0) { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_keep.emplace(start_idx + i); + for (size_t i = POSE_SIZE; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_marg.emplace(start_idx + i); + } else { + BASALT_ASSERT(kv.first == last_state_to_marg); + for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_keep.emplace(start_idx + i); + } + } + } + + if (config.vio_debug) { + std::cout << "keeping " << idx_to_keep.size() << " marg " + << idx_to_marg.size() << " total " << asize << std::endl; + std::cout << "last_state_to_marg " << last_state_to_marg + << " frame_poses " << frame_poses.size() << " frame_states " + << frame_states.size() << std::endl; + } + + Eigen::MatrixXd marg_H_new; + Eigen::VectorXd marg_b_new; + marginalizeHelper(accum.getH(), accum.getB(), idx_to_keep, idx_to_marg, + marg_H_new, marg_b_new); + + { + BASALT_ASSERT(frame_states.at(last_state_to_marg).isLinearized() == + false); + frame_states.at(last_state_to_marg).setLinTrue(); + } + + for (const int64_t id : states_to_marg_all) { + frame_states.erase(id); + imu_meas.erase(id); + prev_opt_flow_res.erase(id); + } + + for (const int64_t id : states_to_marg_vel_bias) { + const PoseVelBiasStateWithLin& state = frame_states.at(id); + PoseStateWithLin pose(state); + + frame_poses[id] = pose; + frame_states.erase(id); + imu_meas.erase(id); + } + + for (const int64_t id : poses_to_marg) { + frame_poses.erase(id); + prev_opt_flow_res.erase(id); + } + + for (auto it = obs.begin(); it != obs.end(); ++it) { + for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) { + if (poses_to_marg.count(it2->first.first) > 0 || + states_to_marg_all.count(it2->first.first) > 0) { + it2 = it->second.erase(it2); + } else { + ++it2; + } + } + } + + AbsOrderMap marg_order_new; + + for (const auto& kv : frame_poses) { + marg_order_new.abs_order_map[kv.first] = + std::make_pair(marg_order_new.total_size, POSE_SIZE); + + marg_order_new.total_size += POSE_SIZE; + marg_order_new.items++; + } + + { + marg_order_new.abs_order_map[last_state_to_marg] = + std::make_pair(marg_order_new.total_size, POSE_VEL_BIAS_SIZE); + marg_order_new.total_size += POSE_VEL_BIAS_SIZE; + marg_order_new.items++; + } + + marg_H = marg_H_new; + marg_b = marg_b_new; + marg_order = marg_order_new; + + BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size); + + Eigen::VectorXd delta; + computeDelta(marg_order, delta); + marg_b -= marg_H * delta; + + if (config.vio_debug) { + std::cout << "marginalizaon done!!" << std::endl; + + std::cout << "======== Marg nullspace ==========" << std::endl; + checkMargNullspace(); + std::cout << "=================================" << std::endl; + } + + // std::cout << "new marg prior order" << std::endl; + // marg_order.print_order(); + } +} + +void KeypointVioEstimator::computeDelta(const AbsOrderMap& marg_order, + Eigen::VectorXd& delta) const { + size_t marg_size = marg_order.total_size; + delta.setZero(marg_size); + for (const auto& kv : marg_order.abs_order_map) { + if (kv.second.second == POSE_SIZE) { + BASALT_ASSERT(frame_poses.at(kv.first).isLinearized()); + delta.segment(kv.second.first) = + frame_poses.at(kv.first).getDelta(); + } else if (kv.second.second == POSE_VEL_BIAS_SIZE) { + BASALT_ASSERT(frame_states.at(kv.first).isLinearized()); + delta.segment(kv.second.first) = + frame_states.at(kv.first).getDelta(); + } else { + BASALT_ASSERT(false); + } + } +} + +void KeypointVioEstimator::optimize() { + if (config.vio_debug) { + std::cout << "=================================" << std::endl; + } + + if (opt_started || frame_states.size() > 4) { + // Optimize + opt_started = true; + + AbsOrderMap aom; + + for (const auto& kv : frame_poses) { + aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE); + + // Check that we have the same order as marginalization + BASALT_ASSERT(marg_order.abs_order_map.at(kv.first) == + aom.abs_order_map.at(kv.first)); + + aom.total_size += POSE_SIZE; + aom.items++; + } + + for (const auto& kv : frame_states) { + aom.abs_order_map[kv.first] = + std::make_pair(aom.total_size, POSE_VEL_BIAS_SIZE); + + // Check that we have the same order as marginalization + if (aom.items < marg_order.abs_order_map.size()) + BASALT_ASSERT(marg_order.abs_order_map.at(kv.first) == + aom.abs_order_map.at(kv.first)); + + aom.total_size += POSE_VEL_BIAS_SIZE; + aom.items++; + } + + // std::cout << "opt order" << std::endl; + // aom.print_order(); + + // std::cout << "marg prior order" << std::endl; + // marg_order.print_order(); + + for (int iter = 0; iter < config.vio_max_iterations; iter++) { + auto t1 = std::chrono::high_resolution_clock::now(); + + double rld_error; + Eigen::vector rld_vec; + linearizeHelper(rld_vec, obs, rld_error); + + BundleAdjustmentBase::LinearizeAbsReduce> lopt( + aom); + + tbb::blocked_range::iterator> range( + rld_vec.begin(), rld_vec.end()); + + tbb::parallel_reduce(range, lopt); + + double marg_prior_error = 0; + double imu_error = 0, bg_error = 0, ba_error = 0; + linearizeAbsIMU(aom, lopt.accum.getH(), lopt.accum.getB(), imu_error, + bg_error, ba_error, frame_states, imu_meas, + gyro_bias_weight, accel_bias_weight, g); + linearizeMargPrior(aom, lopt.accum.getH(), lopt.accum.getB(), + marg_prior_error); + + double error_total = + rld_error + imu_error + marg_prior_error + ba_error + bg_error; + + lopt.accum.getH().diagonal() *= 1.0001; + const Eigen::VectorXd inc = lopt.accum.solve(); + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // apply increment to states + for (auto& kv : frame_states) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); + + if (config.vio_debug) { + double after_update_marg_prior_error = 0; + double after_update_vision_error = 0, after_update_imu_error = 0, + after_bg_error = 0, after_ba_error = 0; + + computeError(after_update_vision_error); + computeImuError(aom, after_update_imu_error, after_bg_error, + after_ba_error, frame_states, imu_meas, + gyro_bias_weight, accel_bias_weight, g); + computeMargPriorError(after_update_marg_prior_error); + + double after_error_total = + after_update_vision_error + after_update_imu_error + + after_update_marg_prior_error + after_bg_error + after_ba_error; + + double error_diff = error_total - after_error_total; + + auto t2 = std::chrono::high_resolution_clock::now(); + + auto elapsed = + std::chrono::duration_cast(t2 - t1); + + std::cout << "iter " << iter + << " before_update_error: vision: " << rld_error + << " imu: " << imu_error << " bg_error: " << bg_error + << " ba_error: " << ba_error + << " marg_prior: " << marg_prior_error + << " total: " << error_total << std::endl; + + std::cout << "iter " << iter << " after_update_error: vision: " + << after_update_vision_error + << " imu: " << after_update_imu_error + << " bg_error: " << after_bg_error + << " ba_error: " << after_ba_error + << " marg prior: " << after_update_marg_prior_error + << " total: " << after_error_total << " max_inc " + << inc.array().abs().maxCoeff() << " error_diff " + << error_diff << " time : " << elapsed.count() + << "(us), num_states " << frame_states.size() + << " num_poses " << frame_poses.size() << std::endl; + + if (after_error_total > error_total) { + std::cout << "increased error after update!!!" << std::endl; + } + } + + if (inc.array().abs().maxCoeff() < 1e-4) break; + + // std::cerr << "LT\n" << LT << std::endl; + // std::cerr << "z_p\n" << z_p.transpose() << std::endl; + // std::cerr << "inc\n" << inc.transpose() << std::endl; + } + } + + if (config.vio_debug) { + std::cout << "=================================" << std::endl; + } +} + +void KeypointVioEstimator::computeProjections( + std::vector>& data) const { + for (const auto& kv : obs) { + const TimeCamId& tcid_h = kv.first; + + for (const auto& obs_kv : kv.second) { + const TimeCamId& tcid_t = obs_kv.first; + + if (tcid_t.first != last_state_t_ns) continue; + + if (tcid_h != tcid_t) { + PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.first); + PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first); + + Sophus::SE3d T_t_h_sophus = + computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.second], + state_t.getPose(), calib.T_i_c[tcid_t.second]); + + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + FrameRelLinData rld; + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = kpts.at(kpt_obs.kpt_id); + + Eigen::Vector2d res; + Eigen::Vector4d proj; + + linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res, nullptr, + nullptr, &proj); + + proj[3] = kpt_obs.kpt_id; + data[tcid_t.second].emplace_back(proj); + } + }, + calib.intrinsics[tcid_t.second].variant); + + } else { + // target and host are the same + // residual does not depend on the pose + // it just depends on the point + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = kpts.at(kpt_obs.kpt_id); + + Eigen::Vector2d res; + Eigen::Vector4d proj; + + linearizePoint(kpt_obs, kpt_pos, Eigen::Matrix4d::Identity(), + cam, res, nullptr, nullptr, &proj); + + proj[3] = kpt_obs.kpt_id; + data[tcid_t.second].emplace_back(proj); + } + }, + calib.intrinsics[tcid_t.second].variant); + } + } + } +} + +} // namespace basalt diff --git a/src/vi_estimator/keypoint_vio_linearize.cpp b/src/vi_estimator/keypoint_vio_linearize.cpp new file mode 100644 index 0000000..8a68501 --- /dev/null +++ b/src/vi_estimator/keypoint_vio_linearize.cpp @@ -0,0 +1,266 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +namespace basalt { + +void KeypointVioEstimator::linearizeMargPrior(const AbsOrderMap& aom, + Eigen::MatrixXd& abs_H, + Eigen::VectorXd& abs_b, + double& marg_prior_error) const { + // Assumed to be in the top left corner + + BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size); + + size_t marg_size = marg_order.total_size; + abs_H.topLeftCorner(marg_size, marg_size) += marg_H; + + Eigen::VectorXd delta; + computeDelta(marg_order, delta); + + abs_b.head(marg_size) += marg_b; + abs_b.head(marg_size) += marg_H * delta; + + marg_prior_error = 0.5 * delta.transpose() * marg_H * delta; + marg_prior_error += delta.transpose() * marg_b; +} + +void KeypointVioEstimator::computeMargPriorError( + double& marg_prior_error) const { + BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size); + + Eigen::VectorXd delta; + computeDelta(marg_order, delta); + + marg_prior_error = 0.5 * delta.transpose() * marg_H * delta; + marg_prior_error += delta.transpose() * marg_b; +} + +void KeypointVioEstimator::linearizeAbsIMU( + const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, + double& imu_error, double& bg_error, double& ba_error, + const Eigen::map& states, + const Eigen::map& imu_meas, + const Eigen::Vector3d& gyro_bias_weight, + const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { + imu_error = 0; + bg_error = 0; + ba_error = 0; + for (const auto& kv : imu_meas) { + if (kv.second.get_dt_ns() != 0) { + int64_t start_t = kv.second.get_start_t_ns(); + int64_t end_t = kv.second.get_start_t_ns() + kv.second.get_dt_ns(); + + if (aom.abs_order_map.count(start_t) == 0 || + aom.abs_order_map.count(end_t) == 0) + continue; + + const size_t start_idx = aom.abs_order_map.at(start_t).first; + const size_t end_idx = aom.abs_order_map.at(end_t).first; + + PoseVelBiasStateWithLin start_state = states.at(start_t); + PoseVelBiasStateWithLin end_state = states.at(end_t); + + IntegratedImuMeasurement::MatNN d_res_d_start, d_res_d_end; + IntegratedImuMeasurement::MatN3 d_res_d_bg, d_res_d_ba; + + PoseVelState::VecN res = kv.second.residual( + start_state.getStateLin(), g, end_state.getStateLin(), + start_state.getStateLin().bias_gyro, + start_state.getStateLin().bias_accel, &d_res_d_start, &d_res_d_end, + &d_res_d_bg, &d_res_d_ba); + + if (start_state.isLinearized() || end_state.isLinearized()) { + res = + kv.second.residual(start_state.getState(), g, end_state.getState(), + start_state.getState().bias_gyro, + start_state.getState().bias_accel); + } + + // error + imu_error += 0.5 * res.transpose() * kv.second.get_cov_inv() * res; + + // states + abs_H.block<9, 9>(start_idx, start_idx) += + d_res_d_start.transpose() * kv.second.get_cov_inv() * d_res_d_start; + abs_H.block<9, 9>(start_idx, end_idx) += + d_res_d_start.transpose() * kv.second.get_cov_inv() * d_res_d_end; + abs_H.block<9, 9>(end_idx, start_idx) += + d_res_d_end.transpose() * kv.second.get_cov_inv() * d_res_d_start; + abs_H.block<9, 9>(end_idx, end_idx) += + d_res_d_end.transpose() * kv.second.get_cov_inv() * d_res_d_end; + + abs_b.segment<9>(start_idx) += + d_res_d_start.transpose() * kv.second.get_cov_inv() * res; + abs_b.segment<9>(end_idx) += + d_res_d_end.transpose() * kv.second.get_cov_inv() * res; + + // bias + IntegratedImuMeasurement::MatN6 d_res_d_bga; + d_res_d_bga.topLeftCorner<9, 3>() = d_res_d_bg; + d_res_d_bga.topRightCorner<9, 3>() = d_res_d_ba; + + abs_H.block<6, 6>(start_idx + 9, start_idx + 9) += + d_res_d_bga.transpose() * kv.second.get_cov_inv() * d_res_d_bga; + + abs_H.block<9, 6>(start_idx, start_idx + 9) += + d_res_d_start.transpose() * kv.second.get_cov_inv() * d_res_d_bga; + + abs_H.block<9, 6>(end_idx, start_idx + 9) += + d_res_d_end.transpose() * kv.second.get_cov_inv() * d_res_d_bga; + + abs_H.block<6, 9>(start_idx + 9, start_idx) += + d_res_d_bga.transpose() * kv.second.get_cov_inv() * d_res_d_start; + + abs_H.block<6, 9>(start_idx + 9, end_idx) += + d_res_d_bga.transpose() * kv.second.get_cov_inv() * d_res_d_end; + + abs_b.segment<6>(start_idx + 9) += + d_res_d_bga.transpose() * kv.second.get_cov_inv() * res; + + // difference between biases + double dt = kv.second.get_dt_ns() * 1e-9; + { + Eigen::Vector3d gyro_bias_weight_dt = gyro_bias_weight / dt; + + // std::cerr << "gyro_bias_weight_dt " << + // gyro_bias_weight_dt.transpose() + // << std::endl; + + Eigen::Vector3d res_bg = + start_state.getState().bias_gyro - end_state.getState().bias_gyro; + + abs_H.block<3, 3>(start_idx + 9, start_idx + 9) += + gyro_bias_weight_dt.asDiagonal(); + abs_H.block<3, 3>(end_idx + 9, end_idx + 9) += + gyro_bias_weight_dt.asDiagonal(); + + abs_H.block<3, 3>(end_idx + 9, start_idx + 9) -= + gyro_bias_weight_dt.asDiagonal(); + abs_H.block<3, 3>(start_idx + 9, end_idx + 9) -= + gyro_bias_weight_dt.asDiagonal(); + + abs_b.segment<3>(start_idx + 9) += + gyro_bias_weight_dt.asDiagonal() * res_bg; + abs_b.segment<3>(end_idx + 9) -= + gyro_bias_weight_dt.asDiagonal() * res_bg; + + bg_error += 0.5 * res_bg.transpose() * + gyro_bias_weight_dt.asDiagonal() * res_bg; + } + + { + Eigen::Vector3d accel_bias_weight_dt = accel_bias_weight / dt; + Eigen::Vector3d res_ba = + start_state.getState().bias_accel - end_state.getState().bias_accel; + + abs_H.block<3, 3>(start_idx + 12, start_idx + 12) += + accel_bias_weight_dt.asDiagonal(); + abs_H.block<3, 3>(end_idx + 12, end_idx + 12) += + accel_bias_weight_dt.asDiagonal(); + abs_H.block<3, 3>(end_idx + 12, start_idx + 12) -= + accel_bias_weight_dt.asDiagonal(); + abs_H.block<3, 3>(start_idx + 12, end_idx + 12) -= + accel_bias_weight_dt.asDiagonal(); + + abs_b.segment<3>(start_idx + 12) += + accel_bias_weight_dt.asDiagonal() * res_ba; + abs_b.segment<3>(end_idx + 12) -= + accel_bias_weight_dt.asDiagonal() * res_ba; + + ba_error += 0.5 * res_ba.transpose() * + accel_bias_weight_dt.asDiagonal() * res_ba; + } + } + } +} + +void KeypointVioEstimator::computeImuError( + const AbsOrderMap& aom, double& imu_error, double& bg_error, + double& ba_error, + const Eigen::map& states, + const Eigen::map& imu_meas, + const Eigen::Vector3d& gyro_bias_weight, + const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { + imu_error = 0; + bg_error = 0; + ba_error = 0; + for (const auto& kv : imu_meas) { + if (kv.second.get_dt_ns() != 0) { + int64_t start_t = kv.second.get_start_t_ns(); + int64_t end_t = kv.second.get_start_t_ns() + kv.second.get_dt_ns(); + + if (aom.abs_order_map.count(start_t) == 0 || + aom.abs_order_map.count(end_t) == 0) + continue; + + PoseVelBiasStateWithLin start_state = states.at(start_t); + PoseVelBiasStateWithLin end_state = states.at(end_t); + + const PoseVelState::VecN res = kv.second.residual( + start_state.getState(), g, end_state.getState(), + start_state.getState().bias_gyro, start_state.getState().bias_accel); + + // std::cout << "res: (" << start_t << "," << end_t << ") " + // << res.transpose() << std::endl; + + // std::cerr << "cov_inv:\n" << kv.second.get_cov_inv() << + // std::endl; + + imu_error += 0.5 * res.transpose() * kv.second.get_cov_inv() * res; + + double dt = kv.second.get_dt_ns() * 1e-9; + { + Eigen::Vector3d gyro_bias_weight_dt = gyro_bias_weight / dt; + Eigen::Vector3d res_bg = + start_state.getState().bias_gyro - end_state.getState().bias_gyro; + + bg_error += 0.5 * res_bg.transpose() * + gyro_bias_weight_dt.asDiagonal() * res_bg; + } + + { + Eigen::Vector3d accel_bias_weight_dt = accel_bias_weight / dt; + Eigen::Vector3d res_ba = + start_state.getState().bias_accel - end_state.getState().bias_accel; + + ba_error += 0.5 * res_ba.transpose() * + accel_bias_weight_dt.asDiagonal() * res_ba; + } + } + } +} +} // namespace basalt diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp new file mode 100644 index 0000000..863ce72 --- /dev/null +++ b/src/vi_estimator/nfr_mapper.cpp @@ -0,0 +1,675 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include +#include +#include +#include +#include + +#include + +namespace basalt { + +NfrMapper::NfrMapper(const Calibration& calib, const VioConfig& config, + const std::string& vocabulary) + : config(config) { + this->calib = calib; + this->obs_std_dev = config.mapper_obs_std_dev; + this->huber_thresh = config.mapper_obs_huber_thresh; + + if (!vocabulary.empty()) { + DBoW3::Vocabulary voc(vocabulary); + bow_database.setVocabulary(voc); + } +} + +void NfrMapper::addMargData(MargData::Ptr& data) { + for (const auto& kv : data->frame_poses) { + PoseStateWithLin p(kv.second.getT_ns(), kv.second.getPose()); + + frame_poses[kv.first] = p; + } + + for (const auto& kv : data->frame_states) { + if (data->kfs_all.count(kv.first) > 0) { + auto state = kv.second; + PoseStateWithLin p(state.getState().t_ns, state.getState().T_w_i); + frame_poses[kv.first] = p; + } + } + + processMargData(*data); + extractNonlinearFactors(*data); +} + +void NfrMapper::processMargData(MargData& m) { + BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols())); + + // std::cout << "rank " << m.abs_H.fullPivLu().rank() << " size " + // << m.abs_H.cols() << std::endl; + + AbsOrderMap aom_new; + std::set idx_to_keep; + std::set idx_to_marg; + + for (const auto& kv : m.aom.abs_order_map) { + if (kv.second.second == POSE_SIZE) { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_keep.emplace(kv.second.first + i); + aom_new.abs_order_map.emplace(kv); + aom_new.total_size += POSE_SIZE; + } else if (kv.second.second == POSE_VEL_BIAS_SIZE) { + if (m.kfs_all.count(kv.first) > 0) { + for (size_t i = 0; i < POSE_SIZE; i++) + idx_to_keep.emplace(kv.second.first + i); + for (size_t i = POSE_SIZE; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_marg.emplace(kv.second.first + i); + + aom_new.abs_order_map[kv.first] = + std::make_pair(aom_new.total_size, POSE_SIZE); + aom_new.total_size += POSE_SIZE; + + PoseStateWithLin p = m.frame_states.at(kv.first); + m.frame_poses[kv.first] = p; + m.frame_states.erase(kv.first); + } else { + for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++) + idx_to_marg.emplace(kv.second.first + i); + m.frame_states.erase(kv.first); + } + } else { + std::cerr << "Unknown size" << std::endl; + std::abort(); + } + + // std::cout << kv.first << " " << kv.second.first << " " << + // kv.second.second + // << std::endl; + } + + Eigen::MatrixXd marg_H_new; + Eigen::VectorXd marg_b_new; + BundleAdjustmentBase::marginalizeHelper(m.abs_H, m.abs_b, idx_to_keep, + idx_to_marg, marg_H_new, marg_b_new); + + // std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size " + // << marg_H_new.cols() << std::endl; + + m.abs_H = marg_H_new; + m.abs_b = marg_b_new; + m.aom = aom_new; + + BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols())); + + // save image data + { + for (const auto& v : m.opt_flow_res) { + img_data[v->t_ns] = v->input_images; + } + } +} + +void NfrMapper::extractNonlinearFactors(MargData& m) { + size_t asize = m.aom.total_size; + // std::cout << "asize " << asize << std::endl; + + Eigen::MatrixXd cov_old; + cov_old.setIdentity(asize, asize); + m.abs_H.ldlt().solveInPlace(cov_old); + + int64_t kf_id = *m.kfs_to_marg.cbegin(); + int kf_start_idx = m.aom.abs_order_map.at(kf_id).first; + + auto state_kf = m.frame_poses.at(kf_id); + + Sophus::SE3d T_w_i_kf = state_kf.getPose(); + + Eigen::Vector3d pos = T_w_i_kf.translation(); + Eigen::Vector3d yaw_dir_body = + T_w_i_kf.so3().inverse() * Eigen::Vector3d::UnitX(); + + Sophus::Matrix d_pos_d_T_w_i; + Sophus::Matrix d_yaw_d_T_w_i; + Sophus::Matrix d_rp_d_T_w_i; + + absPositionError(T_w_i_kf, pos, &d_pos_d_T_w_i); + yawError(T_w_i_kf, yaw_dir_body, &d_yaw_d_T_w_i); + rollPitchError(T_w_i_kf, T_w_i_kf.so3(), &d_rp_d_T_w_i); + + { + Eigen::MatrixXd J; + J.setZero(POSE_SIZE, asize); + J.block<3, POSE_SIZE>(0, kf_start_idx) = d_pos_d_T_w_i; + J.block<1, POSE_SIZE>(3, kf_start_idx) = d_yaw_d_T_w_i; + J.block<2, POSE_SIZE>(4, kf_start_idx) = d_rp_d_T_w_i; + + Sophus::Matrix6d cov_new = J * cov_old * J.transpose(); + + // std::cout << "cov_new\n" << cov_new << std::endl; + + RollPitchFactor rpf; + rpf.t_ns = kf_id; + rpf.R_w_i_meas = T_w_i_kf.so3(); + rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse(); + + roll_pitch_factors.emplace_back(rpf); + } + + for (int64_t other_id : m.kfs_all) { + if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) { + continue; + } + + auto state_o = m.frame_poses.at(other_id); + + Sophus::SE3d T_w_i_o = state_o.getPose(); + Sophus::SE3d T_kf_o = T_w_i_kf.inverse() * T_w_i_o; + + int o_start_idx = m.aom.abs_order_map.at(other_id).first; + + Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j; + relPoseError(T_kf_o, T_w_i_kf, T_w_i_o, &d_res_d_T_w_i, &d_res_d_T_w_j); + + Eigen::MatrixXd J; + J.setZero(POSE_SIZE, asize); + J.block(0, kf_start_idx) = d_res_d_T_w_i; + J.block(0, o_start_idx) = d_res_d_T_w_j; + + Sophus::Matrix6d cov_new = J * cov_old * J.transpose(); + RelPoseFactor rpf; + rpf.t_i_ns = kf_id; + rpf.t_j_ns = other_id; + rpf.T_i_j = T_kf_o; + rpf.cov_inv.setIdentity(); + cov_new.ldlt().solveInPlace(rpf.cov_inv); + + // std::cout << "rpf.cov_inv\n" << rpf.cov_inv << std::endl; + + rel_pose_factors.emplace_back(rpf); + } +} + +void NfrMapper::optimize(int num_iterations) { + AbsOrderMap aom; + + for (const auto& kv : frame_poses) { + aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE); + aom.total_size += POSE_SIZE; + } + + for (int iter = 0; iter < num_iterations; iter++) { + auto t1 = std::chrono::high_resolution_clock::now(); + + double rld_error; + Eigen::vector rld_vec; + linearizeHelper(rld_vec, obs, rld_error); + + // SparseHashAccumulator accum; + // accum.reset(aom.total_size); + + // for (auto& rld : rld_vec) { + // rld.invert_keypoint_hessians(); + + // Eigen::MatrixXd rel_H; + // Eigen::VectorXd rel_b; + // linearizeRel(rld, rel_H, rel_b); + + // linearizeAbs(rel_H, rel_b, rld, aom, accum); + // } + + MapperLinearizeAbsReduce> lopt(aom, + &frame_poses); + tbb::blocked_range::iterator> range( + rld_vec.begin(), rld_vec.end()); + tbb::blocked_range::const_iterator> range1( + roll_pitch_factors.begin(), roll_pitch_factors.end()); + tbb::blocked_range::const_iterator> range2( + rel_pose_factors.begin(), rel_pose_factors.end()); + + tbb::parallel_reduce(range, lopt); + tbb::parallel_reduce(range1, lopt); + tbb::parallel_reduce(range2, lopt); + + double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error; + + std::cout << "iter " << iter + << " before_update_error: vision: " << rld_error + << " rel_error: " << lopt.rel_error + << " roll_pitch_error: " << lopt.roll_pitch_error + << " total: " << error_total << std::endl; + + lopt.accum.iterative_solver = true; + lopt.accum.print_info = true; + + const Eigen::VectorXd inc = lopt.accum.solve(); + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + BASALT_ASSERT(!kv.second.isLinearized()); + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); + + double after_update_vision_error = 0; + double after_rel_error = 0; + double after_roll_pitch_error = 0; + + computeError(after_update_vision_error); + computeRelPose(after_rel_error); + computeRollPitch(after_roll_pitch_error); + + double after_error_total = + after_update_vision_error + after_rel_error + after_roll_pitch_error; + + double error_diff = error_total - after_error_total; + + auto t2 = std::chrono::high_resolution_clock::now(); + + auto elapsed = + std::chrono::duration_cast(t2 - t1); + + std::cout << "iter " << iter + << " after_update_error: vision: " << after_update_vision_error + << " rel_error: " << after_rel_error + << " roll_pitch_error: " << after_roll_pitch_error + << " total: " << after_error_total << " max_inc " + << inc.array().abs().maxCoeff() << " error_diff " << error_diff + << " time : " << elapsed.count() << "(us), num_states " + << frame_states.size() << " num_poses " << frame_poses.size() + << std::endl; + + if (after_error_total > error_total) { + std::cout << "increased error after update!!!" << std::endl; + } + + if (inc.array().abs().maxCoeff() < 1e-4) break; + + // std::cerr << "LT\n" << LT << std::endl; + // std::cerr << "z_p\n" << z_p.transpose() << std::endl; + // std::cerr << "inc\n" << inc.transpose() << std::endl; + } +} + +Eigen::map& NfrMapper::getFramePoses() { + return frame_poses; +} + +void NfrMapper::computeRelPose(double& rel_error) { + rel_error = 0; + + for (const RelPoseFactor& rpf : rel_pose_factors) { + const Sophus::SE3d& pose_i = frame_poses.at(rpf.t_i_ns).getPose(); + const Sophus::SE3d& pose_j = frame_poses.at(rpf.t_j_ns).getPose(); + + Sophus::Vector6d res = relPoseError(rpf.T_i_j, pose_i, pose_j); + + rel_error += res.transpose() * rpf.cov_inv * res; + } +} + +void NfrMapper::computeRollPitch(double& roll_pitch_error) { + roll_pitch_error = 0; + + for (const RollPitchFactor& rpf : roll_pitch_factors) { + const Sophus::SE3d& pose = frame_poses.at(rpf.t_ns).getPose(); + + Sophus::Vector2d res = rollPitchError(pose, rpf.R_w_i_meas); + + roll_pitch_error += res.transpose() * rpf.cov_inv * res; + } +} + +void NfrMapper::detect_keypoints() { + std::vector keys; + for (const auto& kv : img_data) { + keys.emplace_back(kv.first); + } + + auto t1 = std::chrono::high_resolution_clock::now(); + + tbb::parallel_for( + tbb::blocked_range(0, keys.size()), + [&](const tbb::blocked_range& r) { + for (size_t j = r.begin(); j != r.end(); ++j) { + auto kv = img_data.find(keys[j]); + if (kv->second.get()) { + for (size_t i = 0; i < kv->second->img_data.size(); i++) { + TimeCamId tcid(kv->first, i); + KeypointsData kd; + + if (!kv->second->img_data[i].img.get()) continue; + + const Image img = + kv->second->img_data[i].img->Reinterpret(); + + detectKeypointsMapping(img, kd, + config.mapper_detection_num_points); + computeAngles(img, kd, true); + computeDescriptors(img, kd); + + std::vector success; + calib.intrinsics[tcid.second].unproject(kd.corners, kd.corners_3d, + success); + + feature_corners[tcid] = kd; + + auto& bow = bow_data[tcid]; + + if (bow_database.usingDirectIndex()) { + bow_database.getVocabulary()->transform( + kd.corner_descriptors, bow.first, bow.second, + bow_database.getDirectIndexLevels()); + } else { + bow_database.getVocabulary()->transform(kd.corner_descriptors, + bow.first); + } + } + } + } + }); + + auto t2 = std::chrono::high_resolution_clock::now(); + + for (const auto& kv : bow_data) { + int bow_id; + if (bow_database.usingDirectIndex()) { + bow_id = bow_database.add(kv.second.first, kv.second.second); + } else { + bow_id = bow_database.add(kv.second.first); + } + bow_id_to_tcid[bow_id] = kv.first; + } + + auto t3 = std::chrono::high_resolution_clock::now(); + + auto elapsed1 = + std::chrono::duration_cast(t2 - t1); + auto elapsed2 = + std::chrono::duration_cast(t3 - t2); + + std::cout << "Processed " << feature_corners.size() << " frames." + << std::endl; + + std::cout << "Detection time: " << elapsed1.count() * 1e-6 + << "s. Adding to DB time: " << elapsed2.count() * 1e-6 << "s." + << std::endl; +} + +void NfrMapper::match_stereo() { + // Pose of camera 1 (right) w.r.t camera 0 (left) + const Sophus::SE3d T_0_1 = calib.T_i_c[0].inverse() * calib.T_i_c[1]; + + // Essential matrix + Eigen::Matrix4d E; + computeEssential(T_0_1, E); + + std::cout << "Matching " << img_data.size() << " stereo pairs..." + << std::endl; + + int num_matches = 0; + int num_inliers = 0; + + for (const auto& kv : img_data) { + const TimeCamId tcid1(kv.first, 0), tcid2(kv.first, 1); + + MatchData md; + md.T_i_j = T_0_1; + + const KeypointsData& kd1 = feature_corners[tcid1]; + const KeypointsData& kd2 = feature_corners[tcid2]; + + matchDescriptors(kd1.corner_descriptors, kd2.corner_descriptors, md.matches, + config.mapper_max_hamming_distance, + config.mapper_second_best_test_ratio); + + num_matches += md.matches.size(); + + findInliersEssential(kd1, kd2, E, 1e-3, md); + + if (md.inliers.size() > 16) { + num_inliers += md.inliers.size(); + feature_matches[std::make_pair(tcid1, tcid2)] = md; + } + } + + std::cout << "Matched " << img_data.size() << " stereo pairs with " + << num_inliers << " inlier matches (" << num_matches << " total)." + << std::endl; +} + +void NfrMapper::match_all() { + std::vector keys; + std::unordered_map id_to_key_idx; + + for (const auto& kv : feature_corners) { + id_to_key_idx[kv.first] = keys.size(); + keys.push_back(kv.first); + } + + auto t1 = std::chrono::high_resolution_clock::now(); + + struct match_pair { + size_t i; + size_t j; + double score; + }; + + tbb::concurrent_vector ids_to_match; + + tbb::blocked_range keys_range(0, keys.size()); + auto compute_pairs = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + DBoW3::QueryResults q; + + auto it = bow_data.find(keys[i]); + + if (it != bow_data.end()) { + bow_database.query(it->second.first, q, + config.mapper_num_frames_to_match); + + for (const auto& r : q) { + // Match only previous frames + + size_t j = id_to_key_idx.at(bow_id_to_tcid.at(r.Id)); + if (r.Score > config.mapper_frames_to_match_threshold && + keys[i].first < keys[j].first) { + match_pair m; + m.i = i; + m.j = j; + m.score = r.Score; + + ids_to_match.emplace_back(m); + } + } + } + } + }; + + tbb::parallel_for(keys_range, compute_pairs); + + auto t2 = std::chrono::high_resolution_clock::now(); + + std::cout << "Matching " << ids_to_match.size() << " image pairs..." + << std::endl; + + std::atomic total_matched = 0; + + tbb::blocked_range range(0, ids_to_match.size()); + auto match_func = [&](const tbb::blocked_range& r) { + int matched = 0; + + for (size_t j = r.begin(); j != r.end(); ++j) { + const TimeCamId& id1 = keys[ids_to_match[j].i]; + const TimeCamId& id2 = keys[ids_to_match[j].j]; + + const KeypointsData& f1 = feature_corners[id1]; + const KeypointsData& f2 = feature_corners[id2]; + + MatchData md; + + matchDescriptors(f1.corner_descriptors, f2.corner_descriptors, md.matches, + 70, 1.2); + + if (int(md.matches.size()) > config.mapper_min_matches) { + matched++; + + findInliersRansac(f1, f2, config.mapper_ransac_threshold, + config.mapper_min_matches, md); + } + + if (!md.inliers.empty()) feature_matches[std::make_pair(id1, id2)] = md; + } + total_matched += matched; + }; + + tbb::parallel_for(range, match_func); + // match_func(range); + + auto t3 = std::chrono::high_resolution_clock::now(); + + auto elapsed1 = + std::chrono::duration_cast(t2 - t1); + auto elapsed2 = + std::chrono::duration_cast(t3 - t2); + + // + int num_matches = 0; + int num_inliers = 0; + + for (const auto& kv : feature_matches) { + num_matches += kv.second.matches.size(); + num_inliers += kv.second.inliers.size(); + } + + std::cout << "Matched " << ids_to_match.size() << " image pairs with " + << num_inliers << " inlier matches (" << num_matches << " total)." + << std::endl; + + std::cout << "DB query " << elapsed1.count() * 1e-6 << "s. matching " + << elapsed2.count() * 1e-6 + << "s. Geometric verification attemts: " << total_matched << "." + << std::endl; +} + +void NfrMapper::build_tracks() { + TrackBuilder trackBuilder; + // Build: Efficient fusion of correspondences + trackBuilder.Build(feature_matches); + // Filter: Remove tracks that have conflict + trackBuilder.Filter(config.mapper_min_track_length); + // Export tree to usable data structure + trackBuilder.Export(feature_tracks); + + // info + size_t inlier_match_count = 0; + for (const auto& it : feature_matches) { + inlier_match_count += it.second.inliers.size(); + } + + size_t total_track_obs_count = 0; + for (const auto& it : feature_tracks) { + total_track_obs_count += it.second.size(); + } + + std::cout << "Built " << feature_tracks.size() << " feature tracks from " + << inlier_match_count << " matches. Average track length is " + << total_track_obs_count / (double)feature_tracks.size() << "." + << std::endl; +} + +void NfrMapper::setup_opt() { + for (const auto& kv : feature_tracks) { + if (kv.second.size() < 2) continue; + + // Take first observation as host + auto it = kv.second.begin(); + TimeCamId tcid_h = it->first; + + FeatureId feat_id_h = it->second; + Eigen::Vector2d pos_2d_h = feature_corners.at(tcid_h).corners[feat_id_h]; + Eigen::Vector4d pos_3d_h; + calib.intrinsics[tcid_h.second].unproject(pos_2d_h, pos_3d_h); + + it++; + + for (; it != kv.second.end(); it++) { + TimeCamId tcid_o = it->first; + + FeatureId feat_id_o = it->second; + Eigen::Vector2d pos_2d_o = feature_corners.at(tcid_o).corners[feat_id_o]; + Eigen::Vector4d pos_3d_o; + calib.intrinsics[tcid_o.second].unproject(pos_2d_o, pos_3d_o); + + Sophus::SE3d T_w_h = + frame_poses.at(tcid_h.first).getPose() * calib.T_i_c[tcid_h.second]; + Sophus::SE3d T_w_o = + frame_poses.at(tcid_o.first).getPose() * calib.T_i_c[tcid_o.second]; + + Eigen::Vector4d pos_3d = triangulate( + pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_w_h.inverse() * T_w_o); + + if (pos_3d[2] < 0.5 || pos_3d.norm() < 0.5) continue; + + KeypointPosition pos; + pos.kf_id = tcid_h; + pos.dir = StereographicParam::project(pos_3d); + pos.id = 1.0 / pos_3d.norm(); + + kpts[kv.first] = pos; + + for (const auto& obs_kv : kv.second) { + KeypointObservation ko; + ko.kpt_id = kv.first; + ko.pos = feature_corners.at(obs_kv.first).corners[obs_kv.second]; + + obs[tcid_h][obs_kv.first].emplace_back(ko); + } + break; + } + } +} + +} // namespace basalt diff --git a/src/vi_estimator/vio_estimator.cpp b/src/vi_estimator/vio_estimator.cpp new file mode 100644 index 0000000..dfd10b2 --- /dev/null +++ b/src/vi_estimator/vio_estimator.cpp @@ -0,0 +1,153 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include + +#include + +namespace basalt { + +VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator( + const VioConfig& config, const Calibration& cam, int64_t t_ns, + const Sophus::SE3d& T_w_i, const Eigen::Vector3d& vel_w_i, + const Eigen::Vector3d& bg, const Eigen::Vector3d& ba, double int_std_dev, + const Eigen::Vector3d& g) { + KeypointVioEstimator::Ptr res; + + res.reset(new KeypointVioEstimator(t_ns, T_w_i, vel_w_i, bg, ba, int_std_dev, + g, cam, config)); + + res->setMaxKfs(config.vio_max_kfs); + res->setMaxStates(config.vio_max_states); + + return res; +} + +double alignSVD(const std::vector& filter_t_ns, + const Eigen::vector& filter_t_w_i, + const std::vector& gt_t_ns, + Eigen::vector& gt_t_w_i) { + Eigen::vector est_associations; + Eigen::vector gt_associations; + + for (size_t i = 0; i < filter_t_w_i.size(); i++) { + int64_t t_ns = filter_t_ns[i]; + + size_t j; + for (j = 0; j < gt_t_ns.size(); j++) { + if (gt_t_ns.at(j) > t_ns) break; + } + j--; + + if (j >= gt_t_ns.size() - 1) { + continue; + } + + double dt_ns = t_ns - gt_t_ns.at(j); + double int_t_ns = gt_t_ns.at(j + 1) - gt_t_ns.at(j); + + BASALT_ASSERT_STREAM(dt_ns >= 0, "dt_ns " << dt_ns); + BASALT_ASSERT_STREAM(int_t_ns > 0, "int_t_ns " << int_t_ns); + + // Skip if the interval between gt larger than 100ms + if (int_t_ns > 1e8) continue; + + double ratio = dt_ns / int_t_ns; + + BASALT_ASSERT(ratio >= 0); + BASALT_ASSERT(ratio < 1); + + Eigen::Vector3d gt = (1 - ratio) * gt_t_w_i[j] + ratio * gt_t_w_i[j + 1]; + + gt_associations.emplace_back(gt); + est_associations.emplace_back(filter_t_w_i[i]); + } + + int num_kfs = est_associations.size(); + + Eigen::Matrix gt, est; + gt.setZero(3, num_kfs); + est.setZero(3, num_kfs); + + for (size_t i = 0; i < est_associations.size(); i++) { + gt.col(i) = gt_associations[i]; + est.col(i) = est_associations[i]; + } + + Eigen::Vector3d mean_gt = gt.rowwise().mean(); + Eigen::Vector3d mean_est = est.rowwise().mean(); + + gt.colwise() -= mean_gt; + est.colwise() -= mean_est; + + Eigen::Matrix3d cov = gt * est.transpose(); + + Eigen::JacobiSVD svd( + cov, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix3d S; + S.setIdentity(); + + if (svd.matrixU().determinant() * svd.matrixV().determinant() < 0) + S(2, 2) = -1; + + Eigen::Matrix3d rot_gt_est = svd.matrixU() * S * svd.matrixV().transpose(); + Eigen::Vector3d trans = mean_gt - rot_gt_est * mean_est; + + Sophus::SE3d T_gt_est(rot_gt_est, trans); + Sophus::SE3d T_est_gt = T_gt_est.inverse(); + + for (size_t i = 0; i < gt_t_w_i.size(); i++) { + gt_t_w_i[i] = T_est_gt * gt_t_w_i[i]; + } + + double error = 0; + for (size_t i = 0; i < est_associations.size(); i++) { + est_associations[i] = T_gt_est * est_associations[i]; + Eigen::Vector3d res = est_associations[i] - gt_associations[i]; + + error += res.transpose() * res; + } + + error /= est_associations.size(); + error = std::sqrt(error); + + std::cout << "T_align\n" << T_gt_est.matrix() << std::endl; + std::cout << "error " << error << std::endl; + std::cout << "number of associations " << num_kfs << std::endl; + + return error; +} +} // namespace basalt diff --git a/src/vio.cpp b/src/vio.cpp new file mode 100644 index 0000000..2e4583f --- /dev/null +++ b/src/vio.cpp @@ -0,0 +1,682 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +// GUI functions +void draw_image_overlay(pangolin::View& v, size_t cam_id); +void draw_scene(); +void load_data(const std::string& calib_path); +bool next_step(); +bool prev_step(); +void draw_plots(); +void alignButton(); + +// Pangolin variables +constexpr int UI_WIDTH = 200; + +using Button = pangolin::Var>; + +pangolin::DataLog imu_data_log, vio_data_log, error_data_log; +pangolin::Plotter* plotter; + +pangolin::Var show_frame("ui.show_frame", 0, 0, 1500); + +pangolin::Var show_obs("ui.show_obs", true, false, true); +pangolin::Var show_ids("ui.show_ids", false, false, true); + +pangolin::Var show_est_pos("ui.show_est_pos", true, false, true); +pangolin::Var show_est_vel("ui.show_est_vel", false, false, true); +pangolin::Var show_est_bg("ui.show_est_bg", false, false, true); +pangolin::Var show_est_ba("ui.show_est_ba", false, false, true); + +pangolin::Var show_gt("ui.show_gt", true, false, true); + +Button next_step_btn("ui.next_step", &next_step); +Button prev_step_btn("ui.prev_step", &prev_step); + +pangolin::Var continue_btn("ui.continue_btn", false, false, true); +pangolin::Var continue_fast("ui.continue_fast", true, false, true); + +Button align_svd_btn("ui.align_svd", &alignButton); + +pangolin::Var follow("ui.follow", true, false, true); + +// Visualization variables +std::unordered_map vis_map; + +tbb::concurrent_bounded_queue out_vis_queue; +tbb::concurrent_bounded_queue out_state_queue; + +std::vector vio_t_ns; +Eigen::vector vio_t_w_i; + +std::vector gt_t_ns; +Eigen::vector gt_t_w_i; + +std::string marg_data_path; +size_t last_frame_processed = 0; + +tbb::concurrent_unordered_map timestamp_to_id; + +std::mutex m; +std::condition_variable cv; +bool step_by_step = false; + +// VIO variables +basalt::Calibration calib; + +basalt::VioDatasetPtr vio_dataset; +basalt::VioConfig vio_config; +basalt::OpticalFlowBase::Ptr opt_flow_ptr; +basalt::VioEstimatorBase::Ptr vio; + +// Feed functions +void feed_images() { + std::cout << "Started input_data thread " << std::endl; + + for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { + if (step_by_step) { + std::unique_lock lk(m); + cv.wait(lk); + } + + basalt::OpticalFlowInput::Ptr data(new basalt::OpticalFlowInput); + + data->t_ns = vio_dataset->get_image_timestamps()[i]; + data->img_data = vio_dataset->get_image_data(data->t_ns); + + timestamp_to_id[data->t_ns] = i; + + opt_flow_ptr->input_queue.push(data); + } + + // Indicate the end of the sequence + opt_flow_ptr->input_queue.push(nullptr); + + std::cout << "Finished input_data thread " << std::endl; +} + +void feed_imu() { + for (size_t i = 0; i < vio_dataset->get_gyro_data().size(); i++) { + basalt::ImuData::Ptr data(new basalt::ImuData); + data->t_ns = vio_dataset->get_gyro_data()[i].timestamp_ns; + + data->accel = vio_dataset->get_accel_data()[i].data; + data->gyro = vio_dataset->get_gyro_data()[i].data; + + const double accel_noise_std = calib.dicreete_time_accel_noise_std(); + const double gyro_noise_std = calib.dicreete_time_gyro_noise_std(); + + data->accel_cov.setConstant(accel_noise_std * accel_noise_std); + data->gyro_cov.setConstant(gyro_noise_std * gyro_noise_std); + + vio->imu_data_queue.push(data); + } + vio->imu_data_queue.push(nullptr); +} + +int main(int argc, char** argv) { + bool show_gui = true; + bool print_queue = false; + std::string cam_calib_path; + std::string dataset_path; + std::string dataset_type; + std::string config_path; + std::string result_path; + int num_threads = 0; + + CLI::App app{"App description"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, + "Ground-truth camera calibration used for simulation.") + ->required(); + + app.add_option("--dataset-path", dataset_path, "Path to dataset.") + ->required(); + + app.add_option("--dataset-type", dataset_type, "Dataset type .") + ->required(); + + app.add_option("--marg-data", marg_data_path, + "Path to folder where marginalization data will be stored."); + + app.add_option("--print-queue", print_queue, "Print queue."); + app.add_option("--config-path", config_path, "Path to config file."); + app.add_option("--result-path", result_path, + "Path to result file where the system will write RMSE ATE."); + app.add_option("--num-threads", num_threads, "Number of threads."); + app.add_option("--step-by-step", step_by_step, "Path to config file."); + + if (num_threads > 0) { + tbb::task_scheduler_init init(num_threads); + } + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError& e) { + return app.exit(e); + } + + if (!config_path.empty()) { + vio_config.load(config_path); + } + + load_data(cam_calib_path); + + { + basalt::DatasetIoInterfacePtr dataset_io = + basalt::DatasetIoFactory::getDatasetIo(dataset_type); + + dataset_io->read(dataset_path); + + vio_dataset = dataset_io->get_data(); + vio_dataset->get_image_timestamps().erase( + vio_dataset->get_image_timestamps().begin()); + + show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1; + show_frame.Meta().gui_changed = true; + + opt_flow_ptr = + basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib); + + for (size_t i = 0; i < vio_dataset->get_gt_pose_data().size(); i++) { + gt_t_ns.push_back(vio_dataset->get_gt_timestamps()[i]); + gt_t_w_i.push_back(vio_dataset->get_gt_pose_data()[i].translation()); + } + } + + const int64_t start_t_ns = vio_dataset->get_image_timestamps().front(); + Sophus::SE3d T_w_i_init; + Eigen::Vector3d vel_w_i_init; + + { + int64_t t_init_ns = vio_dataset->get_image_timestamps()[0]; + + { + T_w_i_init.setQuaternion(Eigen::Quaterniond::FromTwoVectors( + vio_dataset->get_accel_data()[0].data, Eigen::Vector3d::UnitZ())); + + std::cout << "T_w_i_init\n" << T_w_i_init.matrix() << std::endl; + std::cout + << "accel_w " + << (T_w_i_init * vio_dataset->get_accel_data()[0].data).transpose() + << std::endl; + } + + vel_w_i_init.setZero(); + + std::cout << "Setting up filter: t_ns " << t_init_ns << std::endl; + std::cout << "T_w_i\n" << T_w_i_init.matrix() << std::endl; + std::cout << "vel_w_i " << vel_w_i_init.transpose() << std::endl; + + vio = basalt::VioEstimatorFactory::getVioEstimator( + vio_config, calib, t_init_ns, T_w_i_init, vel_w_i_init, + Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), 0.0001, + basalt::constants::g); + + opt_flow_ptr->output_queue = &vio->vision_data_queue; + if (show_gui) vio->out_vis_queue = &out_vis_queue; + vio->out_state_queue = &out_state_queue; + } + + basalt::MargDataSaver::Ptr marg_data_saver; + + if (!marg_data_path.empty()) { + marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path)); + vio->out_marg_queue = &marg_data_saver->in_marg_queue; + + // Save gt. + { + std::string p = marg_data_path + "/gt.cereal"; + std::ofstream os(p, std::ios::binary); + + { + cereal::BinaryOutputArchive archive(os); + archive(gt_t_ns); + archive(gt_t_w_i); + } + os.close(); + } + } + + vio_data_log.Clear(); + + std::thread t1(&feed_images); + std::thread t2(&feed_imu); + + std::shared_ptr t3; + + if (show_gui) + t3.reset(new std::thread([&]() { + basalt::VioVisualizationData::Ptr data; + + while (true) { + out_vis_queue.pop(data); + + if (data.get()) { + vis_map[data->t_ns] = data; + } else { + break; + } + } + + std::cout << "Finished t3" << std::endl; + })); + + std::thread t4([&]() { + basalt::PoseVelBiasState::Ptr data; + + while (true) { + out_state_queue.pop(data); + + if (!data.get()) break; + + int64_t t_ns = data->t_ns; + + // std::cerr << "t_ns " << t_ns << std::endl; + Sophus::SE3d T_w_i = data->T_w_i; + Eigen::Vector3d vel_w_i = data->vel_w_i; + Eigen::Vector3d bg = data->bias_gyro; + Eigen::Vector3d ba = data->bias_accel; + + vio_t_ns.emplace_back(data->t_ns); + vio_t_w_i.emplace_back(T_w_i.translation()); + + if (show_gui) { + std::vector vals; + vals.push_back((t_ns - start_t_ns) * 1e-9); + + for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]); + for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]); + for (int i = 0; i < 3; i++) vals.push_back(bg[i]); + for (int i = 0; i < 3; i++) vals.push_back(ba[i]); + + vio_data_log.Log(vals); + } + } + + std::cout << "Finished t4" << std::endl; + }); + + std::shared_ptr t5; + + if (print_queue) { + t5.reset(new std::thread([&]() { + while (true) { + std::cout << "opt_flow_ptr->input_queue " + << opt_flow_ptr->input_queue.size() + << " opt_flow_ptr->output_queue " + << opt_flow_ptr->output_queue->size() << " out_state_queue " + << out_state_queue.size() << std::endl; + sleep(1); + } + })); + } + + if (show_gui) { + pangolin::CreateWindowAndBind("Main", 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::View& img_view_display = + pangolin::CreateDisplay() + .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.4) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds( + 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + plotter = new pangolin::Plotter(&imu_data_log, 0.0, 100, -10.0, 10.0, 0.01f, + 0.01f); + plot_display.AddDisplay(*plotter); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < calib.intrinsics.size()) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display.AddDisplay(*iv); + iv->extern_draw_function = + std::bind(&draw_image_overlay, std::placeholders::_1, idx); + } + + Eigen::Vector3d cam_p(-0.5, -3, -5); + cam_p = T_w_i_init.so3() * calib.T_i_c[0].so3() * cam_p; + + pangolin::OpenGlRenderState camera( + pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), + pangolin::ModelViewLookAt(cam_p[0], cam_p[1], cam_p[2], 0, 0, 0, + pangolin::AxisZ)); + + pangolin::View& display3D = + pangolin::CreateDisplay() + .SetAspect(-640 / 480.0) + .SetBounds(0.4, 1.0, 0.4, 1.0) + .SetHandler(new pangolin::Handler3D(camera)); + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (follow) { + size_t frame_id = show_frame; + int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; + auto it = vis_map.find(t_ns); + + if (it != vis_map.end()) { + auto T_w_i = it->second->states.back(); + T_w_i.so3() = Sophus::SO3d(); + + camera.Follow(T_w_i.matrix()); + } + } + + display3D.Activate(camera); + glClearColor(1.0f, 1.0f, 1.0f, 1.0f); + + draw_scene(); + + img_view_display.Activate(); + + if (show_frame.GuiChanged()) { + for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) { + size_t frame_id = static_cast(show_frame); + int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id]; + + std::vector img_vec = + vio_dataset->get_image_data(timestamp); + + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + img_view[cam_id]->SetImage( + img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, + img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt); + } + + draw_plots(); + } + + if (show_est_vel.GuiChanged() || show_est_pos.GuiChanged() || + show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) { + draw_plots(); + } + + pangolin::FinishFrame(); + + if (continue_btn) { + if (!next_step()) + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + if (continue_fast) { + int64_t t_ns = vio->last_processed_t_ns; + if (timestamp_to_id.count(t_ns)) { + show_frame = timestamp_to_id[t_ns]; + show_frame.Meta().gui_changed = true; + } + + if (vio->finished) { + continue_fast = false; + } + } + } + } + + t1.join(); + t2.join(); + if (t3.get()) t3->join(); + t4.join(); + + if (!result_path.empty()) { + double error = basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); + + std::ofstream os(result_path); + os << error << std::endl; + os.close(); + } + + return 0; +} + +void draw_image_overlay(pangolin::View& v, size_t cam_id) { + // size_t frame_id = show_frame; + // basalt::TimeCamId tcid = + // std::make_pair(vio_dataset->get_image_timestamps()[frame_id], + // cam_id); + + size_t frame_id = show_frame; + auto it = vis_map.find(vio_dataset->get_image_timestamps()[frame_id]); + + if (show_obs) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (it != vis_map.end() && cam_id < it->second->projections.size()) { + const auto& points = it->second->projections[cam_id]; + + if (points.size() > 0) { + double min_id = points[0][2], max_id = points[0][2]; + + for (const auto& points2 : it->second->projections) + for (const auto& p : points2) { + min_id = std::min(min_id, p[2]); + max_id = std::max(max_id, p[2]); + } + + for (const auto& c : points) { + const float radius = 6.5; + + float r, g, b; + getcolor(c[2] - min_id, max_id - min_id, b, g, r); + glColor3f(r, g, b); + + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]); + } + } + + glColor3f(1.0, 0.0, 0.0); + pangolin::GlFont::I() + .Text("Tracked %d points", points.size()) + .Draw(5, 20); + } + } +} + +void draw_scene() { + glPointSize(3); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glColor3ubv(cam_color); + Eigen::vector sub_gt(vio_t_w_i.begin(), + vio_t_w_i.begin() + show_frame); + pangolin::glDrawLineStrip(sub_gt); + + glColor3ubv(gt_color); + if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i); + + size_t frame_id = show_frame; + int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; + auto it = vis_map.find(t_ns); + + if (it != vis_map.end()) { + for (const auto& p : it->second->states) + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f); + + for (const auto& p : it->second->frames) + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); + + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(), 2.0f, + cam_color, 0.1f); + + glColor3ubv(pose_color); + pangolin::glDrawPoints(it->second->points); + } + + pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); +} + +void load_data(const std::string& calib_path) { + std::ifstream os(calib_path, std::ios::binary); + + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras" + << std::endl; + + } else { + std::cerr << "could not load camera calibration " << calib_path + << std::endl; + std::abort(); + } +} + +bool next_step() { + if (show_frame < int(vio_dataset->get_image_timestamps().size()) - 1) { + show_frame = show_frame + 1; + show_frame.Meta().gui_changed = true; + cv.notify_one(); + return true; + } else { + return false; + } +} + +bool prev_step() { + if (show_frame > 1) { + show_frame = show_frame - 1; + show_frame.Meta().gui_changed = true; + return true; + } else { + return false; + } +} + +void draw_plots() { + plotter->ClearSeries(); + plotter->ClearMarkers(); + + if (show_est_pos) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "position x", &vio_data_log); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "position y", &vio_data_log); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "position z", &vio_data_log); + } + + if (show_est_vel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "velocity x", &vio_data_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "velocity y", &vio_data_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "velocity z", &vio_data_log); + } + + if (show_est_bg) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "gyro bias x", &vio_data_log); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "gyro bias y", &vio_data_log); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "gyro bias z", &vio_data_log); + } + + if (show_est_ba) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "accel bias x", &vio_data_log); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "accel bias y", + &vio_data_log); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "accel bias z", &vio_data_log); + } + + double t = vio_dataset->get_image_timestamps()[show_frame] * 1e-9; + plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal, + pangolin::Colour::White()); +} + +void alignButton() { basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); } diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp new file mode 100644 index 0000000..bacbd17 --- /dev/null +++ b/src/vio_sim.cpp @@ -0,0 +1,900 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. +*/ + + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +// GUI functions +void draw_image_overlay(pangolin::View& v, size_t cam_id); +void draw_scene(); +void load_data(const std::string& calib_path); +void gen_data(); +void compute_projections(); +void setup_vio(); +void draw_plots(); +bool next_step(); +void alignButton(); + +// Parameters for simulations +int NUM_POINTS = 1000; +double POINT_DIST = 10.0; +constexpr int NUM_FRAMES = 1000; +constexpr int CAM_FREQ = 20; +constexpr int IMU_FREQ = 200; + +static const int knot_time = 3; +static const double obs_std_dev = 0.5; +static const double accel_std_dev = 0.5; +static const double gyro_std_dev = 0.008; + +static const double accel_bias_std_dev = 0.00123; +static const double gyro_bias_std_dev = 0.000234; + +Eigen::Vector3d g(0, 0, -9.81); + +// std::random_device rd{}; +// std::mt19937 gen{rd()}; +std::mt19937 gen{1}; + +std::normal_distribution<> obs_noise_dist{0, obs_std_dev}; +std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev}; +std::normal_distribution<> accel_noise_dist{0, accel_std_dev}; + +std::normal_distribution<> gyro_bias_dist{0, gyro_bias_std_dev}; +std::normal_distribution<> accel_bias_dist{0, accel_bias_std_dev}; + +// Simulated data + +basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9)); + +Eigen::vector gt_points; +Eigen::vector gt_frame_T_w_i; +Eigen::vector gt_frame_t_w_i, vio_t_w_i; +std::vector gt_frame_t_ns; +Eigen::vector gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, + noisy_accel, noisy_gyro, gt_vel; +std::vector gt_imu_t_ns; + +std::map gt_observations; +std::map noisy_observations; + +std::string marg_data_path; + +// VIO vars +basalt::Calibration calib; +basalt::KeypointVioEstimator::Ptr vio; + +// Visualization vars +std::unordered_map vis_map; +tbb::concurrent_bounded_queue out_vis_queue; +tbb::concurrent_bounded_queue out_state_queue; + +std::vector images; + +// Pangolin vars +constexpr int UI_WIDTH = 200; +pangolin::DataLog imu_data_log, vio_data_log, error_data_log; +pangolin::Plotter* plotter; + +pangolin::Var show_frame("ui.show_frame", 0, 0, NUM_FRAMES - 1); + +pangolin::Var show_obs("ui.show_obs", true, false, true); +pangolin::Var show_obs_noisy("ui.show_obs_noisy", true, false, true); +pangolin::Var show_obs_vio("ui.show_obs_vio", true, false, true); + +pangolin::Var show_ids("ui.show_ids", false, false, true); + +pangolin::Var show_accel("ui.show_accel", false, false, true); +pangolin::Var show_gyro("ui.show_gyro", false, false, true); +pangolin::Var show_gt_vel("ui.show_gt_vel", false, false, true); +pangolin::Var show_gt_pos("ui.show_gt_pos", true, false, true); +pangolin::Var show_gt_bg("ui.show_gt_bg", false, false, true); +pangolin::Var show_gt_ba("ui.show_gt_ba", false, false, true); + +pangolin::Var show_est_vel("ui.show_est_vel", false, false, true); +pangolin::Var show_est_pos("ui.show_est_pos", true, false, true); +pangolin::Var show_est_bg("ui.show_est_bg", false, false, true); +pangolin::Var show_est_ba("ui.show_est_ba", false, false, true); + +using Button = pangolin::Var>; + +Button next_step_btn("ui.next_step", &next_step); + +pangolin::Var continue_btn("ui.continue_btn", true, false, true); + +Button align_step_btn("ui.align_button", &alignButton); + +int main(int argc, char** argv) { + srand(1); + + bool show_gui = true; + std::string cam_calib_path; + + CLI::App app{"App description"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, + "Ground-truth camera calibration used for simulation.") + ->required(); + + app.add_option("--marg-data", marg_data_path, + "Folder to store marginalization data.") + ->required(); + + app.add_option("--num-points", NUM_POINTS, "Number of points in simulation."); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError& e) { + return app.exit(e); + } + + basalt::MargDataSaver::Ptr mds; + if (!marg_data_path.empty()) { + mds.reset(new basalt::MargDataSaver(marg_data_path)); + } + + load_data(cam_calib_path); + gen_data(); + + setup_vio(); + + vio->out_vis_queue = &out_vis_queue; + vio->out_state_queue = &out_state_queue; + + if (mds.get()) { + vio->out_marg_queue = &mds->in_marg_queue; + } + + std::thread t0([&]() { + for (size_t i = 0; i < gt_imu_t_ns.size(); i++) { + basalt::ImuData::Ptr data(new basalt::ImuData); + data->t_ns = gt_imu_t_ns[i]; + + data->accel = noisy_accel[i]; + data->gyro = noisy_gyro[i]; + + data->accel_cov.setConstant(accel_std_dev * accel_std_dev); + data->gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + + vio->addIMUToQueue(data); + } + + vio->addIMUToQueue(nullptr); + + std::cout << "Finished t0" << std::endl; + }); + + std::thread t1([&]() { + for (size_t i = 0; i < gt_frame_t_ns.size(); i++) { + basalt::OpticalFlowResult::Ptr data(new basalt::OpticalFlowResult); + data->t_ns = gt_frame_t_ns[i]; + + for (size_t j = 0; j < calib.T_i_c.size(); j++) { + data->observations.emplace_back(); + basalt::TimeCamId tcid(data->t_ns, j); + const basalt::SimObservations& obs = noisy_observations.at(tcid); + for (size_t k = 0; k < obs.pos.size(); k++) { + Eigen::AffineCompact2f t; + t.setIdentity(); + t.translation() = obs.pos[k].cast(); + data->observations.back()[obs.id[k]] = t; + } + } + + vio->addVisionToQueue(data); + } + + vio->addVisionToQueue(nullptr); + + std::cout << "Finished t1" << std::endl; + }); + + std::thread t2([&]() { + basalt::VioVisualizationData::Ptr data; + + while (true) { + out_vis_queue.pop(data); + + if (data.get()) { + vis_map[data->t_ns] = data; + } else { + break; + } + } + + std::cout << "Finished t2" << std::endl; + }); + + std::thread t3([&]() { + basalt::PoseVelBiasState::Ptr data; + + while (true) { + out_state_queue.pop(data); + + if (!data.get()) break; + + int64_t t_ns = data->t_ns; + + // std::cerr << "t_ns " << t_ns << std::endl; + Sophus::SE3d T_w_i = data->T_w_i; + Eigen::Vector3d vel_w_i = data->vel_w_i; + Eigen::Vector3d bg = data->bias_gyro; + Eigen::Vector3d ba = data->bias_accel; + + vio_t_w_i.emplace_back(T_w_i.translation()); + + { + std::vector vals; + vals.push_back(t_ns * 1e-9); + + for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]); + for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]); + for (int i = 0; i < 3; i++) vals.push_back(bg[i]); + for (int i = 0; i < 3; i++) vals.push_back(ba[i]); + + vio_data_log.Log(vals); + } + } + + std::cout << "Finished t3" << std::endl; + }); + + // std::thread t4([&]() { + + // basalt::MargData::Ptr data; + + // while (true) { + // out_marg_queue.pop(data); + + // if (data.get()) { + // int64_t kf_id = *data->kfs_to_marg.begin(); + + // std::string path = cache_path + "/" + std::to_string(kf_id) + + // ".cereal"; + // std::ofstream os(path, std::ios::binary); + + // { + // cereal::BinaryOutputArchive archive(os); + // archive(*data); + // } + // os.close(); + + // } else { + // break; + // } + // } + + // std::cout << "Finished t4" << std::endl; + + // }); + + if (show_gui) { + pangolin::CreateWindowAndBind("Main", 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::View& img_view_display = + pangolin::CreateDisplay() + .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.5) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds( + 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + plotter = new pangolin::Plotter(&imu_data_log, 0.0, + gt_frame_t_ns[NUM_FRAMES - 1] * 1e-9, -10.0, + 10.0, 0.01f, 0.01f); + plot_display.AddDisplay(*plotter); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < calib.intrinsics.size()) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display.AddDisplay(*iv); + iv->extern_draw_function = + std::bind(&draw_image_overlay, std::placeholders::_1, idx); + } + + pangolin::OpenGlRenderState camera( + pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), + pangolin::ModelViewLookAt(15, 3, 15, 0, 0, 0, pangolin::AxisZ)); + + pangolin::View& display3D = + pangolin::CreateDisplay() + .SetAspect(-640 / 480.0) + .SetBounds(0.4, 1.0, 0.5, 1.0) + .SetHandler(new pangolin::Handler3D(camera)); + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + display3D.Activate(camera); + glClearColor(0.95f, 0.95f, 0.95f, 1.0f); + + draw_scene(); + + img_view_display.Activate(); + + if (show_frame.GuiChanged()) { + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + img_view[i]->SetImage(images[i]); + } + draw_plots(); + } + + if (show_accel.GuiChanged() || show_gyro.GuiChanged() || + show_gt_vel.GuiChanged() || show_gt_pos.GuiChanged() || + show_gt_ba.GuiChanged() || show_gt_bg.GuiChanged() || + show_est_vel.GuiChanged() || show_est_pos.GuiChanged() || + show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) { + draw_plots(); + } + + pangolin::FinishFrame(); + + if (continue_btn) { + if (!next_step()) continue_btn = false; + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + } + + t0.join(); + t1.join(); + t2.join(); + t3.join(); + // t4.join(); + + return 0; +} + +void draw_image_overlay(pangolin::View& v, size_t cam_id) { + size_t frame_id = show_frame; + basalt::TimeCamId tcid = std::make_pair(gt_frame_t_ns[frame_id], cam_id); + + if (show_obs) { + glLineWidth(1.0); + glColor3ubv(gt_color); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (gt_observations.find(tcid) != gt_observations.end()) { + const basalt::SimObservations& cr = gt_observations.at(tcid); + + for (size_t i = 0; i < cr.pos.size(); i++) { + const float radius = 2; + const Eigen::Vector2f c = cr.pos[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]); + } + + pangolin::GlFont::I().Text("%d gt points", cr.pos.size()).Draw(5, 20); + } + } + + if (show_obs_noisy) { + glLineWidth(1.0); + glColor3f(1.0, 1.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (noisy_observations.find(tcid) != noisy_observations.end()) { + const basalt::SimObservations& cr = noisy_observations.at(tcid); + + for (size_t i = 0; i < cr.pos.size(); i++) { + const float radius = 2; + const Eigen::Vector2f c = cr.pos[i].cast(); + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]); + } + + pangolin::GlFont::I().Text("%d noisy points", cr.pos.size()).Draw(5, 40); + } + } + + if (show_obs_vio) { + glLineWidth(1.0); + glColor3f(0.0, 0.0, 1.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + auto it = vis_map.find(gt_frame_t_ns[frame_id]); + + if (it != vis_map.end() && cam_id < it->second->projections.size()) { + const auto& points = it->second->projections[cam_id]; + + if (points.size() > 0) { + double min_id = points[0][2], max_id = points[0][2]; + for (size_t i = 0; i < points.size(); i++) { + min_id = std::min(min_id, points[i][2]); + max_id = std::max(max_id, points[i][2]); + } + + for (size_t i = 0; i < points.size(); i++) { + const float radius = 2; + const Eigen::Vector4d c = points[i]; + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) + pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]); + } + } + + glColor3f(0.0, 0.0, 1.0); + pangolin::GlFont::I().Text("%d vio points", points.size()).Draw(5, 60); + } + } +} + +void draw_scene() { + glPointSize(3); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glColor3ubv(gt_color); + pangolin::glDrawPoints(gt_points); + pangolin::glDrawLineStrip(gt_frame_t_w_i); + + glColor3ubv(cam_color); + pangolin::glDrawLineStrip(vio_t_w_i); + + size_t frame_id = show_frame; + + auto it = vis_map.find(gt_frame_t_ns[frame_id]); + + if (it != vis_map.end()) { + for (const auto& p : it->second->states) + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, cam_color, 0.1f); + + for (const auto& p : it->second->frames) + for (size_t i = 0; i < calib.T_i_c.size(); i++) + render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); + + glColor3ubv(pose_color); + pangolin::glDrawPoints(it->second->points); + } + + pangolin::glDrawAxis(gt_frame_T_w_i[frame_id].matrix(), 0.1); + + pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); +} + +void load_data(const std::string& calib_path) { + std::ifstream os(calib_path, std::ios::binary); + + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + std::cout << "Loaded camera with " << calib.intrinsics.size() + << " cameras" << std::endl; + + } else { + std::cerr << "could not load camera calibration " << calib_path + << std::endl; + std::abort(); + } +} + +void compute_projections() { + for (size_t i = 0; i < gt_frame_T_w_i.size(); i++) { + for (size_t j = 0; j < calib.T_i_c.size(); j++) { + basalt::TimeCamId tcid(gt_frame_t_ns[i], j); + basalt::SimObservations obs, obs_noisy; + + for (size_t k = 0; k < gt_points.size(); k++) { + Eigen::Vector4d p_cam; + p_cam.head<3>() = + (gt_frame_T_w_i[i] * calib.T_i_c[j]).inverse() * gt_points[k]; + + std::visit( + [&](const auto& cam) { + if (p_cam[2] > 0.1) { + Eigen::Vector2d p_2d; + cam.project(p_cam, p_2d); + + const int border = 5; + if (p_2d[0] >= border && p_2d[1] >= border && + p_2d[0] < calib.resolution[j][0] - border - 1 && + p_2d[1] < calib.resolution[j][1] - border - 1) { + obs.pos.emplace_back(p_2d); + obs.id.emplace_back(k); + + p_2d[0] += obs_noise_dist(gen); + p_2d[1] += obs_noise_dist(gen); + + obs_noisy.pos.emplace_back(p_2d); + obs_noisy.id.emplace_back(k); + } + } + }, + calib.intrinsics[j].variant); + } + + gt_observations[tcid] = obs; + noisy_observations[tcid] = obs_noisy; + } + } +} + +void gen_data() { + for (size_t i = 0; i < calib.intrinsics.size(); i++) { + images.emplace_back(); + images.back() = pangolin::TypedImage( + calib.resolution[i][0], calib.resolution[i][1], + pangolin::PixelFormatFromString("GRAY8")); + + images.back().Fill(200); + } + show_frame.Meta().gui_changed = true; + + double seconds = NUM_FRAMES / CAM_FREQ; + int num_knots = seconds / knot_time + 5; + // for (int i = 0; i < 2; i++) gt_spline.knots_push_back(Sophus::SE3d()); + gt_spline.genRandomTrajectory(num_knots); + + int64_t t_ns = 0; + int64_t dt_ns = int64_t(1e9) / CAM_FREQ; + + for (int i = 0; i < NUM_FRAMES; i++) { + gt_frame_T_w_i.emplace_back(gt_spline.pose(t_ns)); + gt_frame_t_w_i.emplace_back(gt_frame_T_w_i.back().translation()); + gt_frame_t_ns.emplace_back(t_ns); + + t_ns += dt_ns; + } + + dt_ns = int64_t(1e9) / IMU_FREQ; + + int64_t offset = + dt_ns / 2; // Offset to make IMU in the center of the interval + t_ns = offset; + + imu_data_log.Clear(); + + gt_accel_bias.clear(); + gt_gyro_bias.clear(); + + gt_accel_bias.emplace_back(Eigen::Vector3d::Random() / 10); + gt_gyro_bias.emplace_back(Eigen::Vector3d::Random() / 100); + + // gt_accel_bias.emplace_back(Eigen::Vector3d::Zero()); + // gt_gyro_bias.emplace_back(Eigen::Vector3d::Zero()); + + while (t_ns < gt_frame_t_ns.back()) { + Sophus::SE3d pose = gt_spline.pose(t_ns); + Eigen::Vector3d accel_body = + pose.so3().inverse() * (gt_spline.transAccelWorld(t_ns) - g); + Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); + + gt_accel.emplace_back(accel_body); + gt_gyro.emplace_back(rot_vel_body); + gt_vel.emplace_back(gt_spline.transVelWorld(t_ns)); + + accel_body[0] += accel_noise_dist(gen); + accel_body[1] += accel_noise_dist(gen); + accel_body[2] += accel_noise_dist(gen); + + accel_body += gt_accel_bias.back(); + + rot_vel_body[0] += gyro_noise_dist(gen); + rot_vel_body[1] += gyro_noise_dist(gen); + rot_vel_body[2] += gyro_noise_dist(gen); + + rot_vel_body += gt_gyro_bias.back(); + + noisy_accel.emplace_back(accel_body); + noisy_gyro.emplace_back(rot_vel_body); + + gt_imu_t_ns.emplace_back(t_ns + offset); + + std::vector vals; + vals.push_back(t_ns * 1e-9); + + for (int i = 0; i < 3; i++) vals.push_back(gt_accel.back()[i]); + for (int i = 0; i < 3; i++) vals.push_back(gt_gyro.back()[i]); + for (int i = 0; i < 3; i++) vals.push_back(gt_vel.back()[i]); + for (int i = 0; i < 3; i++) vals.push_back(pose.translation()[i]); + for (int i = 0; i < 3; i++) vals.push_back(gt_gyro_bias.back()[i]); + for (int i = 0; i < 3; i++) vals.push_back(gt_accel_bias.back()[i]); + + imu_data_log.Log(vals); + + double dt_sqrt = std::sqrt(dt_ns * 1e-9); + Eigen::Vector3d gt_accel_bias_next = gt_accel_bias.back(); + gt_accel_bias_next[0] += accel_bias_dist(gen) * dt_sqrt; + gt_accel_bias_next[1] += accel_bias_dist(gen) * dt_sqrt; + gt_accel_bias_next[2] += accel_bias_dist(gen) * dt_sqrt; + gt_accel_bias.emplace_back(gt_accel_bias_next); + + Eigen::Vector3d gt_gyro_bias_next = gt_gyro_bias.back(); + gt_gyro_bias_next[0] += gyro_bias_dist(gen) * dt_sqrt; + gt_gyro_bias_next[1] += gyro_bias_dist(gen) * dt_sqrt; + gt_gyro_bias_next[2] += gyro_bias_dist(gen) * dt_sqrt; + gt_gyro_bias.emplace_back(gt_gyro_bias_next); + + t_ns += dt_ns; + } + + show_accel.Meta().gui_changed = true; + + for (int i = 0; i < NUM_POINTS; i++) { + Eigen::Vector3d point; + + point = Eigen::Vector3d::Random().normalized() * POINT_DIST; + + gt_points.push_back(point); + } + + compute_projections(); + + // Save spline data + { + std::string path = marg_data_path + "/gt_spline.cereal"; + + std::cout << "Saving gt_spline " << path << std::endl; + + std::ofstream os(path, std::ios::binary); + { + cereal::JSONOutputArchive archive(os); + + int64_t t_ns = gt_spline.getDtNs(); + + Eigen::vector knots; + for (size_t i = 0; i < gt_spline.numKnots(); i++) { + knots.push_back(gt_spline.getKnot(i)); + } + + archive(cereal::make_nvp("t_ns", t_ns)); + archive(cereal::make_nvp("knots", knots)); + + archive(cereal::make_nvp("noisy_accel", noisy_accel)); + archive(cereal::make_nvp("noisy_gyro", noisy_gyro)); + archive(cereal::make_nvp("noisy_accel", gt_accel)); + archive(cereal::make_nvp("gt_gyro", gt_gyro)); + archive(cereal::make_nvp("gt_points", gt_points)); + archive(cereal::make_nvp("gt_accel_bias", gt_accel_bias)); + archive(cereal::make_nvp("gt_gyro_bias", gt_gyro_bias)); + + archive(cereal::make_nvp("gt_observations", gt_observations)); + archive(cereal::make_nvp("noisy_observations", noisy_observations)); + + archive(cereal::make_nvp("gt_points", gt_points)); + + archive(cereal::make_nvp("gt_frame_t_ns", gt_frame_t_ns)); + archive(cereal::make_nvp("gt_imu_t_ns", gt_imu_t_ns)); + } + + os.close(); + } +} + +void draw_plots() { + plotter->ClearSeries(); + plotter->ClearMarkers(); + + if (show_accel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "accel measurements x"); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "accel measurements y"); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "accel measurements z"); + } + + if (show_gyro) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "gyro measurements x"); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "gyro measurements y"); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "gyro measurements z"); + } + + if (show_gt_vel) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth velocity x"); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth velocity y"); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth velocity z"); + } + + if (show_gt_pos) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth position x"); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth position y"); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth position z"); + } + + if (show_gt_bg) { + plotter->AddSeries("$0", "$13", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth gyro bias x"); + plotter->AddSeries("$0", "$14", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth gyro bias y"); + plotter->AddSeries("$0", "$15", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth gyro bias z"); + } + + if (show_gt_ba) { + plotter->AddSeries("$0", "$16", pangolin::DrawingModeDashed, + pangolin::Colour::Red(), "ground-truth accel bias x"); + plotter->AddSeries("$0", "$17", pangolin::DrawingModeDashed, + pangolin::Colour::Green(), "ground-truth accel bias y"); + plotter->AddSeries("$0", "$18", pangolin::DrawingModeDashed, + pangolin::Colour::Blue(), "ground-truth accel bias z"); + } + + if (show_est_vel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated velocity x", + &vio_data_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated velocity y", + &vio_data_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated velocity z", + &vio_data_log); + } + + if (show_est_pos) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated position x", + &vio_data_log); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated position y", + &vio_data_log); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated position z", + &vio_data_log); + } + + if (show_est_bg) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated gyro bias x", + &vio_data_log); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated gyro bias y", + &vio_data_log); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated gyro bias z", + &vio_data_log); + } + + if (show_est_ba) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "estimated accel bias x", + &vio_data_log); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "estimated accel bias y", + &vio_data_log); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "estimated accel bias z", + &vio_data_log); + } + + double t = gt_frame_t_ns[show_frame] * 1e-9; + plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal, + pangolin::Colour::White()); +} + +void setup_vio() { + int64_t t_init_ns = gt_frame_t_ns[0]; + Sophus::SE3d T_w_i_init = gt_frame_T_w_i[0]; + Eigen::Vector3d vel_w_i_init = gt_spline.transVelWorld(t_init_ns); + + std::cout << "Setting up filter: t_ns " << t_init_ns << std::endl; + std::cout << "T_w_i\n" << T_w_i_init.matrix() << std::endl; + std::cout << "vel_w_i " << vel_w_i_init.transpose() << std::endl; + + Eigen::Vector3d gyro_bias_std; + gyro_bias_std.setConstant(gyro_bias_std_dev); + + Eigen::Vector3d accel_bias_std; + accel_bias_std.setConstant(accel_bias_std_dev); + + basalt::VioConfig config; + + vio.reset(new basalt::KeypointVioEstimator( + t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(), + gt_accel_bias.front(), 0.0001, g, calib, config)); + + vio->setMaxStates(3); + vio->setMaxKfs(5); + + // int iteration = 0; + vio_data_log.Clear(); + error_data_log.Clear(); + vio_t_w_i.clear(); +} + +bool next_step() { + if (show_frame < NUM_FRAMES - 1) { + show_frame = show_frame + 1; + show_frame.Meta().gui_changed = true; + return true; + } else { + return false; + } +} + +void alignButton() { + basalt::alignSVD(gt_frame_t_ns, vio_t_w_i, gt_frame_t_ns, gt_frame_t_w_i); +} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..26d576a --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.2) + +find_package(TBB REQUIRED) +include_directories(${TBB_INCLUDE_DIR}) + +set(GTEST_MAIN_LIBRARY gtest_main) +set(GTEST_LIBRARY gtest) + + +include_directories(../thirdparty/basalt-headers/test/include) + + +add_executable(test_image src/test_image.cpp) +target_link_libraries(test_image ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${OpenCV_LIBS} basalt) + +add_executable(test_spline_opt src/test_spline_opt.cpp) +target_link_libraries(test_spline_opt ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${TBB_LIBRARIES} opengv basalt) + +add_executable(test_vio src/test_vio.cpp) +target_link_libraries(test_vio ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${TBB_LIBRARIES} opengv basalt) + +add_executable(test_nfr src/test_nfr.cpp) +target_link_libraries(test_nfr ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${TBB_LIBRARIES} opengv basalt) + + +enable_testing() + +add_test(test_image test_image COMMAND Test) +add_test(test_spline_opt test_spline_opt COMMAND Test) +add_test(test_vio test_vio COMMAND Test) +add_test(test_nfr test_nfr COMMAND Test) diff --git a/test/src/test_image.cpp b/test/src/test_image.cpp new file mode 100644 index 0000000..ab47c20 --- /dev/null +++ b/test/src/test_image.cpp @@ -0,0 +1,127 @@ + +#include + +#include + +#include "gtest/gtest.h" +#include "test_utils.h" + +void setImageData(uint16_t *imageArray, int size) { + double norm = RAND_MAX; + norm /= (double)std::numeric_limits::max(); + + for (int i = 0; i < size; i++) { + imageArray[i] = (unsigned char)(rand() / norm); + } +} + +TEST(Pattern, ImageInterp) { + Eigen::Vector2d offset(231.234242, 123.23424); + + basalt::ManagedImage img(640, 480); + setImageData(img.ptr, img.size()); + + Eigen::Vector3d vg = img.interpGrad(offset); + + Eigen::Matrix J = vg.tail<2>(); + + // std::cerr << "vg\n" << vg << std::endl; + + test_jacobian("d_val_d_p", J, + [&](const Eigen::Vector2d &x) { + Eigen::Matrix res; + Eigen::Vector2d p1 = offset + x; + res[0] = img.interpGrad(p1)[0]; + return res; + }, + Eigen::Vector2d::Zero(), 1.0); +} + +TEST(Image, ImageInterpolate) { + Eigen::Vector2i offset(231, 123); + + basalt::ManagedImage img(640, 480); + setImageData(img.ptr, img.size()); + + double eps = 1e-12; + double threshold = 1e-8; + + { + Eigen::Vector2i pi = offset; + Eigen::Vector2d pd = pi.cast() + Eigen::Vector2d(eps, eps); + + uint16_t val1 = img(pi); + double val2 = img.interp(pd); + double val3 = img.interpGrad(pd)[0]; + + EXPECT_LE(std::abs(val2 - val1), threshold); + EXPECT_FLOAT_EQ(val2, val3); + } + + { + Eigen::Vector2i pi = offset; + Eigen::Vector2d pd = pi.cast() + Eigen::Vector2d(eps, eps); + + uint16_t val1 = img(pi); + double val2 = img.interp(pd); + double val3 = img.interpGrad(pd)[0]; + + EXPECT_LE(std::abs(val2 - val1), threshold); + EXPECT_FLOAT_EQ(val2, val3); + } + + { + Eigen::Vector2i pi = offset + Eigen::Vector2i(1, 0); + Eigen::Vector2d pd = pi.cast() + Eigen::Vector2d(-eps, eps); + + uint16_t val1 = img(pi); + double val2 = img.interp(pd); + double val3 = img.interpGrad(pd)[0]; + + EXPECT_LE(std::abs(val2 - val1), threshold); + EXPECT_FLOAT_EQ(val2, val3); + } + + { + Eigen::Vector2i pi = offset + Eigen::Vector2i(0, 1); + Eigen::Vector2d pd = pi.cast() + Eigen::Vector2d(eps, -eps); + + uint16_t val1 = img(pi); + double val2 = img.interp(pd); + double val3 = img.interpGrad(pd)[0]; + + EXPECT_LE(std::abs(val2 - val1), threshold); + EXPECT_FLOAT_EQ(val2, val3); + } + + { + Eigen::Vector2i pi = offset + Eigen::Vector2i(1, 1); + Eigen::Vector2d pd = pi.cast() + Eigen::Vector2d(-eps, -eps); + + uint16_t val1 = img(pi); + double val2 = img.interp(pd); + double val3 = img.interpGrad(pd)[0]; + + EXPECT_LE(std::abs(val2 - val1), threshold); + EXPECT_FLOAT_EQ(val2, val3); + } +} + +TEST(Image, ImageInterpolateGrad) { + Eigen::Vector2i offset(231, 123); + + basalt::ManagedImage img(640, 480); + setImageData(img.ptr, img.size()); + + Eigen::Vector2d pd = offset.cast() + Eigen::Vector2d(0.4, 0.34345); + + Eigen::Vector3d valGrad = img.interpGrad(pd); + Eigen::Matrix J = valGrad.tail<2>(); + + test_jacobian( + "d_res_d_x", J, + [&](const Eigen::Vector2d &x) { + return Eigen::Matrix(img.interp(pd + x)); + }, + Eigen::Vector2d::Zero(), 1); +} diff --git a/test/src/test_nfr.cpp b/test/src/test_nfr.cpp new file mode 100644 index 0000000..ebf6fea --- /dev/null +++ b/test/src/test_nfr.cpp @@ -0,0 +1,137 @@ + + +#include +#include + +#include + +#include "gtest/gtest.h" +#include "test_utils.h" + +static const double accel_std_dev = 0.23; +static const double gyro_std_dev = 0.0027; + +// Smaller noise for testing +// static const double accel_std_dev = 0.00023; +// static const double gyro_std_dev = 0.0000027; + +std::random_device rd{}; +std::mt19937 gen{rd()}; + +std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev}; +std::normal_distribution<> accel_noise_dist{0, accel_std_dev}; + +TEST(PreIntegrationTestSuite, RelPoseTest) { + Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_j = Sophus::expd(Sophus::Vector6d::Random()); + + Sophus::SE3d T_i_j = + Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i.inverse() * T_w_j; + + Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j; + basalt::relPoseError(T_i_j, T_w_i, T_w_j, &d_res_d_T_w_i, &d_res_d_T_w_j); + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); + + return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j); + + }, + x0); + } + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian("d_res_d_T_w_j", d_res_d_T_w_j, + [&](const Sophus::Vector6d& x) { + auto T_w_j_new = T_w_j; + basalt::PoseState::incPose(x, T_w_j_new); + + return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new); + + }, + x0); + } +} + +TEST(PreIntegrationTestSuite, AbsPositionTest) { + Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + + Eigen::Vector3d pos = T_w_i.translation() + Eigen::Vector3d::Random() / 10; + + Sophus::Matrix d_res_d_T_w_i; + basalt::absPositionError(T_w_i, pos, &d_res_d_T_w_i); + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); + + return basalt::absPositionError(T_w_i_new, pos); + + }, + x0); + } +} + +TEST(PreIntegrationTestSuite, YawTest) { + Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + + Eigen::Vector3d yaw_dir_body = + T_w_i.so3().inverse() * Eigen::Vector3d::UnitX(); + + T_w_i = Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i; + + Sophus::Matrix d_res_d_T_w_i; + basalt::yawError(T_w_i, yaw_dir_body, &d_res_d_T_w_i); + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); + + double res = basalt::yawError(T_w_i_new, yaw_dir_body); + + return Eigen::Matrix(res); + + }, + x0); + } +} + +TEST(PreIntegrationTestSuite, RollPitchTest) { + Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + + Sophus::SO3d R_w_i = T_w_i.so3(); + + T_w_i = Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i; + + Sophus::Matrix d_res_d_T_w_i; + basalt::rollPitchError(T_w_i, R_w_i, &d_res_d_T_w_i); + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); + + return basalt::rollPitchError(T_w_i_new, R_w_i); + + }, + x0); + } +} diff --git a/test/src/test_spline_opt.cpp b/test/src/test_spline_opt.cpp new file mode 100644 index 0000000..de154d2 --- /dev/null +++ b/test/src/test_spline_opt.cpp @@ -0,0 +1,109 @@ + + +#include + +#include + +#include "gtest/gtest.h" +#include "test_utils.h" + +TEST(SplineOpt, SplineOptTest) { + tbb::task_scheduler_init init( + tbb::task_scheduler_init::default_num_threads()); + + int num_knots = 15; + + basalt::CalibAccelBias accel_bias_full; + accel_bias_full.setRandom(); + + basalt::CalibGyroBias gyro_bias_full; + gyro_bias_full.setRandom(); + + Eigen::Vector3d g(0, 0, -9.81); + + basalt::Se3Spline<5> gt_spline(int64_t(2e9)); + gt_spline.genRandomTrajectory(num_knots); + + basalt::SplineOptimization<5, double> spline_opt(int64_t(2e9)); + + int64_t pose_dt_ns = 1e8; + for (int64_t t_ns = pose_dt_ns / 2; t_ns < gt_spline.maxTimeNs(); + t_ns += pose_dt_ns) { + Sophus::SE3d pose_gt = gt_spline.pose(t_ns); + + spline_opt.addPoseMeasurement(t_ns, pose_gt); + } + + int64_t dt_ns = 1e7; + for (int64_t t_ns = 0; t_ns < gt_spline.maxTimeNs(); t_ns += dt_ns) { + Sophus::SE3d pose = gt_spline.pose(t_ns); + Eigen::Vector3d accel_body = + pose.so3().inverse() * (gt_spline.transAccelWorld(t_ns) + g); + + // accel_body + accel_bias = (I + scale) * meas + + spline_opt.addAccelMeasurement( + t_ns, accel_bias_full.invertCalibration(accel_body)); + } + + for (int64_t t_ns = 0; t_ns < gt_spline.maxTimeNs(); t_ns += dt_ns) { + Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); + + spline_opt.addGyroMeasurement( + t_ns, gyro_bias_full.invertCalibration(rot_vel_body)); + } + + spline_opt.resetCalib(0, {}); + + spline_opt.initSpline(gt_spline); + spline_opt.init(); + + double error; + double reprojection_error; + int num_inliers; + for (int i = 0; i < 5; i++) + spline_opt.optimize(false, true, false, false, true, false, 0.002, error, + num_inliers, reprojection_error); + + ASSERT_TRUE( + spline_opt.getAccelBias().getParam().isApprox(accel_bias_full.getParam())) + << "spline_opt.getCalib().accel_bias " + << spline_opt.getGyroBias().getParam().transpose() << " and accel_bias " + << accel_bias_full.getParam().transpose() << " are not the same"; + + ASSERT_TRUE( + spline_opt.getGyroBias().getParam().isApprox(gyro_bias_full.getParam())) + << "spline_opt.getCalib().gyro_bias " + << spline_opt.getGyroBias().getParam().transpose() << " and gyro_bias " + << gyro_bias_full.getParam().transpose() << " are not the same"; + + ASSERT_TRUE(spline_opt.getG().isApprox(g)) + << "spline_opt.getG() " << spline_opt.getG().transpose() << " and g " + << g.transpose() << " are not the same"; + + for (int64_t t_ns = 0; t_ns < gt_spline.maxTimeNs(); t_ns += 1e7) { + Sophus::SE3d pose_gt = gt_spline.pose(t_ns); + Sophus::SE3d pose = spline_opt.getSpline().pose(t_ns); + + Eigen::Vector3d pos_gt = pose_gt.translation(); + Eigen::Vector3d pos = pose.translation(); + + Eigen::Vector4d quat_gt = pose_gt.unit_quaternion().coeffs(); + Eigen::Vector4d quat = pose.unit_quaternion().coeffs(); + + Eigen::Vector3d accel_gt = gt_spline.transAccelWorld(t_ns); + Eigen::Vector3d accel = spline_opt.getSpline().transAccelWorld(t_ns); + + Eigen::Vector3d gyro_gt = gt_spline.rotVelBody(t_ns); + Eigen::Vector3d gyro = spline_opt.getSpline().rotVelBody(t_ns); + + ASSERT_TRUE(pos_gt.isApprox(pos)) << "pos_gt and pos are not the same"; + + ASSERT_TRUE(quat_gt.isApprox(quat)) << "quat_gt and quat are not the same"; + + ASSERT_TRUE(accel_gt.isApprox(accel)) + << "accel_gt and accel are not the same"; + + ASSERT_TRUE(gyro_gt.isApprox(gyro)) << "gyro_gt and gyro are not the same"; + } +} diff --git a/test/src/test_vio.cpp b/test/src/test_vio.cpp new file mode 100644 index 0000000..ab039fa --- /dev/null +++ b/test/src/test_vio.cpp @@ -0,0 +1,408 @@ + + +#include +#include + +#include + +#include "gtest/gtest.h" +#include "test_utils.h" + +static const double accel_std_dev = 0.23; +static const double gyro_std_dev = 0.0027; + +// Smaller noise for testing +// static const double accel_std_dev = 0.00023; +// static const double gyro_std_dev = 0.0000027; + +std::random_device rd{}; +std::mt19937 gen{rd()}; + +std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev}; +std::normal_distribution<> accel_noise_dist{0, accel_std_dev}; + +TEST(PreIntegrationTestSuite, ImuNullspace2Test) { + int num_knots = 15; + + Eigen::Vector3d bg, ba; + bg = Eigen::Vector3d::Random() / 100; + ba = Eigen::Vector3d::Random() / 10; + + basalt::IntegratedImuMeasurement imu_meas(0, bg, ba); + + basalt::Se3Spline<5> gt_spline(int64_t(10e9)); + gt_spline.genRandomTrajectory(num_knots); + + basalt::PoseVelBiasState state0, state1, state1_gt; + + state0.t_ns = 0; + state0.T_w_i = gt_spline.pose(int64_t(0)); + state0.vel_w_i = gt_spline.transVelWorld(int64_t(0)); + state0.bias_gyro = bg; + state0.bias_accel = ba; + + int64_t dt_ns = 1e7; + for (int64_t t_ns = dt_ns / 2; + t_ns < int64_t(1e8); // gt_spline.maxTimeNs() - int64_t(1e9); + t_ns += dt_ns) { + Sophus::SE3d pose = gt_spline.pose(t_ns); + Eigen::Vector3d accel_body = + pose.so3().inverse() * + (gt_spline.transAccelWorld(t_ns) - basalt::constants::g); + Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); + + basalt::ImuData data; + data.accel = accel_body + ba; + data.gyro = rot_vel_body + bg; + + data.accel[0] += accel_noise_dist(gen); + data.accel[1] += accel_noise_dist(gen); + data.accel[2] += accel_noise_dist(gen); + + data.gyro[0] += gyro_noise_dist(gen); + data.gyro[1] += gyro_noise_dist(gen); + data.gyro[2] += gyro_noise_dist(gen); + + data.accel_cov.setConstant(accel_std_dev * accel_std_dev); + data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + + data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval; + + imu_meas.integrate(data); + } + + state1.t_ns = imu_meas.get_dt_ns(); + state1.T_w_i = gt_spline.pose(imu_meas.get_dt_ns()) * + Sophus::expd(Sophus::Vector6d::Random() / 10); + state1.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns()) + + Sophus::Vector3d::Random() / 10; + state1.bias_gyro = bg; + state1.bias_accel = ba; + + Eigen::Vector3d gyro_weight; + gyro_weight.setConstant(1e6); + + Eigen::Vector3d accel_weight; + accel_weight.setConstant(1e6); + + Eigen::map imu_meas_vec; + Eigen::map frame_states; + Eigen::map frame_poses; + + imu_meas_vec[state0.t_ns] = imu_meas; + frame_states[state0.t_ns] = state0; + frame_states[state1.t_ns] = state1; + + int asize = 30; + Eigen::MatrixXd H; + Eigen::VectorXd b; + H.setZero(asize, asize); + b.setZero(asize); + + basalt::AbsOrderMap aom; + aom.total_size = 30; + aom.items = 2; + aom.abs_order_map[state0.t_ns] = std::make_pair(0, 15); + aom.abs_order_map[state1.t_ns] = std::make_pair(15, 15); + + double imu_error, bg_error, ba_error; + basalt::KeypointVioEstimator::linearizeAbsIMU( + aom, H, b, imu_error, bg_error, ba_error, frame_states, imu_meas_vec, + gyro_weight, accel_weight, basalt::constants::g); + + // Check quadratic approximation + for (int i = 0; i < 10; i++) { + Eigen::VectorXd rand_inc; + rand_inc.setRandom(asize); + rand_inc.normalize(); + rand_inc /= 10000; + + auto frame_states_copy = frame_states; + frame_states_copy[state0.t_ns].applyInc(rand_inc.segment<15>(0)); + frame_states_copy[state1.t_ns].applyInc(rand_inc.segment<15>(15)); + + double imu_error_u, bg_error_u, ba_error_u; + basalt::KeypointVioEstimator::computeImuError( + aom, imu_error_u, bg_error_u, ba_error_u, frame_states_copy, + imu_meas_vec, gyro_weight, accel_weight, basalt::constants::g); + + double e0 = imu_error + bg_error + ba_error; + double e1 = imu_error_u + bg_error_u + ba_error_u - e0; + + double e2 = 0.5 * rand_inc.transpose() * H * rand_inc; + e2 += rand_inc.transpose() * b; + + EXPECT_LE(std::abs(e1 - e2), 1e-2) << "e1 " << e1 << " e2 " << e2; + } + + std::cout << "=========================================" << std::endl; + Eigen::VectorXd null_res = basalt::KeypointVioEstimator::checkNullspace( + H, b, aom, frame_states, frame_poses); + std::cout << "=========================================" << std::endl; + + EXPECT_LE(std::abs(null_res[0]), 1e-8); + EXPECT_LE(std::abs(null_res[1]), 1e-8); + EXPECT_LE(std::abs(null_res[2]), 1e-8); + EXPECT_LE(std::abs(null_res[5]), 1e-6); +} + +TEST(PreIntegrationTestSuite, ImuNullspace3Test) { + int num_knots = 15; + + Eigen::Vector3d bg, ba; + bg = Eigen::Vector3d::Random() / 100; + ba = Eigen::Vector3d::Random() / 10; + + basalt::IntegratedImuMeasurement imu_meas1(0, bg, ba); + + basalt::Se3Spline<5> gt_spline(int64_t(10e9)); + gt_spline.genRandomTrajectory(num_knots); + + basalt::PoseVelBiasState state0, state1, state2; + + state0.t_ns = 0; + state0.T_w_i = gt_spline.pose(int64_t(0)); + state0.vel_w_i = gt_spline.transVelWorld(int64_t(0)); + state0.bias_gyro = bg; + state0.bias_accel = ba; + + int64_t dt_ns = 1e7; + for (int64_t t_ns = dt_ns / 2; + t_ns < int64_t(1e9); // gt_spline.maxTimeNs() - int64_t(1e9); + t_ns += dt_ns) { + Sophus::SE3d pose = gt_spline.pose(t_ns); + Eigen::Vector3d accel_body = + pose.so3().inverse() * + (gt_spline.transAccelWorld(t_ns) - basalt::constants::g); + Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); + + basalt::ImuData data; + data.accel = accel_body + ba; + data.gyro = rot_vel_body + bg; + + data.accel[0] += accel_noise_dist(gen); + data.accel[1] += accel_noise_dist(gen); + data.accel[2] += accel_noise_dist(gen); + + data.gyro[0] += gyro_noise_dist(gen); + data.gyro[1] += gyro_noise_dist(gen); + data.gyro[2] += gyro_noise_dist(gen); + + data.accel_cov.setConstant(accel_std_dev * accel_std_dev); + data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + + data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval; + + imu_meas1.integrate(data); + } + + basalt::IntegratedImuMeasurement imu_meas2(imu_meas1.get_dt_ns(), bg, ba); + for (int64_t t_ns = imu_meas1.get_dt_ns() + dt_ns / 2; + t_ns < int64_t(2e9); // gt_spline.maxTimeNs() - int64_t(1e9); + t_ns += dt_ns) { + Sophus::SE3d pose = gt_spline.pose(t_ns); + Eigen::Vector3d accel_body = + pose.so3().inverse() * + (gt_spline.transAccelWorld(t_ns) - basalt::constants::g); + Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); + + basalt::ImuData data; + data.accel = accel_body + ba; + data.gyro = rot_vel_body + bg; + + data.accel[0] += accel_noise_dist(gen); + data.accel[1] += accel_noise_dist(gen); + data.accel[2] += accel_noise_dist(gen); + + data.gyro[0] += gyro_noise_dist(gen); + data.gyro[1] += gyro_noise_dist(gen); + data.gyro[2] += gyro_noise_dist(gen); + + data.accel_cov.setConstant(accel_std_dev * accel_std_dev); + data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + + data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval; + + imu_meas2.integrate(data); + } + + state1.t_ns = imu_meas1.get_dt_ns(); + state1.T_w_i = gt_spline.pose(state1.t_ns) * + Sophus::expd(Sophus::Vector6d::Random() / 10); + state1.vel_w_i = + gt_spline.transVelWorld(state1.t_ns) + Sophus::Vector3d::Random() / 10; + state1.bias_gyro = bg; + state1.bias_accel = ba; + + state2.t_ns = imu_meas1.get_dt_ns() + imu_meas2.get_dt_ns(); + state2.T_w_i = gt_spline.pose(state2.t_ns) * + Sophus::expd(Sophus::Vector6d::Random() / 10); + state2.vel_w_i = + gt_spline.transVelWorld(state2.t_ns) + Sophus::Vector3d::Random() / 10; + state2.bias_gyro = bg; + state2.bias_accel = ba; + + Eigen::Vector3d gyro_weight; + gyro_weight.setConstant(1e6); + + Eigen::Vector3d accel_weight; + accel_weight.setConstant(1e6); + + Eigen::map imu_meas_vec; + Eigen::map frame_states; + Eigen::map frame_poses; + + imu_meas_vec[imu_meas1.get_start_t_ns()] = imu_meas1; + imu_meas_vec[imu_meas2.get_start_t_ns()] = imu_meas2; + frame_states[state0.t_ns] = state0; + frame_states[state1.t_ns] = state1; + frame_states[state2.t_ns] = state2; + + int asize = 45; + Eigen::MatrixXd H; + Eigen::VectorXd b; + H.setZero(asize, asize); + b.setZero(asize); + + basalt::AbsOrderMap aom; + aom.total_size = asize; + aom.items = 2; + aom.abs_order_map[state0.t_ns] = std::make_pair(0, 15); + aom.abs_order_map[state1.t_ns] = std::make_pair(15, 15); + aom.abs_order_map[state2.t_ns] = std::make_pair(30, 15); + + double imu_error, bg_error, ba_error; + basalt::KeypointVioEstimator::linearizeAbsIMU( + aom, H, b, imu_error, bg_error, ba_error, frame_states, imu_meas_vec, + gyro_weight, accel_weight, basalt::constants::g); + + std::cout << "=========================================" << std::endl; + Eigen::VectorXd null_res = basalt::KeypointVioEstimator::checkNullspace( + H, b, aom, frame_states, frame_poses); + std::cout << "=========================================" << std::endl; + + EXPECT_LE(std::abs(null_res[0]), 1e-8); + EXPECT_LE(std::abs(null_res[1]), 1e-8); + EXPECT_LE(std::abs(null_res[2]), 1e-8); + EXPECT_LE(std::abs(null_res[5]), 1e-6); +} + +TEST(PreIntegrationTestSuite, RelPoseTest) { + Sophus::SE3d T_w_i_h = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i_t = Sophus::expd(Sophus::Vector6d::Random()); + + Sophus::SE3d T_i_c_h = Sophus::expd(Sophus::Vector6d::Random() / 10); + Sophus::SE3d T_i_c_t = Sophus::expd(Sophus::Vector6d::Random() / 10); + + Sophus::Matrix6d d_rel_d_h, d_rel_d_t; + + Sophus::SE3d T_t_h_sophus = basalt::KeypointVioEstimator::computeRelPose( + T_w_i_h, T_i_c_h, T_w_i_t, T_i_c_t, &d_rel_d_h, &d_rel_d_t); + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian( + "d_rel_d_h", d_rel_d_h, + [&](const Sophus::Vector6d& x) { + Sophus::SE3d T_w_h_new = T_w_i_h; + basalt::PoseState::incPose(x, T_w_h_new); + + Sophus::SE3d T_t_h_sophus_new = + basalt::KeypointVioEstimator::computeRelPose(T_w_h_new, T_i_c_h, + T_w_i_t, T_i_c_t); + + return Sophus::logd(T_t_h_sophus_new * T_t_h_sophus.inverse()); + }, + x0); + } + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian( + "d_rel_d_t", d_rel_d_t, + [&](const Sophus::Vector6d& x) { + Sophus::SE3d T_w_t_new = T_w_i_t; + basalt::PoseState::incPose(x, T_w_t_new); + + Sophus::SE3d T_t_h_sophus_new = + basalt::KeypointVioEstimator::computeRelPose(T_w_i_h, T_i_c_h, + T_w_t_new, T_i_c_t); + return Sophus::logd(T_t_h_sophus_new * T_t_h_sophus.inverse()); + }, + x0); + } +} + +TEST(PreIntegrationTestSuite, LinearizePointsTest) { + basalt::ExtendedUnifiedCamera cam = + basalt::ExtendedUnifiedCamera::getTestProjections()[0]; + + basalt::KeypointVioEstimator::KeypointPosition kpt_pos; + + Eigen::Vector4d point3d; + cam.unproject(Eigen::Vector2d::Random() * 50, point3d); + kpt_pos.dir = basalt::StereographicParam::project(point3d); + kpt_pos.id = 0.1231231; + + Sophus::SE3d T_w_h = Sophus::expd(Sophus::Vector6d::Random() / 100); + Sophus::SE3d T_w_t = Sophus::expd(Sophus::Vector6d::Random() / 100); + T_w_t.translation()[0] += 0.1; + + Sophus::SE3d T_t_h_sophus = T_w_t.inverse() * T_w_h; + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + Eigen::Vector4d p_trans; + p_trans = basalt::StereographicParam::unproject(kpt_pos.dir); + p_trans(3) = kpt_pos.id; + + p_trans = T_t_h * p_trans; + + basalt::KeypointVioEstimator::KeypointObservation kpt_obs; + cam.project(p_trans, kpt_obs.pos); + + Eigen::Vector2d res; + Eigen::Matrix d_res_d_xi; + Eigen::Matrix d_res_d_p; + + basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, + res, &d_res_d_xi, &d_res_d_p); + + { + Sophus::Vector6d x0; + x0.setZero(); + test_jacobian("d_res_d_xi", d_res_d_xi, + [&](const Sophus::Vector6d& x) { + Eigen::Matrix4d T_t_h_new = + (Sophus::expd(x) * T_t_h_sophus).matrix(); + + Eigen::Vector2d res; + basalt::KeypointVioEstimator::linearizePoint( + kpt_obs, kpt_pos, T_t_h_new, cam, res); + + return res; + }, + x0); + } + + { + Eigen::Vector3d x0; + x0.setZero(); + test_jacobian("d_res_d_p", d_res_d_p, + [&](const Eigen::Vector3d& x) { + basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = + kpt_pos; + + kpt_pos_new.dir += x.head<2>(); + kpt_pos_new.id += x[2]; + + Eigen::Vector2d res; + basalt::KeypointVioEstimator::linearizePoint( + kpt_obs, kpt_pos_new, T_t_h, cam, res); + + return res; + }, + x0); + } +} diff --git a/thirdparty/CLI11 b/thirdparty/CLI11 new file mode 160000 index 0000000..76d2cde --- /dev/null +++ b/thirdparty/CLI11 @@ -0,0 +1 @@ +Subproject commit 76d2cde6568c9c8870b728aa9bc64b70b29127fd diff --git a/thirdparty/DBoW3/CMakeLists.txt b/thirdparty/DBoW3/CMakeLists.txt new file mode 100644 index 0000000..c03b656 --- /dev/null +++ b/thirdparty/DBoW3/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8) +PROJECT(DBoW3) +set(PROJECT_VERSION "0.0.1") +string(REGEX MATCHALL "[0-9]" PROJECT_VERSION_PARTS "${PROJECT_VERSION}") +list(GET PROJECT_VERSION_PARTS 0 PROJECT_VERSION_MAJOR) +list(GET PROJECT_VERSION_PARTS 1 PROJECT_VERSION_MINOR) +list(GET PROJECT_VERSION_PARTS 2 PROJECT_VERSION_PATCH) +set(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") + +#------------------------------------------------ +# DIRS +#------------------------------------------------ +ADD_SUBDIRECTORY(src) +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + diff --git a/thirdparty/DBoW3/LICENSE.txt b/thirdparty/DBoW3/LICENSE.txt new file mode 100644 index 0000000..fa723d9 --- /dev/null +++ b/thirdparty/DBoW3/LICENSE.txt @@ -0,0 +1,44 @@ +DBoW3: bag-of-words library for C++ with generic descriptors + +Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +2. 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. +3. Neither the name of copyright holders 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 COPYRIGHT HOLDERS OR CONTRIBUTORS +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. + +If you use it in an academic work, please cite: + + @ARTICLE{GalvezTRO12, + author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, + journal={IEEE Transactions on Robotics}, + title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, + year={2012}, + month={October}, + volume={28}, + number={5}, + pages={1188--1197}, + doi={10.1109/TRO.2012.2197158}, + ISSN={1552-3098} + } + diff --git a/thirdparty/DBoW3/README.txt b/thirdparty/DBoW3/README.txt new file mode 100644 index 0000000..aab0eae --- /dev/null +++ b/thirdparty/DBoW3/README.txt @@ -0,0 +1,7 @@ +You should have received this DBoW3 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). +See the original DBoW3 library at: https://github.com/dorian3d/DBoW3 +All files included in this version are BSD, see LICENSE.txt + +We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. +See the original DLib library at: https://github.com/dorian3d/DLib +All files included in this version are BSD, see LICENSE.txt diff --git a/thirdparty/DBoW3/src/BowVector.cpp b/thirdparty/DBoW3/src/BowVector.cpp new file mode 100644 index 0000000..5af960b --- /dev/null +++ b/thirdparty/DBoW3/src/BowVector.cpp @@ -0,0 +1,136 @@ +/** + * File: BowVector.cpp + * Date: March 2011 + * Author: Dorian Galvez-Lopez + * Description: bag of words vector + * License: see the LICENSE.txt file + * + */ + +#include +#include +#include +#include +#include + +#include "BowVector.h" + +namespace DBoW3 { + +// -------------------------------------------------------------------------- + +BowVector::BowVector(void) {} + +// -------------------------------------------------------------------------- + +BowVector::~BowVector(void) {} + +// -------------------------------------------------------------------------- + +void BowVector::addWeight(WordId id, WordValue v) { + BowVector::iterator vit = this->lower_bound(id); + + if (vit != this->end() && !(this->key_comp()(id, vit->first))) { + vit->second += v; + } else { + this->insert(vit, BowVector::value_type(id, v)); + } +} + +// -------------------------------------------------------------------------- + +void BowVector::addIfNotExist(WordId id, WordValue v) { + BowVector::iterator vit = this->lower_bound(id); + + if (vit == this->end() || (this->key_comp()(id, vit->first))) { + this->insert(vit, BowVector::value_type(id, v)); + } +} + +// -------------------------------------------------------------------------- + +void BowVector::normalize(LNorm norm_type) { + double norm = 0.0; + BowVector::iterator it; + + if (norm_type == DBoW3::L1) { + for (it = begin(); it != end(); ++it) norm += fabs(it->second); + } else { + for (it = begin(); it != end(); ++it) norm += it->second * it->second; + norm = sqrt(norm); + } + + if (norm > 0.0) { + for (it = begin(); it != end(); ++it) it->second /= norm; + } +} + +// -------------------------------------------------------------------------- + +std::ostream &operator<<(std::ostream &out, const BowVector &v) { + BowVector::const_iterator vit; + // std::vector::const_iterator iit; + unsigned int i = 0; + const size_t N = v.size(); + for (vit = v.begin(); vit != v.end(); ++vit, ++i) { + out << "<" << vit->first << ", " << vit->second << ">"; + + if (i < N - 1) out << ", "; + } + return out; +} + +// -------------------------------------------------------------------------- + +void BowVector::saveM(const std::string &filename, size_t W) const { + std::fstream f(filename.c_str(), std::ios::out); + + WordId last = 0; + BowVector::const_iterator bit; + for (bit = this->begin(); bit != this->end(); ++bit) { + for (; last < bit->first; ++last) { + f << "0 "; + } + f << bit->second << " "; + + last = bit->first + 1; + } + for (; last < (WordId)W; ++last) f << "0 "; + + f.close(); +} +// -------------------------------------------------------------------------- + +void BowVector::toStream(std::ostream &str) const { + uint32_t s = size(); + str.write((char *)&s, sizeof(s)); + for (auto d : *this) { + str.write((char *)&d.first, sizeof(d.first)); + str.write((char *)&d.second, sizeof(d.second)); + } +} +// -------------------------------------------------------------------------- + +void BowVector::fromStream(std::istream &str) { + clear(); + uint32_t s; + + str.read((char *)&s, sizeof(s)); + for (uint32_t i = 0; i < s; i++) { + WordId wid; + WordValue wv; + str.read((char *)&wid, sizeof(wid)); + str.read((char *)&wv, sizeof(wv)); + insert(std::make_pair(wid, wv)); + } +} + +uint64_t BowVector::getSignature() const { + uint64_t sig = 0; + for (auto ww : *this) sig += ww.first + 1e6 * ww.second; + return sig; +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW3 diff --git a/thirdparty/DBoW3/src/BowVector.h b/thirdparty/DBoW3/src/BowVector.h new file mode 100644 index 0000000..d8c17e0 --- /dev/null +++ b/thirdparty/DBoW3/src/BowVector.h @@ -0,0 +1,117 @@ +/** + * File: BowVector.h + * Date: March 2011 + * Author: Dorian Galvez-Lopez + * Description: bag of words vector + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_BOW_VECTOR__ +#define __D_T_BOW_VECTOR__ + +#include +#include +#include "exports.h" +#if _WIN32 +#include +#endif +namespace DBoW3 { + +/// Id of words +typedef unsigned int WordId; + +/// Value of a word +typedef double WordValue; + +/// Id of nodes in the vocabulary tree +typedef unsigned int NodeId; + +/// L-norms for normalization +enum LNorm +{ + L1, + L2 +}; + +/// Weighting type +enum WeightingType +{ + TF_IDF, + TF, + IDF, + BINARY +}; + +/// Scoring type +enum ScoringType +{ + L1_NORM, + L2_NORM, + CHI_SQUARE, + KL, + BHATTACHARYYA, + DOT_PRODUCT +}; + +/// Vector of words to represent images +class DBOW_API BowVector: + public std::map +{ +public: + + /** + * Constructor + */ + BowVector(void); + + /** + * Destructor + */ + ~BowVector(void); + + /** + * Adds a value to a word value existing in the vector, or creates a new + * word with the given value + * @param id word id to look for + * @param v value to create the word with, or to add to existing word + */ + void addWeight(WordId id, WordValue v); + + /** + * Adds a word with a value to the vector only if this does not exist yet + * @param id word id to look for + * @param v value to give to the word if this does not exist + */ + void addIfNotExist(WordId id, WordValue v); + + /** + * L1-Normalizes the values in the vector + * @param norm_type norm used + */ + void normalize(LNorm norm_type); + + /** + * Prints the content of the bow vector + * @param out stream + * @param v + */ + friend std::ostream& operator<<(std::ostream &out, const BowVector &v); + + /** + * Saves the bow vector as a vector in a matlab file + * @param filename + * @param W number of words in the vocabulary + */ + void saveM(const std::string &filename, size_t W) const; + + //returns a unique number from the configuration + uint64_t getSignature()const; + //serialization + void toStream(std::ostream &str)const; + void fromStream(std::istream &str); +}; + +} // namespace DBoW3 + +#endif diff --git a/thirdparty/DBoW3/src/CMakeLists.txt b/thirdparty/DBoW3/src/CMakeLists.txt new file mode 100644 index 0000000..7d1019b --- /dev/null +++ b/thirdparty/DBoW3/src/CMakeLists.txt @@ -0,0 +1,20 @@ +FILE(GLOB hdrs_base "*.h" ) +FILE(GLOB srcs_base "*.c*") + +FILE(GLOB hdrs ${hdrs_base} ) +FILE(GLOB srcs ${srcs_base} ) + + +ADD_LIBRARY(${PROJECT_NAME} ${srcs} ${hdrs}) +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR} ) + +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES # create *nix style library versions + symbolic links + DEFINE_SYMBOL DBOW_DSO_EXPORTS + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_SOVERSION} + CLEAN_DIRECT_OUTPUT 1 # allow creating static and shared libs without conflicts + OUTPUT_NAME "${PROJECT_NAME}${PROJECT_DLLVERSION}" # avoid conflicts between library and binary target names +) + +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${REQUIRED_LIBRARIES} ${OpenCV_LIBS}) + diff --git a/thirdparty/DBoW3/src/DBoW3.h b/thirdparty/DBoW3/src/DBoW3.h new file mode 100644 index 0000000..aabeadb --- /dev/null +++ b/thirdparty/DBoW3/src/DBoW3.h @@ -0,0 +1,68 @@ +/* + * File: DBoW3.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: Generic include file for the DBoW3 classes and + * the specialized vocabularies and databases + * License: see the LICENSE.txt file + * + */ + +/*! \mainpage DBoW3 Library + * + * DBoW3 library for C++: + * Bag-of-word image database for image retrieval. + * + * Written by Rafael Muñoz Salinas, + * University of Cordoba (Spain) + * + * + * \section requirements Requirements + * This library requires the OpenCV libraries, + * as well as the boost::dynamic_bitset class. + * + * \section citation Citation + * If you use this software in academic works, please cite: + 
+   @@ARTICLE{GalvezTRO12,
+    author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
+    journal={IEEE Transactions on Robotics},
+    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
+    year={2012},
+    month={October},
+    volume={28},
+    number={5},
+    pages={1188--1197},
+    doi={10.1109/TRO.2012.2197158},
+    ISSN={1552-3098}
+  }
+
+ * + * \section license License + * This file is licensed under a Creative Commons + * Attribution-NonCommercial-ShareAlike 3.0 license. + * This file can be freely used and users can use, download and edit this file + * provided that credit is attributed to the original author. No users are + * permitted to use this file for commercial purposes unless explicit permission + * is given by the original author. Derivative works must be licensed using the + * same or similar license. + * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further + * details. + * + */ + +#ifndef __D_T_DBOW3__ +#define __D_T_DBOW3__ + +/// Includes all the data structures to manage vocabularies and image databases + +#include "Vocabulary.h" +#include "Database.h" +#include "BowVector.h" +#include "FeatureVector.h" +#include "QueryResults.h" + + + +#endif + diff --git a/thirdparty/DBoW3/src/Database.cpp b/thirdparty/DBoW3/src/Database.cpp new file mode 100644 index 0000000..c31bd74 --- /dev/null +++ b/thirdparty/DBoW3/src/Database.cpp @@ -0,0 +1,855 @@ +#include "Database.h" + +#include + +namespace DBoW3 { + +// For query functions +static int MIN_COMMON_WORDS = 5; + +// -------------------------------------------------------------------------- + +Database::Database(bool use_di, int di_levels) + : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels), m_nentries(0) {} + +// -------------------------------------------------------------------------- + +Database::Database(const Vocabulary &voc, bool use_di, int di_levels) + : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels) { + setVocabulary(voc); + clear(); +} + +// -------------------------------------------------------------------------- + +Database::Database(const Database &db) : m_voc(NULL) { *this = db; } + +// -------------------------------------------------------------------------- + +Database::Database(const std::string &filename) : m_voc(NULL) { + load(filename); +} + +// -------------------------------------------------------------------------- + +Database::Database(const char *filename) : m_voc(NULL) { load(filename); } + +// -------------------------------------------------------------------------- + +Database::~Database(void) { delete m_voc; } + +// -------------------------------------------------------------------------- + +Database &Database::operator=(const Database &db) { + if (this != &db) { + m_dfile = db.m_dfile; + m_dilevels = db.m_dilevels; + m_ifile = db.m_ifile; + m_nentries = db.m_nentries; + m_use_di = db.m_use_di; + if (db.m_voc != 0) setVocabulary(*db.m_voc); + } + return *this; +} + +// -------------------------------------------------------------------------- + +EntryId Database::add(const cv::Mat &features, BowVector *bowvec, + FeatureVector *fvec) { + std::vector vf(features.rows); + for (int r = 0; r < features.rows; r++) vf[r] = features.rowRange(r, r + 1); + return add(vf, bowvec, fvec); +} + +EntryId Database::add(const std::vector &features, BowVector *bowvec, + FeatureVector *fvec) { + BowVector aux; + BowVector &v = (bowvec ? *bowvec : aux); + + if (m_use_di && fvec != NULL) { + m_voc->transform(features, v, *fvec, m_dilevels); // with features + return add(v, *fvec); + } else if (m_use_di) { + FeatureVector fv; + m_voc->transform(features, v, fv, m_dilevels); // with features + return add(v, fv); + } else if (fvec != NULL) { + m_voc->transform(features, v, *fvec, m_dilevels); // with features + return add(v); + } else { + m_voc->transform(features, v); // with features + return add(v); + } +} + +EntryId Database::add(const std::vector> &features, + BowVector *bowvec, FeatureVector *fvec) { + BowVector aux; + BowVector &v = (bowvec ? *bowvec : aux); + + if (m_use_di && fvec != NULL) { + m_voc->transform(features, v, *fvec, m_dilevels); // with features + return add(v, *fvec); + } else if (m_use_di) { + FeatureVector fv; + m_voc->transform(features, v, fv, m_dilevels); // with features + return add(v, fv); + } else if (fvec != NULL) { + m_voc->transform(features, v, *fvec, m_dilevels); // with features + return add(v); + } else { + m_voc->transform(features, v); // with features + return add(v); + } +} + +// --------------------------------------------------------------------------- + +EntryId Database::add(const BowVector &v, const FeatureVector &fv) { + EntryId entry_id = m_nentries++; + + BowVector::const_iterator vit; + // std::vector::const_iterator iit; + + if (m_use_di) { + // update direct file + if (entry_id == m_dfile.size()) { + m_dfile.push_back(fv); + } else { + m_dfile[entry_id] = fv; + } + } + + // update inverted file + for (vit = v.begin(); vit != v.end(); ++vit) { + const WordId &word_id = vit->first; + const WordValue &word_weight = vit->second; + + IFRow &ifrow = m_ifile[word_id]; + ifrow.push_back(IFPair(entry_id, word_weight)); + } + + return entry_id; +} + +// -------------------------------------------------------------------------- + +void Database::setVocabulary(const Vocabulary &voc) { + delete m_voc; + m_voc = new Vocabulary(voc); + clear(); +} + +// -------------------------------------------------------------------------- + +void Database::setVocabulary(const Vocabulary &voc, bool use_di, + int di_levels) { + m_use_di = use_di; + m_dilevels = di_levels; + delete m_voc; + m_voc = new Vocabulary(voc); + clear(); +} + +// -------------------------------------------------------------------------- + +void Database::clear() { + // resize vectors + m_ifile.resize(0); + m_ifile.resize(m_voc->size()); + m_dfile.resize(0); + m_nentries = 0; +} + +// -------------------------------------------------------------------------- + +void Database::allocate(int nd, int ni) { + // m_ifile already contains |words| items + if (ni > 0) { + for (auto rit = m_ifile.begin(); rit != m_ifile.end(); ++rit) { + int n = (int)rit->size(); + if (ni > n) { + rit->resize(ni); + rit->resize(n); + } + } + } + + if (m_use_di && (int)m_dfile.size() < nd) { + m_dfile.resize(nd); + } +} + +// -------------------------------------------------------------------------- + +void Database::query(const cv::Mat &features, QueryResults &ret, + int max_results, int max_id) const { + std::vector vf(features.rows); + for (int r = 0; r < features.rows; r++) vf[r] = features.rowRange(r, r + 1); + query(vf, ret, max_results, max_id); +} + +void Database::query(const std::vector &features, QueryResults &ret, + int max_results, int max_id) const { + BowVector vec; + m_voc->transform(features, vec); + query(vec, ret, max_results, max_id); +} + +void Database::query(const std::vector> &features, + QueryResults &ret, int max_results, int max_id) const { + BowVector vec; + m_voc->transform(features, vec); + query(vec, ret, max_results, max_id); +} + +// -------------------------------------------------------------------------- + +void Database::query(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const { + ret.resize(0); + + switch (m_voc->getScoringType()) { + case L1_NORM: + queryL1(vec, ret, max_results, max_id); + break; + + case L2_NORM: + queryL2(vec, ret, max_results, max_id); + break; + + case CHI_SQUARE: + queryChiSquare(vec, ret, max_results, max_id); + break; + + case KL: + queryKL(vec, ret, max_results, max_id); + break; + + case BHATTACHARYYA: + queryBhattacharyya(vec, ret, max_results, max_id); + break; + + case DOT_PRODUCT: + queryDotProduct(vec, ret, max_results, max_id); + break; + } +} + +// -------------------------------------------------------------------------- + +void Database::queryL1(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const { + BowVector::const_iterator vit; + + std::unordered_map pairs; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordId word_id = vit->first; + const WordValue &qvalue = vit->second; + + const IFRow &row = m_ifile[word_id]; + + // IFRows are sorted in ascending entry_id order + + for (const auto &rit : row) { + const EntryId &entry_id = rit.entry_id; + const WordValue &dvalue = rit.word_weight; + + if ((int)entry_id < max_id || max_id == -1) { + double value = fabs(qvalue - dvalue) - fabs(qvalue) - fabs(dvalue); + + auto it = pairs.find(entry_id); + if (it != pairs.end()) { + it->second += value; + } else { + pairs.emplace(entry_id, value); + } + } + + } // for each inverted row + } // for each query word + + // move to vector + ret.reserve(pairs.size()); + for (const auto &pit : pairs) { + ret.push_back(Result(pit.first, pit.second)); + } + + // resulting "scores" are now in [-2 best .. 0 worst] + + // sort vector in ascending order of score + std::sort(ret.begin(), ret.end()); + // (ret is inverted now --the lower the better--) + + // cut vector + if (max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); + + // complete and scale score to [0 worst .. 1 best] + // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) + // for all i | v_i != 0 and w_i != 0 + // (Nister, 2006) + // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} + QueryResults::iterator qit; + for (qit = ret.begin(); qit != ret.end(); qit++) + qit->Score = -qit->Score / 2.0; +} + +// -------------------------------------------------------------------------- + +void Database::queryL2(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const { + BowVector::const_iterator vit; + + std::map pairs; + std::map::iterator pit; + + // map counters; + // map::iterator cit; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordId word_id = vit->first; + const WordValue &qvalue = vit->second; + + const IFRow &row = m_ifile[word_id]; + + // IFRows are sorted in ascending entry_id order + + for (auto rit = row.begin(); rit != row.end(); ++rit) { + const EntryId entry_id = rit->entry_id; + const WordValue &dvalue = rit->word_weight; + + if ((int)entry_id < max_id || max_id == -1) { + double value = -qvalue * dvalue; // minus sign for sorting trick + + pit = pairs.lower_bound(entry_id); + // cit = counters.lower_bound(entry_id); + if (pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { + pit->second += value; + // cit->second += 1; + } else { + pairs.insert(pit, + std::map::value_type(entry_id, value)); + + // counters.insert(cit, + // map::value_type(entry_id, 1)); + } + } + + } // for each inverted row + } // for each query word + + // move to vector + ret.reserve(pairs.size()); + // cit = counters.begin(); + for (pit = pairs.begin(); pit != pairs.end(); ++pit) //, ++cit) + { + ret.push_back(Result(pit->first, pit->second)); // / cit->second)); + } + + // resulting "scores" are now in [-1 best .. 0 worst] + + // sort vector in ascending order of score + std::sort(ret.begin(), ret.end()); + // (ret is inverted now --the lower the better--) + + // cut vector + if (max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); + + // complete and scale score to [0 worst .. 1 best] + // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) + // for all i | v_i != 0 and w_i != 0 ) + // (Nister, 2006) + QueryResults::iterator qit; + for (qit = ret.begin(); qit != ret.end(); qit++) { + if (qit->Score <= -1.0) // rounding error + qit->Score = 1.0; + else + qit->Score = 1.0 - sqrt(1.0 + qit->Score); // [0..1] + // the + sign is ok, it is due to - sign in + // value = - qvalue * dvalue + } +} + +// -------------------------------------------------------------------------- + +void Database::queryChiSquare(const BowVector &vec, QueryResults &ret, + int max_results, int max_id) const { + BowVector::const_iterator vit; + + std::map> pairs; + std::map>::iterator pit; + + std::map> sums; // < sum vi, sum wi > + std::map>::iterator sit; + + // In the current implementation, we suppose vec is not normalized + + // map expected; + // map::iterator eit; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordId word_id = vit->first; + const WordValue &qvalue = vit->second; + + const IFRow &row = m_ifile[word_id]; + + // IFRows are sorted in ascending entry_id order + + for (auto rit = row.begin(); rit != row.end(); ++rit) { + const EntryId entry_id = rit->entry_id; + const WordValue &dvalue = rit->word_weight; + + if ((int)entry_id < max_id || max_id == -1) { + // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) + // we move the 4 out + double value = 0; + if (qvalue + dvalue != 0.0) // words may have weight zero + value = -qvalue * dvalue / (qvalue + dvalue); + + pit = pairs.lower_bound(entry_id); + sit = sums.lower_bound(entry_id); + // eit = expected.lower_bound(entry_id); + if (pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { + pit->second.first += value; + pit->second.second += 1; + // eit->second += dvalue; + sit->second.first += qvalue; + sit->second.second += dvalue; + } else { + pairs.insert(pit, + std::map>::value_type( + entry_id, std::make_pair(value, 1))); + // expected.insert(eit, + // map::value_type(entry_id, dvalue)); + + sums.insert(sit, + std::map>::value_type( + entry_id, std::make_pair(qvalue, dvalue))); + } + } + + } // for each inverted row + } // for each query word + + // move to vector + ret.reserve(pairs.size()); + sit = sums.begin(); + for (pit = pairs.begin(); pit != pairs.end(); ++pit, ++sit) { + if (pit->second.second >= MIN_COMMON_WORDS) { + ret.push_back(Result(pit->first, pit->second.first)); + ret.back().nWords = pit->second.second; + ret.back().sumCommonVi = sit->second.first; + ret.back().sumCommonWi = sit->second.second; + ret.back().expectedChiScore = + 2 * sit->second.second / (1 + sit->second.second); + } + + // ret.push_back(Result(pit->first, pit->second)); + } + + // resulting "scores" are now in [-2 best .. 0 worst] + // we have to add +2 to the scores to obtain the chi square score + + // sort vector in ascending order of score + std::sort(ret.begin(), ret.end()); + // (ret is inverted now --the lower the better--) + + // cut vector + if (max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); + + // complete and scale score to [0 worst .. 1 best] + QueryResults::iterator qit; + for (qit = ret.begin(); qit != ret.end(); qit++) { + // this takes the 4 into account + qit->Score = -2. * qit->Score; // [0..1] + + qit->chiScore = qit->Score; + } +} + +// -------------------------------------------------------------------------- + +void Database::queryKL(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const { + BowVector::const_iterator vit; + + std::map pairs; + std::map::iterator pit; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordId word_id = vit->first; + const WordValue &vi = vit->second; + + const IFRow &row = m_ifile[word_id]; + + // IFRows are sorted in ascending entry_id order + + for (auto rit = row.begin(); rit != row.end(); ++rit) { + const EntryId entry_id = rit->entry_id; + const WordValue &wi = rit->word_weight; + + if ((int)entry_id < max_id || max_id == -1) { + double value = 0; + if (vi != 0 && wi != 0) value = vi * log(vi / wi); + + pit = pairs.lower_bound(entry_id); + if (pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { + pit->second += value; + } else { + pairs.insert(pit, + std::map::value_type(entry_id, value)); + } + } + + } // for each inverted row + } // for each query word + + // resulting "scores" are now in [-X worst .. 0 best .. X worst] + // but we cannot make sure which ones are better without calculating + // the complete score + + // complete scores and move to vector + ret.reserve(pairs.size()); + for (pit = pairs.begin(); pit != pairs.end(); ++pit) { + EntryId eid = pit->first; + double value = 0.0; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordValue &vi = vit->second; + const IFRow &row = m_ifile[vit->first]; + + if (vi != 0) { + if (row.end() == find(row.begin(), row.end(), eid)) { + value += vi * (log(vi) - GeneralScoring::LOG_EPS); + } + } + } + + pit->second += value; + + // to vector + ret.push_back(Result(pit->first, pit->second)); + } + + // real scores are now in [0 best .. X worst] + + // sort vector in ascending order + // (scores are inverted now --the lower the better--) + std::sort(ret.begin(), ret.end()); + + // cut vector + if (max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); + + // cannot scale scores +} + +// -------------------------------------------------------------------------- + +void Database::queryBhattacharyya(const BowVector &vec, QueryResults &ret, + int max_results, int max_id) const { + BowVector::const_iterator vit; + + // map pairs; + // map::iterator pit; + + std::map> pairs; // > + std::map>::iterator pit; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordId word_id = vit->first; + const WordValue &qvalue = vit->second; + + const IFRow &row = m_ifile[word_id]; + + // IFRows are sorted in ascending entry_id order + + for (auto rit = row.begin(); rit != row.end(); ++rit) { + const EntryId entry_id = rit->entry_id; + const WordValue &dvalue = rit->word_weight; + + if ((int)entry_id < max_id || max_id == -1) { + double value = sqrt(qvalue * dvalue); + + pit = pairs.lower_bound(entry_id); + if (pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { + pit->second.first += value; + pit->second.second += 1; + } else { + pairs.insert(pit, + std::map>::value_type( + entry_id, std::make_pair(value, 1))); + } + } + + } // for each inverted row + } // for each query word + + // move to vector + ret.reserve(pairs.size()); + for (pit = pairs.begin(); pit != pairs.end(); ++pit) { + if (pit->second.second >= MIN_COMMON_WORDS) { + ret.push_back(Result(pit->first, pit->second.first)); + ret.back().nWords = pit->second.second; + ret.back().bhatScore = pit->second.first; + } + } + + // scores are already in [0..1] + + // sort vector in descending order + std::sort(ret.begin(), ret.end(), Result::gt); + + // cut vector + if (max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); +} + +// --------------------------------------------------------------------------- + +void Database::queryDotProduct(const BowVector &vec, QueryResults &ret, + int max_results, int max_id) const { + BowVector::const_iterator vit; + + std::map pairs; + std::map::iterator pit; + + for (vit = vec.begin(); vit != vec.end(); ++vit) { + const WordId word_id = vit->first; + const WordValue &qvalue = vit->second; + + const IFRow &row = m_ifile[word_id]; + + // IFRows are sorted in ascending entry_id order + + for (auto rit = row.begin(); rit != row.end(); ++rit) { + const EntryId entry_id = rit->entry_id; + const WordValue &dvalue = rit->word_weight; + + if ((int)entry_id < max_id || max_id == -1) { + double value; + if (this->m_voc->getWeightingType() == BINARY) + value = 1; + else + value = qvalue * dvalue; + + pit = pairs.lower_bound(entry_id); + if (pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { + pit->second += value; + } else { + pairs.insert(pit, + std::map::value_type(entry_id, value)); + } + } + + } // for each inverted row + } // for each query word + + // move to vector + ret.reserve(pairs.size()); + for (pit = pairs.begin(); pit != pairs.end(); ++pit) { + ret.push_back(Result(pit->first, pit->second)); + } + + // scores are the greater the better + + // sort vector in descending order + std::sort(ret.begin(), ret.end(), Result::gt); + + // cut vector + if (max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); + + // these scores cannot be scaled +} + +// --------------------------------------------------------------------------- + +const FeatureVector &Database::retrieveFeatures(EntryId id) const { + assert(id < size()); + return m_dfile[id]; +} + +// -------------------------------------------------------------------------- + +void Database::save(const std::string &filename) const { + cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); + if (!fs.isOpened()) throw std::string("Could not open file ") + filename; + + save(fs); +} + +// -------------------------------------------------------------------------- + +void Database::save(cv::FileStorage &fs, const std::string &name) const { + // Format YAML: + // vocabulary { ... see TemplatedVocabulary::save } + // database + // { + // nEntries: + // usingDI: + // diLevels: + // invertedIndex + // [ + // [ + // { + // imageId: + // weight: + // } + // ] + // ] + // directIndex + // [ + // [ + // { + // nodeId: + // features: [ ] + // } + // ] + // ] + + // invertedIndex[i] is for the i-th word + // directIndex[i] is for the i-th entry + // directIndex may be empty if not using direct index + // + // imageId's and nodeId's must be stored in ascending order + // (according to the construction of the indexes) + + m_voc->save(fs); + + fs << name << "{"; + + fs << "nEntries" << m_nentries; + fs << "usingDI" << (m_use_di ? 1 : 0); + fs << "diLevels" << m_dilevels; + + fs << "invertedIndex" + << "["; + + for (auto iit = m_ifile.begin(); iit != m_ifile.end(); ++iit) { + fs << "["; // word of IF + for (auto irit = iit->begin(); irit != iit->end(); ++irit) { + fs << "{:" + << "imageId" << (int)irit->entry_id << "weight" << irit->word_weight + << "}"; + } + fs << "]"; // word of IF + } + + fs << "]"; // invertedIndex + + fs << "directIndex" + << "["; + + for (auto dit = m_dfile.begin(); dit != m_dfile.end(); ++dit) { + fs << "["; // entry of DF + + for (auto drit = dit->begin(); drit != dit->end(); ++drit) { + NodeId nid = drit->first; + const std::vector &features = drit->second; + + // save info of last_nid + fs << "{"; + fs << "nodeId" << (int)nid; + // msvc++ 2010 with opencv 2.3.1 does not allow FileStorage::operator<< + // with vectors of unsigned int + fs << "features" + << "[" << *(const std::vector *)(&features) << "]"; + fs << "}"; + } + + fs << "]"; // entry of DF + } + + fs << "]"; // directIndex + + fs << "}"; // database +} + +// -------------------------------------------------------------------------- + +void Database::load(const std::string &filename) { + cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); + if (!fs.isOpened()) throw std::string("Could not open file ") + filename; + + load(fs); +} + +// -------------------------------------------------------------------------- + +void Database::load(const cv::FileStorage &fs, const std::string &name) { + // load voc first + // subclasses must instantiate m_voc before calling this ::load + if (!m_voc) m_voc = new Vocabulary; + + m_voc->load(fs); + + // load database now + clear(); // resizes inverted file + + cv::FileNode fdb = fs[name]; + + m_nentries = (int)fdb["nEntries"]; + m_use_di = (int)fdb["usingDI"] != 0; + m_dilevels = (int)fdb["diLevels"]; + + cv::FileNode fn = fdb["invertedIndex"]; + for (WordId wid = 0; wid < fn.size(); ++wid) { + cv::FileNode fw = fn[wid]; + + for (unsigned int i = 0; i < fw.size(); ++i) { + EntryId eid = (int)fw[i]["imageId"]; + WordValue v = fw[i]["weight"]; + + m_ifile[wid].push_back(IFPair(eid, v)); + } + } + + if (m_use_di) { + fn = fdb["directIndex"]; + + m_dfile.resize(fn.size()); + assert(m_nentries == (int)fn.size()); + + FeatureVector::iterator dit; + for (EntryId eid = 0; eid < fn.size(); ++eid) { + cv::FileNode fe = fn[eid]; + + m_dfile[eid].clear(); + for (unsigned int i = 0; i < fe.size(); ++i) { + NodeId nid = (int)fe[i]["nodeId"]; + + dit = m_dfile[eid].insert(m_dfile[eid].end(), + make_pair(nid, std::vector())); + + // this failed to compile with some opencv versions (2.3.1) + // fe[i]["features"] >> dit->second; + + // this was ok until OpenCV 2.4.1 + // std::vector aux; + // fe[i]["features"] >> aux; // OpenCV < 2.4.1 + // dit->second.resize(aux.size()); + // std::copy(aux.begin(), aux.end(), dit->second.begin()); + + cv::FileNode ff = fe[i]["features"][0]; + dit->second.reserve(ff.size()); + + cv::FileNodeIterator ffit; + for (ffit = ff.begin(); ffit != ff.end(); ++ffit) { + dit->second.push_back((int)*ffit); + } + } + } // for each entry + } // if use_id +} + +std::ostream &operator<<(std::ostream &os, const Database &db) { + os << "Database: Entries = " << db.size() << ", " + "Using direct index = " + << (db.usingDirectIndex() ? "yes" : "no"); + + if (db.usingDirectIndex()) + os << ", Direct index levels = " << db.getDirectIndexLevels(); + + os << ". " << *db.getVocabulary(); + return os; +} +} // namespace DBoW3 diff --git a/thirdparty/DBoW3/src/Database.h b/thirdparty/DBoW3/src/Database.h new file mode 100644 index 0000000..2397404 --- /dev/null +++ b/thirdparty/DBoW3/src/Database.h @@ -0,0 +1,354 @@ +/** + * File: Database.h + * Date: March 2011 + * Modified By Rafael Muñoz in 2016 + * Author: Dorian Galvez-Lopez + * Description: database of images + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_DATABASE__ +#define __D_T_DATABASE__ + +#include +#include +#include +#include +#include +#include +#include + +#include "BowVector.h" +#include "FeatureVector.h" +#include "QueryResults.h" +#include "ScoringObject.h" +#include "Vocabulary.h" +#include "exports.h" + +namespace DBoW3 { + +/// Database +class DBOW_API Database { + public: + /** + * Creates an empty database without vocabulary + * @param use_di a direct index is used to store feature indexes + * @param di_levels levels to go up the vocabulary tree to select the + * node id to store in the direct index when adding images + */ + explicit Database(bool use_di = true, int di_levels = 0); + + /** + * Creates a database with the given vocabulary + * @param T class inherited from Vocabulary + * @param voc vocabulary + * @param use_di a direct index is used to store feature indexes + * @param di_levels levels to go up the vocabulary tree to select the + * node id to store in the direct index when adding images + */ + + explicit Database(const Vocabulary &voc, bool use_di = true, + int di_levels = 0); + + /** + * Copy constructor. Copies the vocabulary too + * @param db object to copy + */ + Database(const Database &db); + + /** + * Creates the database from a file + * @param filename + */ + Database(const std::string &filename); + + /** + * Creates the database from a file + * @param filename + */ + Database(const char *filename); + + /** + * Destructor + */ + virtual ~Database(void); + + /** + * Copies the given database and its vocabulary + * @param db database to copy + */ + Database &operator=(const Database &db); + + /** + * Sets the vocabulary to use and clears the content of the database. + * @param T class inherited from Vocabulary + * @param voc vocabulary to copy + */ + void setVocabulary(const Vocabulary &voc); + + /** + * Sets the vocabulary to use and the direct index parameters, and clears + * the content of the database + * @param T class inherited from Vocabulary + * @param voc vocabulary to copy + * @param use_di a direct index is used to store feature indexes + * @param di_levels levels to go up the vocabulary tree to select the + * node id to store in the direct index when adding images + */ + + void setVocabulary(const Vocabulary &voc, bool use_di, int di_levels = 0); + + /** + * Returns a pointer to the vocabulary used + * @return vocabulary + */ + inline const Vocabulary *getVocabulary() const { return m_voc; } + + /** + * Allocates some memory for the direct and inverted indexes + * @param nd number of expected image entries in the database + * @param ni number of expected words per image + * @note Use 0 to ignore a parameter + */ + void allocate(int nd = 0, int ni = 0); + + /** + * Adds an entry to the database and returns its index + * @param features features of the new entry + * @param bowvec if given, the bow vector of these features is returned + * @param fvec if given, the vector of nodes and feature indexes is returned + * @return id of new entry + */ + EntryId add(const std::vector> &features, + BowVector *bowvec = NULL, FeatureVector *fvec = NULL); + + /** + * Adds an entry to the database and returns its index + * @param features features of the new entry + * @param bowvec if given, the bow vector of these features is returned + * @param fvec if given, the vector of nodes and feature indexes is returned + * @return id of new entry + */ + EntryId add(const std::vector &features, BowVector *bowvec = NULL, + FeatureVector *fvec = NULL); + /** + * Adds an entry to the database and returns its index + * @param features features of the new entry, one per row + * @param bowvec if given, the bow vector of these features is returned + * @param fvec if given, the vector of nodes and feature indexes is returned + * @return id of new entry + */ + EntryId add(const cv::Mat &features, BowVector *bowvec = NULL, + FeatureVector *fvec = NULL); + + /** + * Adss an entry to the database and returns its index + * @param vec bow vector + * @param fec feature vector to add the entry. Only necessary if using the + * direct index + * @return id of new entry + */ + EntryId add(const BowVector &vec, const FeatureVector &fec = FeatureVector()); + + /** + * Empties the database + */ + inline void clear(); + + /** + * Returns the number of entries in the database + * @return number of entries in the database + */ + unsigned int size() const { return m_nentries; } + + /** + * Checks if the direct index is being used + * @return true iff using direct index + */ + bool usingDirectIndex() const { return m_use_di; } + + /** + * Returns the di levels when using direct index + * @return di levels + */ + int getDirectIndexLevels() const { return m_dilevels; } + + /** + * Queries the database with some features + * @param features query features + * @param ret (out) query results + * @param max_results number of results to return. <= 0 means all + * @param max_id only entries with id <= max_id are returned in ret. + * < 0 means all + */ + void query(const std::vector &features, QueryResults &ret, + int max_results = 1, int max_id = -1) const; + + void query(const std::vector> &features, QueryResults &ret, + int max_results = 1, int max_id = -1) const; + + /** + * Queries the database with some features + * @param features query features,one per row + * @param ret (out) query results + * @param max_results number of results to return. <= 0 means all + * @param max_id only entries with id <= max_id are returned in ret. + * < 0 means all + */ + void query(const cv::Mat &features, QueryResults &ret, int max_results = 1, + int max_id = -1) const; + + /** + * Queries the database with a vector + * @param vec bow vector already normalized + * @param ret results + * @param max_results number of results to return. <= 0 means all + * @param max_id only entries with id <= max_id are returned in ret. + * < 0 means all + */ + void query(const BowVector &vec, QueryResults &ret, int max_results = 1, + int max_id = -1) const; + + /** + * Returns the a feature vector associated with a database entry + * @param id entry id (must be < size()) + * @return const reference to map of nodes and their associated features in + * the given entry + */ + const FeatureVector &retrieveFeatures(EntryId id) const; + + /** + * Stores the database in a file + * @param filename + */ + void save(const std::string &filename) const; + + /** + * Loads the database from a file + * @param filename + */ + void load(const std::string &filename); + + /** + * Stores the database in the given file storage structure + * @param fs + * @param name node name + */ + virtual void save(cv::FileStorage &fs, + const std::string &name = "database") const; + + /** + * Loads the database from the given file storage structure + * @param fs + * @param name node name + */ + virtual void load(const cv::FileStorage &fs, + const std::string &name = "database"); + + // -------------------------------------------------------------------------- + + /** + * Writes printable information of the database + * @param os stream to write to + * @param db + */ + DBOW_API friend std::ostream &operator<<(std::ostream &os, + const Database &db); + + protected: + /// Query with L1 scoring + void queryL1(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const; + + /// Query with L2 scoring + void queryL2(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const; + + /// Query with Chi square scoring + void queryChiSquare(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const; + + /// Query with Bhattacharyya scoring + void queryBhattacharyya(const BowVector &vec, QueryResults &ret, + int max_results, int max_id) const; + + /// Query with KL divergence scoring + void queryKL(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const; + + /// Query with dot product scoring + void queryDotProduct(const BowVector &vec, QueryResults &ret, int max_results, + int max_id) const; + + protected: + /* Inverted file declaration */ + + /// Item of IFRow + struct IFPair { + /// Entry id + EntryId entry_id; + + /// Word weight in this entry + WordValue word_weight; + + /** + * Creates an empty pair + */ + IFPair() {} + + /** + * Creates an inverted file pair + * @param eid entry id + * @param wv word weight + */ + IFPair(EntryId eid, WordValue wv) : entry_id(eid), word_weight(wv) {} + + /** + * Compares the entry ids + * @param eid + * @return true iff this entry id is the same as eid + */ + inline bool operator==(EntryId eid) const { return entry_id == eid; } + }; + + /// Row of InvertedFile + typedef std::vector IFRow; + // IFRows are sorted in ascending entry_id order + + /// Inverted index + typedef std::vector InvertedFile; + // InvertedFile[word_id] --> inverted file of that word + + /* Direct file declaration */ + + /// Direct index + typedef std::vector DirectFile; + // DirectFile[entry_id] --> [ directentry, ... ] + + protected: + /// Associated vocabulary + Vocabulary *m_voc; + + /// Flag to use direct index + bool m_use_di; + + /// Levels to go up the vocabulary tree to select nodes to store + /// in the direct index + int m_dilevels; + + /// Inverted file (must have size() == |words|) + InvertedFile m_ifile; + + /// Direct file (resized for allocation) + DirectFile m_dfile; + + /// Number of valid entries in m_dfile + int m_nentries; +}; + +// -------------------------------------------------------------------------- + +} // namespace DBoW3 + +#endif diff --git a/thirdparty/DBoW3/src/DescManip.cpp b/thirdparty/DBoW3/src/DescManip.cpp new file mode 100644 index 0000000..aa0553d --- /dev/null +++ b/thirdparty/DBoW3/src/DescManip.cpp @@ -0,0 +1,239 @@ +/** + * File: DescManip.cpp + * Date: June 2012 + * Author: Dorian Galvez-Lopez + * Description: functions for ORB descriptors + * License: see the LICENSE.txt file + * + */ + +#include +#include +#include +#include +#include +#include + +#include "DescManip.h" + +using namespace std; + +namespace DBoW3 { + +// -------------------------------------------------------------------------- + +void DescManip::meanValue(const std::vector &descriptors, + cv::Mat &mean) { + if (descriptors.empty()) return; + + if (descriptors.size() == 1) { + mean = descriptors[0].clone(); + return; + } + // binary descriptor + if (descriptors[0].type() == CV_8U) { + // determine number of bytes of the binary descriptor + int L = getDescSizeBytes(descriptors[0]); + vector sum(L * 8, 0); + + for (size_t i = 0; i < descriptors.size(); ++i) { + const cv::Mat &d = descriptors[i]; + const unsigned char *p = d.ptr(); + + for (int j = 0; j < d.cols; ++j, ++p) { + if (*p & (1 << 7)) ++sum[j * 8]; + if (*p & (1 << 6)) ++sum[j * 8 + 1]; + if (*p & (1 << 5)) ++sum[j * 8 + 2]; + if (*p & (1 << 4)) ++sum[j * 8 + 3]; + if (*p & (1 << 3)) ++sum[j * 8 + 4]; + if (*p & (1 << 2)) ++sum[j * 8 + 5]; + if (*p & (1 << 1)) ++sum[j * 8 + 6]; + if (*p & (1)) ++sum[j * 8 + 7]; + } + } + + mean = cv::Mat::zeros(1, L, CV_8U); + unsigned char *p = mean.ptr(); + + const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; + for (size_t i = 0; i < sum.size(); ++i) { + if (sum[i] >= N2) { + // set bit + *p |= 1 << (7 - (i % 8)); + } + + if (i % 8 == 7) ++p; + } + } + // non binary descriptor + else { + assert(descriptors[0].type() == CV_32F); // ensure it is float + + mean.create(1, descriptors[0].cols, descriptors[0].type()); + mean.setTo(cv::Scalar::all(0)); + float inv_s = 1. / double(descriptors.size()); + for (size_t i = 0; i < descriptors.size(); i++) + mean += descriptors[i] * inv_s; + } +} + +// -------------------------------------------------------------------------- +// static inline uint32_t distance_8uc1(const cv::Mat &a, const cv::Mat &b); + +double DescManip::distance(const cv::Mat &a, const cv::Mat &b) { + // binary descriptor + if (a.type() == CV_8U) { + // Bit count function got from: + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetKernighan + // This implementation assumes that a.cols (CV_8U) % sizeof(uint64_t) == 0 + + const uint64_t *pa, *pb; + pa = a.ptr(); // a & b are actually CV_8U + pb = b.ptr(); + + uint64_t v, ret = 0; + for (size_t i = 0; i < a.cols / sizeof(uint64_t); ++i, ++pa, ++pb) { + v = *pa ^ *pb; + v = v - ((v >> 1) & (uint64_t) ~(uint64_t)0 / 3); + v = (v & (uint64_t) ~(uint64_t)0 / 15 * 3) + + ((v >> 2) & (uint64_t) ~(uint64_t)0 / 15 * 3); + v = (v + (v >> 4)) & (uint64_t) ~(uint64_t)0 / 255 * 15; + ret += (uint64_t)(v * ((uint64_t) ~(uint64_t)0 / 255)) >> + (sizeof(uint64_t) - 1) * CHAR_BIT; + } + + return ret; + } else { + double sqd = 0.; + assert(a.type() == CV_32F); + assert(a.rows == 1); + const float *a_ptr = a.ptr(0); + const float *b_ptr = b.ptr(0); + for (int i = 0; i < a.cols; i++) + sqd += (a_ptr[i] - b_ptr[i]) * (a_ptr[i] - b_ptr[i]); + return sqd; + } +} + +// -------------------------------------------------------------------------- + +std::string DescManip::toString(const cv::Mat &a) { + stringstream ss; + // introduce a magic value to distinguish from DBoW3 + ss << "dbw3 "; + // save size and type + + ss << a.type() << " " << a.cols << " "; + + if (a.type() == CV_8U) { + const unsigned char *p = a.ptr(); + for (int i = 0; i < a.cols; ++i, ++p) ss << (int)*p << " "; + } else { + const float *p = a.ptr(); + for (int i = 0; i < a.cols; ++i, ++p) ss << *p << " "; + } + + return ss.str(); +} + +// -------------------------------------------------------------------------- + +void DescManip::fromString(cv::Mat &a, const std::string &s) { + // check if the dbow3 is present + string ss_aux; + ss_aux.reserve(10); + for (size_t i = 0; i < 10 && i < s.size(); i++) ss_aux.push_back(s[i]); + if (ss_aux.find("dbw3") == std::string::npos) { // is DBoW3 + // READ UNTIL END + stringstream ss(s); + int val; + vector data; + data.reserve(100); + while (ss >> val) data.push_back(val); + // copy to a + a.create(1, data.size(), CV_8UC1); + memcpy(a.ptr(0), &data[0], data.size()); + } else { + int type, cols; + stringstream ss(s); + ss >> type >> cols; + a.create(1, cols, type); + if (type == CV_8UC1) { + unsigned char *p = a.ptr(); + int n; + for (int i = 0; i < a.cols; ++i, ++p) + if (ss >> n) *p = (unsigned char)n; + } else { + float *p = a.ptr(); + for (int i = 0; i < a.cols; ++i, ++p) + if (!(ss >> *p)) + cerr << "Error reading. Unexpected EOF. DescManip::fromString" + << endl; + } + } +} + +// -------------------------------------------------------------------------- + +void DescManip::toMat32F(const std::vector &descriptors, + cv::Mat &mat) { + if (descriptors.empty()) { + mat.release(); + return; + } + + if (descriptors[0].type() == CV_8UC1) { + const size_t N = descriptors.size(); + int L = getDescSizeBytes(descriptors[0]); + mat.create(N, L * 8, CV_32F); + float *p = mat.ptr(); + + for (size_t i = 0; i < N; ++i) { + const int C = descriptors[i].cols; + const unsigned char *desc = descriptors[i].ptr(); + + for (int j = 0; j < C; ++j, p += 8) { + p[0] = (desc[j] & (1 << 7) ? 1 : 0); + p[1] = (desc[j] & (1 << 6) ? 1 : 0); + p[2] = (desc[j] & (1 << 5) ? 1 : 0); + p[3] = (desc[j] & (1 << 4) ? 1 : 0); + p[4] = (desc[j] & (1 << 3) ? 1 : 0); + p[5] = (desc[j] & (1 << 2) ? 1 : 0); + p[6] = (desc[j] & (1 << 1) ? 1 : 0); + p[7] = desc[j] & (1); + } + } + } else { + assert(descriptors[0].type() == CV_32F); + const int N = descriptors.size(); + int L = descriptors[0].cols; + mat.create(N, L, CV_32F); + for (int i = 0; i < N; ++i) + memcpy(mat.ptr(i), descriptors[i].ptr(0), + sizeof(float) * L); + } +} + +void DescManip::toStream(const cv::Mat &m, std::ostream &str) { + assert(m.rows == 1 || m.isContinuous()); + int type = m.type(); + int cols = m.cols; + int rows = m.rows; + str.write((char *)&cols, sizeof(cols)); + str.write((char *)&rows, sizeof(rows)); + str.write((char *)&type, sizeof(type)); + str.write((char *)m.ptr(0), m.elemSize() * m.cols); +} + +void DescManip::fromStream(cv::Mat &m, std::istream &str) { + int type, cols, rows; + str.read((char *)&cols, sizeof(cols)); + str.read((char *)&rows, sizeof(rows)); + str.read((char *)&type, sizeof(type)); + m.create(rows, cols, type); + str.read((char *)m.ptr(0), m.elemSize() * m.cols); +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW3 diff --git a/thirdparty/DBoW3/src/DescManip.h b/thirdparty/DBoW3/src/DescManip.h new file mode 100644 index 0000000..8281176 --- /dev/null +++ b/thirdparty/DBoW3/src/DescManip.h @@ -0,0 +1,99 @@ +/** + * File: FClass.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: generic FClass to instantiate templated classes + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_DESCMANIP__ +#define __D_T_DESCMANIP__ + +#include +#include +#include +#include "exports.h" + +namespace DBoW3 { + +/// Class to manipulate descriptors (calculating means, differences and IO +/// routines) +class DBOW_API DescManip { + public: + /** + * Calculates the mean value of a set of descriptors + * @param descriptors + * @param mean mean descriptor + */ + static void meanValue(const std::vector &descriptors, cv::Mat &mean); + + /** + * Calculates the distance between two descriptors + * @param a + * @param b + * @return distance + */ + static double distance(const cv::Mat &a, const cv::Mat &b); + static inline uint32_t distance_8uc1(const cv::Mat &a, const cv::Mat &b); + + /** + * Returns a string version of the descriptor + * @param a descriptor + * @return string version + */ + static std::string toString(const cv::Mat &a); + + /** + * Returns a descriptor from a string + * @param a descriptor + * @param s string version + */ + static void fromString(cv::Mat &a, const std::string &s); + + /** + * Returns a mat with the descriptors in float format + * @param descriptors + * @param mat (out) NxL 32F matrix + */ + static void toMat32F(const std::vector &descriptors, cv::Mat &mat); + + /**io routines*/ + static void toStream(const cv::Mat &m, std::ostream &str); + static void fromStream(cv::Mat &m, std::istream &str); + + public: + /**Returns the number of bytes of the descriptor + * used for binary descriptors only*/ + static size_t getDescSizeBytes(const cv::Mat &d) { + return d.cols * d.elemSize(); + } +}; + +uint32_t DescManip::distance_8uc1(const cv::Mat &a, const cv::Mat &b) { + // binary descriptor + + // Bit count function got from: + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetKernighan + // This implementation assumes that a.cols (CV_8U) % sizeof(uint64_t) == 0 + + const uint64_t *pa, *pb; + pa = a.ptr(); // a & b are actually CV_8U + pb = b.ptr(); + + uint64_t v, ret = 0; + size_t n = a.cols / sizeof(uint64_t); + for (size_t i = 0; i < n; ++i, ++pa, ++pb) { + v = *pa ^ *pb; + v = v - ((v >> 1) & (uint64_t) ~(uint64_t)0 / 3); + v = (v & (uint64_t) ~(uint64_t)0 / 15 * 3) + + ((v >> 2) & (uint64_t) ~(uint64_t)0 / 15 * 3); + v = (v + (v >> 4)) & (uint64_t) ~(uint64_t)0 / 255 * 15; + ret += (uint64_t)(v * ((uint64_t) ~(uint64_t)0 / 255)) >> + (sizeof(uint64_t) - 1) * CHAR_BIT; + } + return ret; +} +} // namespace DBoW3 + +#endif diff --git a/thirdparty/DBoW3/src/FeatureVector.cpp b/thirdparty/DBoW3/src/FeatureVector.cpp new file mode 100644 index 0000000..880eab1 --- /dev/null +++ b/thirdparty/DBoW3/src/FeatureVector.cpp @@ -0,0 +1,85 @@ +/** + * File: FeatureVector.cpp + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: feature vector + * License: see the LICENSE.txt file + * + */ + +#include "FeatureVector.h" +#include +#include +#include + +namespace DBoW3 { + +// --------------------------------------------------------------------------- + +FeatureVector::FeatureVector(void) +{ +} + +// --------------------------------------------------------------------------- + +FeatureVector::~FeatureVector(void) +{ +} + +// --------------------------------------------------------------------------- + +void FeatureVector::addFeature(NodeId id, unsigned int i_feature) +{ + FeatureVector::iterator vit = this->lower_bound(id); + + if(vit != this->end() && vit->first == id) + { + vit->second.push_back(i_feature); + } + else + { + vit = this->insert(vit, FeatureVector::value_type(id, + std::vector() )); + vit->second.push_back(i_feature); + } +} + +// --------------------------------------------------------------------------- + +std::ostream& operator<<(std::ostream &out, + const FeatureVector &v) +{ + if(!v.empty()) + { + FeatureVector::const_iterator vit = v.begin(); + + const std::vector* f = &vit->second; + + out << "<" << vit->first << ": ["; + if(!f->empty()) out << (*f)[0]; + for(unsigned int i = 1; i < f->size(); ++i) + { + out << ", " << (*f)[i]; + } + out << "]>"; + + for(++vit; vit != v.end(); ++vit) + { + f = &vit->second; + + out << ", <" << vit->first << ": ["; + if(!f->empty()) out << (*f)[0]; + for(unsigned int i = 1; i < f->size(); ++i) + { + out << ", " << (*f)[i]; + } + out << "]>"; + } + } + + return out; +} + +// --------------------------------------------------------------------------- + +} // namespace DBoW3 diff --git a/thirdparty/DBoW3/src/FeatureVector.h b/thirdparty/DBoW3/src/FeatureVector.h new file mode 100644 index 0000000..f06bc1e --- /dev/null +++ b/thirdparty/DBoW3/src/FeatureVector.h @@ -0,0 +1,55 @@ +/** + * File: FeatureVector.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: feature vector + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_FEATURE_VECTOR__ +#define __D_T_FEATURE_VECTOR__ + +#include "BowVector.h" +#include +#include +#include "exports.h" +namespace DBoW3 { + +/// Vector of nodes with indexes of local features +class DBOW_API FeatureVector: + public std::map > +{ +public: + + /** + * Constructor + */ + FeatureVector(void); + + /** + * Destructor + */ + ~FeatureVector(void); + + /** + * Adds a feature to an existing node, or adds a new node with an initial + * feature + * @param id node id to add or to modify + * @param i_feature index of feature to add to the given node + */ + void addFeature(NodeId id, unsigned int i_feature); + + /** + * Sends a string versions of the feature vector through the stream + * @param out stream + * @param v feature vector + */ + friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); + +}; + +} // namespace DBoW3 + +#endif + diff --git a/thirdparty/DBoW3/src/QueryResults.cpp b/thirdparty/DBoW3/src/QueryResults.cpp new file mode 100644 index 0000000..7062400 --- /dev/null +++ b/thirdparty/DBoW3/src/QueryResults.cpp @@ -0,0 +1,63 @@ +/** + * File: QueryResults.cpp + * Date: March, November 2011 + * Author: Dorian Galvez-Lopez + * Description: structure to store results of database queries + * License: see the LICENSE.txt file + * + */ + +#include +#include +#include "QueryResults.h" + +using namespace std; + +namespace DBoW3 +{ + +// --------------------------------------------------------------------------- + +ostream & operator<<(ostream& os, const Result& ret ) +{ + os << ""; + return os; +} + +// --------------------------------------------------------------------------- + +ostream & operator<<(ostream& os, const QueryResults& ret ) +{ + if(ret.size() == 1) + os << "1 result:" << endl; + else + os << ret.size() << " results:" << endl; + + QueryResults::const_iterator rit; + for(rit = ret.begin(); rit != ret.end(); ++rit) + { + os << *rit; + if(rit + 1 != ret.end()) os << endl; + } + return os; +} + +// --------------------------------------------------------------------------- + +void QueryResults::saveM(const std::string &filename) const +{ + fstream f(filename.c_str(), ios::out); + + QueryResults::const_iterator qit; + for(qit = begin(); qit != end(); ++qit) + { + f << qit->Id << " " << qit->Score << endl; + } + + f.close(); +} + +// --------------------------------------------------------------------------- + +} // namespace DBoW3 + diff --git a/thirdparty/DBoW3/src/QueryResults.h b/thirdparty/DBoW3/src/QueryResults.h new file mode 100644 index 0000000..bebbd1e --- /dev/null +++ b/thirdparty/DBoW3/src/QueryResults.h @@ -0,0 +1,205 @@ +/** + * File: QueryResults.h + * Date: March, November 2011 + * Author: Dorian Galvez-Lopez + * Description: structure to store results of database queries + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_QUERY_RESULTS__ +#define __D_T_QUERY_RESULTS__ + +#include +#include "exports.h" +namespace DBoW3 { + +/// Id of entries of the database +typedef unsigned int EntryId; + +/// Single result of a query +class DBOW_API Result +{ +public: + + /// Entry id + EntryId Id; + + /// Score obtained + double Score; + + /// debug + int nWords; // words in common + // !!! this is filled only by Bhatt score! + // (and for BCMatching, BCThresholding then) + + double bhatScore, chiScore; + /// debug + + // only done by ChiSq and BCThresholding + double sumCommonVi; + double sumCommonWi; + double expectedChiScore; + /// debug + + /** + * Empty constructors + */ + inline Result(){} + + /** + * Creates a result with the given data + * @param _id entry id + * @param _score score + */ + inline Result(EntryId _id, double _score): Id(_id), Score(_score){} + + /** + * Compares the scores of two results + * @return true iff this.score < r.score + */ + inline bool operator<(const Result &r) const + { + return this->Score < r.Score; + } + + /** + * Compares the scores of two results + * @return true iff this.score > r.score + */ + inline bool operator>(const Result &r) const + { + return this->Score > r.Score; + } + + /** + * Compares the entry id of the result + * @return true iff this.id == id + */ + inline bool operator==(EntryId id) const + { + return this->Id == id; + } + + /** + * Compares the score of this entry with a given one + * @param s score to compare with + * @return true iff this score < s + */ + inline bool operator<(double s) const + { + return this->Score < s; + } + + /** + * Compares the score of this entry with a given one + * @param s score to compare with + * @return true iff this score > s + */ + inline bool operator>(double s) const + { + return this->Score > s; + } + + /** + * Compares the score of two results + * @param a + * @param b + * @return true iff a.Score > b.Score + */ + static inline bool gt(const Result &a, const Result &b) + { + return a.Score > b.Score; + } + + /** + * Compares the scores of two results + * @return true iff a.Score > b.Score + */ + inline static bool ge(const Result &a, const Result &b) + { + return a.Score > b.Score; + } + + /** + * Returns true iff a.Score >= b.Score + * @param a + * @param b + * @return true iff a.Score >= b.Score + */ + static inline bool geq(const Result &a, const Result &b) + { + return a.Score >= b.Score; + } + + /** + * Returns true iff a.Score >= s + * @param a + * @param s + * @return true iff a.Score >= s + */ + static inline bool geqv(const Result &a, double s) + { + return a.Score >= s; + } + + + /** + * Returns true iff a.Id < b.Id + * @param a + * @param b + * @return true iff a.Id < b.Id + */ + static inline bool ltId(const Result &a, const Result &b) + { + return a.Id < b.Id; + } + + /** + * Prints a string version of the result + * @param os ostream + * @param ret Result to print + */ + friend std::ostream & operator<<(std::ostream& os, const Result& ret ); +}; + +/// Multiple results from a query +class QueryResults: public std::vector +{ +public: + + /** + * Multiplies all the scores in the vector by factor + * @param factor + */ + inline void scaleScores(double factor); + + /** + * Prints a string version of the results + * @param os ostream + * @param ret QueryResults to print + */ + DBOW_API friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret ); + + /** + * Saves a matlab file with the results + * @param filename + */ + void saveM(const std::string &filename) const; + +}; + +// -------------------------------------------------------------------------- + +inline void QueryResults::scaleScores(double factor) +{ + for(QueryResults::iterator qit = begin(); qit != end(); ++qit) + qit->Score *= factor; +} + +// -------------------------------------------------------------------------- + +} // namespace TemplatedBoW + +#endif + diff --git a/thirdparty/DBoW3/src/ScoringObject.cpp b/thirdparty/DBoW3/src/ScoringObject.cpp new file mode 100644 index 0000000..7cf0812 --- /dev/null +++ b/thirdparty/DBoW3/src/ScoringObject.cpp @@ -0,0 +1,315 @@ +/** + * File: ScoringObject.cpp + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: functions to compute bow scores + * License: see the LICENSE.txt file + * + */ + +#include +#include "Vocabulary.h" +#include "BowVector.h" + +using namespace DBoW3; + +// If you change the type of WordValue, make sure you change also the +// epsilon value (this is needed by the KL method) +const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double L1Scoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += fabs(vi - wi) - fabs(vi) - fabs(wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) + // for all i | v_i != 0 and w_i != 0 + // (Nister, 2006) + // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} + score = -score/2.0; + + return score; // [0..1] +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double L2Scoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += vi * wi; + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) + // for all i | v_i != 0 and w_i != 0 ) + // (Nister, 2006) + if(score >= 1) // rounding errors + score = 1.0; + else + score = 1.0 - sqrt(1.0 - score); // [0..1] + + return score; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) + const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + // all the items are taken into account + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) + // we move the -4 out + if(vi + wi != 0.0) score += vi * wi / (vi + wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + } + } + + // this takes the -4 into account + score = 2. * score; // [0..1] + + return score; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double KLScoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + // all the items or v are taken into account + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + if(vi != 0 && wi != 0) score += vi * log(vi/wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + score += vi * (log(vi) - LOG_EPS); + ++v1_it; + } + else + { + // move v2_it forward, do not add any score + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // sum rest of items of v + for(; v1_it != v1_end; ++v1_it) + if(v1_it->second != 0) + score += v1_it->second * (log(v1_it->second) - LOG_EPS); + + return score; // cannot be scaled +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double BhattacharyyaScoring::score(const BowVector &v1, + const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += sqrt(vi * wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + return score; // already scaled +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double DotProductScoring::score(const BowVector &v1, + const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += vi * wi; + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + return score; // cannot scale +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + diff --git a/thirdparty/DBoW3/src/ScoringObject.h b/thirdparty/DBoW3/src/ScoringObject.h new file mode 100644 index 0000000..8d6c64e --- /dev/null +++ b/thirdparty/DBoW3/src/ScoringObject.h @@ -0,0 +1,95 @@ +/** + * File: ScoringObject.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: functions to compute bow scores + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_SCORING_OBJECT__ +#define __D_T_SCORING_OBJECT__ + +#include "BowVector.h" +#include "exports.h" +namespace DBoW3 { + +/// Base class of scoring functions +class DBOW_API GeneralScoring +{ +public: + /** + * Computes the score between two vectors. Vectors must be sorted and + * normalized if necessary + * @param v (in/out) + * @param w (in/out) + * @return score + */ + virtual double score(const BowVector &v, const BowVector &w) const = 0; + + /** + * Returns whether a vector must be normalized before scoring according + * to the scoring scheme + * @param norm norm to use + * @return true iff must normalize + */ + virtual bool mustNormalize(LNorm &norm) const = 0; + + /// Log of epsilon + static const double LOG_EPS; + // If you change the type of WordValue, make sure you change also the + // epsilon value (this is needed by the KL method) + + virtual ~GeneralScoring() {} //!< Required for virtual base classes +}; + +/** + * Macro for defining Scoring classes + * @param NAME name of class + * @param MUSTNORMALIZE if vectors must be normalized to compute the score + * @param NORM type of norm to use when MUSTNORMALIZE + */ +#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ + NAME: public GeneralScoring \ + { public: \ + /** \ + * Computes score between two vectors \ + * @param v \ + * @param w \ + * @return score between v and w \ + */ \ + virtual double score(const BowVector &v, const BowVector &w) const; \ + \ + /** \ + * Says if a vector must be normalized according to the scoring function \ + * @param norm (out) if true, norm to use + * @return true iff vectors must be normalized \ + */ \ + virtual inline bool mustNormalize(LNorm &norm) const \ + { norm = NORM; return MUSTNORMALIZE; } \ + } + +/// L1 Scoring object +class __SCORING_CLASS(L1Scoring, true, L1); + +/// L2 Scoring object +class __SCORING_CLASS(L2Scoring, true, L2); + +/// Chi square Scoring object +class __SCORING_CLASS(ChiSquareScoring, true, L1); + +/// KL divergence Scoring object +class __SCORING_CLASS(KLScoring, true, L1); + +/// Bhattacharyya Scoring object +class __SCORING_CLASS(BhattacharyyaScoring, true, L1); + +/// Dot product Scoring object +class __SCORING_CLASS(DotProductScoring, false, L1); + +#undef __SCORING_CLASS + +} // namespace DBoW3 + +#endif + diff --git a/thirdparty/DBoW3/src/Vocabulary.cpp b/thirdparty/DBoW3/src/Vocabulary.cpp new file mode 100644 index 0000000..47d64f5 --- /dev/null +++ b/thirdparty/DBoW3/src/Vocabulary.cpp @@ -0,0 +1,1558 @@ +#include "Vocabulary.h" +#include +#include +#include "DescManip.h" +#include "quicklz.h" +#include "timers.h" + +namespace DBoW3 { +// -------------------------------------------------------------------------- + +Vocabulary::Vocabulary(int k, int L, WeightingType weighting, + ScoringType scoring) + : m_k(k), + m_L(L), + m_weighting(weighting), + m_scoring(scoring), + m_scoring_object(NULL) { + createScoringObject(); +} + +// -------------------------------------------------------------------------- + +Vocabulary::Vocabulary(const std::string &filename) : m_scoring_object(NULL) { + load(filename); +} + +// -------------------------------------------------------------------------- + +Vocabulary::Vocabulary(const char *filename) : m_scoring_object(NULL) { + load(filename); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::createScoringObject() { + delete m_scoring_object; + m_scoring_object = NULL; + + switch (m_scoring) { + case L1_NORM: + m_scoring_object = new L1Scoring; + break; + + case L2_NORM: + m_scoring_object = new L2Scoring; + break; + + case CHI_SQUARE: + m_scoring_object = new ChiSquareScoring; + break; + + case KL: + m_scoring_object = new KLScoring; + break; + + case BHATTACHARYYA: + m_scoring_object = new BhattacharyyaScoring; + break; + + case DOT_PRODUCT: + m_scoring_object = new DotProductScoring; + break; + } +} + +// -------------------------------------------------------------------------- + +void Vocabulary::setScoringType(ScoringType type) { + m_scoring = type; + createScoringObject(); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::setWeightingType(WeightingType type) { + this->m_weighting = type; +} + +// -------------------------------------------------------------------------- + +Vocabulary::Vocabulary(const Vocabulary &voc) : m_scoring_object(NULL) { + *this = voc; +} + +// -------------------------------------------------------------------------- + +Vocabulary::~Vocabulary() { delete m_scoring_object; } + +// -------------------------------------------------------------------------- + +Vocabulary &Vocabulary::operator=(const Vocabulary &voc) { + this->m_k = voc.m_k; + this->m_L = voc.m_L; + this->m_scoring = voc.m_scoring; + this->m_weighting = voc.m_weighting; + + this->createScoringObject(); + + this->m_nodes.clear(); + this->m_words.clear(); + + this->m_nodes = voc.m_nodes; + this->createWords(); + + return *this; +} + +void Vocabulary::create(const std::vector &training_features) { + std::vector> vtf(training_features.size()); + for (size_t i = 0; i < training_features.size(); i++) { + vtf[i].resize(training_features[i].rows); + for (int r = 0; r < training_features[i].rows; r++) + vtf[i][r] = training_features[i].rowRange(r, r + 1); + } + create(vtf); +} + +void Vocabulary::create( + const std::vector> &training_features) { + m_nodes.clear(); + m_words.clear(); + + // expected_nodes = Sum_{i=0..L} ( k^i ) + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1) / (m_k - 1)); + + m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree + + std::vector features; + getFeatures(training_features, features); + + // create root + m_nodes.push_back(Node(0)); // root + + // create the tree + HKmeansStep(0, features, 1); + + // create the words + createWords(); + + // and set the weight of each node of the tree + setNodeWeights(training_features); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::create( + const std::vector> &training_features, int k, int L) { + m_k = k; + m_L = L; + + create(training_features); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::create( + const std::vector> &training_features, int k, int L, + WeightingType weighting, ScoringType scoring) { + m_k = k; + m_L = L; + m_weighting = weighting; + m_scoring = scoring; + createScoringObject(); + + create(training_features); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::getFeatures( + const std::vector> &training_features, + std::vector &features) const { + features.resize(0); + for (size_t i = 0; i < training_features.size(); i++) + for (size_t j = 0; j < training_features[i].size(); j++) + features.push_back(training_features[i][j]); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::HKmeansStep(NodeId parent_id, + const std::vector &descriptors, + int current_level) { + if (descriptors.empty()) return; + + // features associated to each cluster + std::vector clusters; + std::vector> groups; // groups[i] = [j1, j2, ...] + // j1, j2, ... indices of descriptors associated to cluster i + + clusters.reserve(m_k); + groups.reserve(m_k); + + if ((int)descriptors.size() <= m_k) { + // trivial case: one cluster per feature + groups.resize(descriptors.size()); + + for (unsigned int i = 0; i < descriptors.size(); i++) { + groups[i].push_back(i); + clusters.push_back(descriptors[i]); + } + } else { + // select clusters and groups with kmeans + + bool first_time = true; + bool goon = true; + + // to check if clusters move after iterations + std::vector last_association, current_association; + + while (goon) { + // 1. Calculate clusters + + if (first_time) { + // random sample + initiateClusters(descriptors, clusters); + } else { + // calculate cluster centres + + for (unsigned int c = 0; c < clusters.size(); ++c) { + std::vector cluster_descriptors; + cluster_descriptors.reserve(groups[c].size()); + std::vector::const_iterator vit; + for (vit = groups[c].begin(); vit != groups[c].end(); ++vit) { + cluster_descriptors.push_back(descriptors[*vit]); + } + + DescManip::meanValue(cluster_descriptors, clusters[c]); + } + + } // if(!first_time) + + // 2. Associate features with clusters + + // calculate distances to cluster centers + groups.clear(); + groups.resize(clusters.size(), std::vector()); + current_association.resize(descriptors.size()); + + // assoc.clear(); + + // unsigned int d = 0; + for (auto fit = descriptors.begin(); fit != descriptors.end(); + ++fit) //, ++d) + { + double best_dist = DescManip::distance((*fit), clusters[0]); + unsigned int icluster = 0; + + for (unsigned int c = 1; c < clusters.size(); ++c) { + double dist = DescManip::distance((*fit), clusters[c]); + if (dist < best_dist) { + best_dist = dist; + icluster = c; + } + } + + // assoc.ref(icluster, d) = 1; + + groups[icluster].push_back(fit - descriptors.begin()); + current_association[fit - descriptors.begin()] = icluster; + } + + // kmeans++ ensures all the clusters has any feature associated with them + + // 3. check convergence + if (first_time) { + first_time = false; + } else { + // goon = !eqUChar(last_assoc, assoc); + + goon = false; + for (unsigned int i = 0; i < current_association.size(); i++) { + if (current_association[i] != last_association[i]) { + goon = true; + break; + } + } + } + + if (goon) { + // copy last feature-cluster association + last_association = current_association; + // last_assoc = assoc.clone(); + } + + } // while(goon) + + } // if must run kmeans + + // create nodes + for (unsigned int i = 0; i < clusters.size(); ++i) { + NodeId id = m_nodes.size(); + m_nodes.push_back(Node(id)); + m_nodes.back().descriptor = clusters[i]; + m_nodes.back().parent = parent_id; + m_nodes[parent_id].children.push_back(id); + } + + // go on with the next level + if (current_level < m_L) { + // iterate again with the resulting clusters + const std::vector &children_ids = m_nodes[parent_id].children; + for (unsigned int i = 0; i < clusters.size(); ++i) { + NodeId id = children_ids[i]; + + std::vector child_features; + child_features.reserve(groups[i].size()); + + std::vector::const_iterator vit; + for (vit = groups[i].begin(); vit != groups[i].end(); ++vit) { + child_features.push_back(descriptors[*vit]); + } + + if (child_features.size() > 1) { + HKmeansStep(id, child_features, current_level + 1); + } + } + } +} + +// -------------------------------------------------------------------------- + +void Vocabulary::initiateClusters(const std::vector &descriptors, + std::vector &clusters) const { + initiateClustersKMpp(descriptors, clusters); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::initiateClustersKMpp(const std::vector &pfeatures, + std::vector &clusters) const { + // Implements kmeans++ seeding algorithm + // Algorithm: + // 1. Choose one center uniformly at random from among the data points. + // 2. For each data point x, compute D(x), the distance between x and the + // nearest + // center that has already been chosen. + // 3. Add one new data point as a center. Each point x is chosen with + // probability + // proportional to D(x)^2. + // 4. Repeat Steps 2 and 3 until k centers have been chosen. + // 5. Now that the initial centers have been chosen, proceed using standard + // k-means + // clustering. + + // DUtils::Random::SeedRandOnce(); + + clusters.resize(0); + clusters.reserve(m_k); + std::vector min_dists(pfeatures.size(), + std::numeric_limits::max()); + + // 1. + + int ifeature = + rand() % + pfeatures.size(); // DUtils::Random::RandomInt(0, pfeatures.size()-1); + + // create first cluster + clusters.push_back(pfeatures[ifeature]); + + // compute the initial distances + std::vector::iterator dit; + dit = min_dists.begin(); + for (auto fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) { + *dit = DescManip::distance((*fit), clusters.back()); + } + + while ((int)clusters.size() < m_k) { + // 2. + dit = min_dists.begin(); + for (auto fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) { + if (*dit > 0) { + double dist = DescManip::distance((*fit), clusters.back()); + if (dist < *dit) *dit = dist; + } + } + + // 3. + double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0); + + if (dist_sum > 0) { + double cut_d; + do { + cut_d = (double(rand()) / double(RAND_MAX)) * dist_sum; + } while (cut_d == 0.0); + + double d_up_now = 0; + for (dit = min_dists.begin(); dit != min_dists.end(); ++dit) { + d_up_now += *dit; + if (d_up_now >= cut_d) break; + } + + if (dit == min_dists.end()) + ifeature = pfeatures.size() - 1; + else + ifeature = dit - min_dists.begin(); + + clusters.push_back(pfeatures[ifeature]); + } // if dist_sum > 0 + else + break; + + } // while(used_clusters < m_k) +} + +// -------------------------------------------------------------------------- + +void Vocabulary::createWords() { + m_words.resize(0); + + if (!m_nodes.empty()) { + m_words.reserve((int)pow((double)m_k, (double)m_L)); + + auto nit = m_nodes.begin(); // ignore root + for (++nit; nit != m_nodes.end(); ++nit) { + if (nit->isLeaf()) { + nit->word_id = m_words.size(); + m_words.push_back(&(*nit)); + } + } + } +} + +// -------------------------------------------------------------------------- + +void Vocabulary::setNodeWeights( + const std::vector> &training_features) { + const unsigned int NWords = m_words.size(); + const unsigned int NDocs = training_features.size(); + + if (m_weighting == TF || m_weighting == BINARY) { + // idf part must be 1 always + for (unsigned int i = 0; i < NWords; i++) m_words[i]->weight = 1; + } else if (m_weighting == IDF || m_weighting == TF_IDF) { + // IDF and TF-IDF: we calculte the idf path now + + // Note: this actually calculates the idf part of the tf-idf score. + // The complete tf-idf score is calculated in ::transform + + std::vector Ni(NWords, 0); + std::vector counted(NWords, false); + + for (auto mit = training_features.begin(); mit != training_features.end(); + ++mit) { + fill(counted.begin(), counted.end(), false); + + for (auto fit = mit->begin(); fit < mit->end(); ++fit) { + WordId word_id; + transform(*fit, word_id); + + if (!counted[word_id]) { + Ni[word_id]++; + counted[word_id] = true; + } + } + } + + // set ln(N/Ni) + for (unsigned int i = 0; i < NWords; i++) { + if (Ni[i] > 0) { + m_words[i]->weight = log((double)NDocs / (double)Ni[i]); + } // else // This cannot occur if using kmeans++ + } + } +} + +// -------------------------------------------------------------------------- + +// -------------------------------------------------------------------------- + +float Vocabulary::getEffectiveLevels() const { + long sum = 0; + for (auto wit = m_words.begin(); wit != m_words.end(); ++wit) { + const Node *p = *wit; + + for (; p->id != 0; sum++) p = &m_nodes[p->parent]; + } + + return (float)((double)sum / (double)m_words.size()); +} + +// -------------------------------------------------------------------------- + +cv::Mat Vocabulary::getWord(WordId wid) const { + return m_words[wid]->descriptor; +} + +// -------------------------------------------------------------------------- + +WordValue Vocabulary::getWordWeight(WordId wid) const { + return m_words[wid]->weight; +} + +// -------------------------------------------------------------------------- + +WordId Vocabulary::transform(const cv::Mat &feature) const { + if (empty()) { + return 0; + } + + WordId wid; + transform(feature, wid); + return wid; +} + +// -------------------------------------------------------------------------- + +void Vocabulary::transform(const cv::Mat &features, BowVector &v) const { + // std::vector vf(features.rows); + // for(int r=0;rmustNormalize(norm); + + if (m_weighting == TF || m_weighting == TF_IDF) { + for (int r = 0; r < features.rows; r++) { + WordId id; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + transform(features.row(r), id, w); + // not stopped + if (w > 0) v.addWeight(id, w); + } + + if (!v.empty() && !must) { + // unnecessary when normalizing + const double nd = v.size(); + for (BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } else // IDF || BINARY + { + for (int r = 0; r < features.rows; r++) { + WordId id; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(features.row(r), id, w); + + // not stopped + if (w > 0) v.addIfNotExist(id, w); + + } // if add_features + } // if m_weighting == ... + + if (must) v.normalize(norm); +} + +void Vocabulary::transform(const std::vector &features, + BowVector &v) const { + v.clear(); + + if (empty()) { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + if (m_weighting == TF || m_weighting == TF_IDF) { + for (auto fit = features.begin(); fit < features.end(); ++fit) { + WordId id; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w); + + // not stopped + if (w > 0) v.addWeight(id, w); + } + + if (!v.empty() && !must) { + // unnecessary when normalizing + const double nd = v.size(); + for (BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } else // IDF || BINARY + { + for (auto fit = features.begin(); fit < features.end(); ++fit) { + WordId id; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w); + + // not stopped + if (w > 0) v.addIfNotExist(id, w); + + } // if add_features + } // if m_weighting == ... + + if (must) v.normalize(norm); +} + +void Vocabulary::transform(const std::vector> &features, + BowVector &v) const { + v.clear(); + + if (empty()) { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + if (m_weighting == TF || m_weighting == TF_IDF) { + for (auto fit = features.begin(); fit < features.end(); ++fit) { + WordId id; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w); + + // not stopped + if (w > 0) v.addWeight(id, w); + } + + if (!v.empty() && !must) { + // unnecessary when normalizing + const double nd = v.size(); + for (BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } else // IDF || BINARY + { + for (auto fit = features.begin(); fit < features.end(); ++fit) { + WordId id; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w); + + // not stopped + if (w > 0) v.addIfNotExist(id, w); + + } // if add_features + } // if m_weighting == ... + + if (must) v.normalize(norm); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::transform(const std::vector> &features, + BowVector &v, FeatureVector &fv, + int levelsup) const { + v.clear(); + fv.clear(); + + if (empty()) // safe for subclasses + { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + if (m_weighting == TF || m_weighting == TF_IDF) { + unsigned int i_feature = 0; + for (auto fit = features.begin(); fit < features.end(); + ++fit, ++i_feature) { + WordId id; + NodeId nid; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w, &nid, levelsup); + + if (w > 0) // not stopped + { + v.addWeight(id, w); + fv.addFeature(nid, i_feature); + } + } + + if (!v.empty() && !must) { + // unnecessary when normalizing + const double nd = v.size(); + for (BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } else // IDF || BINARY + { + unsigned int i_feature = 0; + for (auto fit = features.begin(); fit < features.end(); + ++fit, ++i_feature) { + WordId id; + NodeId nid; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w, &nid, levelsup); + + if (w > 0) // not stopped + { + v.addIfNotExist(id, w); + fv.addFeature(nid, i_feature); + } + } + } // if m_weighting == ... + + if (must) v.normalize(norm); +} + +void Vocabulary::transform(const std::vector &features, BowVector &v, + FeatureVector &fv, int levelsup) const { + v.clear(); + fv.clear(); + + if (empty()) // safe for subclasses + { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + if (m_weighting == TF || m_weighting == TF_IDF) { + unsigned int i_feature = 0; + for (auto fit = features.begin(); fit < features.end(); + ++fit, ++i_feature) { + WordId id; + NodeId nid; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w, &nid, levelsup); + + if (w > 0) // not stopped + { + v.addWeight(id, w); + fv.addFeature(nid, i_feature); + } + } + + if (!v.empty() && !must) { + // unnecessary when normalizing + const double nd = v.size(); + for (BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } else // IDF || BINARY + { + unsigned int i_feature = 0; + for (auto fit = features.begin(); fit < features.end(); + ++fit, ++i_feature) { + WordId id; + NodeId nid; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w, &nid, levelsup); + + if (w > 0) // not stopped + { + v.addIfNotExist(id, w); + fv.addFeature(nid, i_feature); + } + } + } // if m_weighting == ... + + if (must) v.normalize(norm); +} + +// -------------------------------------------------------------------------- + +// -------------------------------------------------------------------------- + +void Vocabulary::transform(const cv::Mat &feature, WordId &id) const { + WordValue weight; + transform(feature, id, weight); +} + +// -------------------------------------------------------------------------- + +void Vocabulary::transform(const std::bitset<256> &feature, WordId &word_id, + WordValue &weight, NodeId *nid, int levelsup) const { + // propagate the feature down the tree + + // level at which the node must be stored in nid, if given + const int nid_level = m_L - levelsup; + if (nid_level <= 0 && nid != NULL) *nid = 0; // root + + NodeId final_id = 0; // root + int current_level = 0; + + do { + ++current_level; + auto const &nodes = m_nodes[final_id].children; + double best_d = std::numeric_limits::max(); + // DescManip::distance(feature, m_nodes[final_id].descriptor); + + for (const auto &id : nodes) { + if (m_nodes[id].descriptor.type() != CV_8U || + m_nodes[id].descriptor.cols != sizeof(std::bitset<256>)) { + std::cerr << "Wrong descriptor type" << std::endl; + std::abort(); + } + + std::bitset<256> *desc = (std::bitset<256> *)m_nodes[id].descriptor.data; + + double d = ((*desc) & feature).count(); + + if (d < best_d) { + best_d = d; + final_id = id; + } + } + + if (nid != NULL && current_level == nid_level) *nid = final_id; + + } while (!m_nodes[final_id].isLeaf()); + + // turn node id into word id + word_id = m_nodes[final_id].word_id; + weight = m_nodes[final_id].weight; +} + +void Vocabulary::transform(const cv::Mat &feature, WordId &word_id, + WordValue &weight, NodeId *nid, int levelsup) const { + // propagate the feature down the tree + + // level at which the node must be stored in nid, if given + const int nid_level = m_L - levelsup; + if (nid_level <= 0 && nid != NULL) *nid = 0; // root + + NodeId final_id = 0; // root + int current_level = 0; + + do { + ++current_level; + auto const &nodes = m_nodes[final_id].children; + double best_d = std::numeric_limits::max(); + // DescManip::distance(feature, m_nodes[final_id].descriptor); + + for (const auto &id : nodes) { + double d = DescManip::distance(feature, m_nodes[id].descriptor); + if (d < best_d) { + best_d = d; + final_id = id; + } + } + + if (nid != NULL && current_level == nid_level) *nid = final_id; + + } while (!m_nodes[final_id].isLeaf()); + + // turn node id into word id + word_id = m_nodes[final_id].word_id; + weight = m_nodes[final_id].weight; +} + +void Vocabulary::transform(const cv::Mat &feature, WordId &word_id, + WordValue &weight) const { + // propagate the feature down the tree + + // level at which the node must be stored in nid, if given + + NodeId final_id = 0; // root + // maximum speed by computing here distance and avoid calling to + // DescManip::distance + + // binary descriptor + // int ntimes=0; + if (feature.type() == CV_8U) { + do { + auto const &nodes = m_nodes[final_id].children; + uint64_t best_d = std::numeric_limits::max(); + int idx = 0; //, bestidx = 0; + for (const auto &id : nodes) { + // compute distance + // std::cout<(); + // for(int i=0;i(0); + // const float *b_ptr=b.ptr(0); + // for(int i = 0; i < a.cols; i ++) + // sqd += (a_ptr[i ] - b_ptr[i ])*(a_ptr[i ] - b_ptr[i ]); + // return sqd; + // } + + // do + // { + // auto const &nodes = m_nodes[final_id].children; + // double best_d = std::numeric_limits::max(); + + // for(const auto &id:nodes) + // { + // double d = DescManip::distance(feature, m_nodes[id].descriptor); + // if(d < best_d) + // { + // best_d = d; + // final_id = id; + // } + // } + // } while( !m_nodes[final_id].isLeaf() ); + + // turn node id into word id + word_id = m_nodes[final_id].word_id; + weight = m_nodes[final_id].weight; +} + +void Vocabulary::transform(const std::bitset<256> &feature, WordId &word_id, + WordValue &weight) const { + // propagate the feature down the tree + + // level at which the node must be stored in nid, if given + + NodeId final_id = 0; // root + // maximum speed by computing here distance and avoid calling to + // DescManip::distance + + // binary descriptor + // int ntimes=0; + if (1) { + do { + auto const &nodes = m_nodes[final_id].children; + uint64_t best_d = std::numeric_limits::max(); + int idx = 0; //, bestidx = 0; + for (const auto &id : nodes) { + // compute distance + // std::cout<)) { + std::cerr << "Wrong descriptor type" << std::endl; + std::abort(); + } + + std::bitset<256> *desc = + (std::bitset<256> *)m_nodes[id].descriptor.data; + + uint64_t dist = ((*desc) & feature).count(); + + if (dist < best_d) { + best_d = dist; + final_id = id; + // bestidx = idx; + } + idx++; + } + // std::cout<(); + // for(int i=0;i(0); + // const float *b_ptr=b.ptr(0); + // for(int i = 0; i < a.cols; i ++) + // sqd += (a_ptr[i ] - b_ptr[i ])*(a_ptr[i ] - b_ptr[i ]); + // return sqd; + // } + + // do + // { + // auto const &nodes = m_nodes[final_id].children; + // double best_d = std::numeric_limits::max(); + + // for(const auto &id:nodes) + // { + // double d = DescManip::distance(feature, m_nodes[id].descriptor); + // if(d < best_d) + // { + // best_d = d; + // final_id = id; + // } + // } + // } while( !m_nodes[final_id].isLeaf() ); + + // turn node id into word id + word_id = m_nodes[final_id].word_id; + weight = m_nodes[final_id].weight; +} + +// -------------------------------------------------------------------------- + +NodeId Vocabulary::getParentNode(WordId wid, int levelsup) const { + NodeId ret = m_words[wid]->id; // node id + while (levelsup > 0 && ret != 0) // ret == 0 --> root + { + --levelsup; + ret = m_nodes[ret].parent; + } + return ret; +} + +// -------------------------------------------------------------------------- + +void Vocabulary::getWordsFromNode(NodeId nid, + std::vector &words) const { + words.clear(); + + if (m_nodes[nid].isLeaf()) { + words.push_back(m_nodes[nid].word_id); + } else { + words.reserve(m_k); // ^1, ^2, ... + + std::vector parents; + parents.push_back(nid); + + while (!parents.empty()) { + NodeId parentid = parents.back(); + parents.pop_back(); + + const std::vector &child_ids = m_nodes[parentid].children; + std::vector::const_iterator cit; + + for (cit = child_ids.begin(); cit != child_ids.end(); ++cit) { + const Node &child_node = m_nodes[*cit]; + + if (child_node.isLeaf()) + words.push_back(child_node.word_id); + else + parents.push_back(*cit); + + } // for each child + } // while !parents.empty + } +} + +// -------------------------------------------------------------------------- + +int Vocabulary::stopWords(double minWeight) { + int c = 0; + for (auto wit = m_words.begin(); wit != m_words.end(); ++wit) { + if ((*wit)->weight < minWeight) { + ++c; + (*wit)->weight = 0; + } + } + return c; +} + +// -------------------------------------------------------------------------- + +void Vocabulary::save(const std::string &filename, + bool binary_compressed) const { + if (filename.find(".yml") == std::string::npos) { + std::ofstream file_out(filename, std::ios::binary); + if (!file_out) + throw std::runtime_error("Vocabulary::saveBinary Could not open file :" + + filename + " for writing"); + toStream(file_out, binary_compressed); + } else { + cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); + if (!fs.isOpened()) throw std::string("Could not open file ") + filename; + save(fs); + } +} + +// -------------------------------------------------------------------------- + +void Vocabulary::load(const std::string &filename) { + // check first if it is a binary file + std::ifstream ifile(filename, std::ios::binary); + if (!ifile) + throw std::runtime_error("Vocabulary::load Could not open file :" + + filename + " for reading"); + uint64_t sig; // magic number describing the file + ifile.read((char *)&sig, sizeof(sig)); + if (sig == 88877711233) { // it is a binary file. read from it + ifile.seekg(0, std::ios::beg); + fromStream(ifile); + + } else { + if (filename.find(".txt") != std::string::npos) { + // read from a text file (used in ORBSLAM2) + load_fromtxt(filename); + } else { + cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); + if (!fs.isOpened()) throw std::string("Could not open file ") + filename; + this->load(fs); + } + } +} + +void Vocabulary::save(cv::FileStorage &f, const std::string &name) const { + f << name << "{"; + + f << "k" << m_k; + f << "L" << m_L; + f << "scoringType" << m_scoring; + f << "weightingType" << m_weighting; + + // tree + f << "nodes" + << "["; + std::vector parents, children; + std::vector::const_iterator pit; + + parents.push_back(0); // root + + while (!parents.empty()) { + NodeId pid = parents.back(); + parents.pop_back(); + + const Node &parent = m_nodes[pid]; + children = parent.children; + + for (pit = children.begin(); pit != children.end(); pit++) { + const Node &child = m_nodes[*pit]; + std::cout << m_nodes[*pit].id << " "; + + // save node data + f << "{:"; + f << "nodeId" << (int)child.id; + f << "parentId" << (int)pid; + f << "weight" << (double)child.weight; + f << "descriptor" << DescManip::toString(child.descriptor); + f << "}"; + + // add to parent list + if (!child.isLeaf()) { + parents.push_back(*pit); + } + } + } + std::cout << "\n"; + + f << "]"; // nodes + + // words + f << "words" + << "["; + + for (auto wit = m_words.begin(); wit != m_words.end(); wit++) { + WordId id = wit - m_words.begin(); + f << "{:"; + f << "wordId" << (int)id; + f << "nodeId" << (int)(*wit)->id; + f << "}"; + } + + f << "]"; // words + + f << "}"; +} + +void Vocabulary::toStream(std::ostream &out_str, bool compressed) const { + uint64_t sig = 88877711233; // magic number describing the file + out_str.write((char *)&sig, sizeof(sig)); + out_str.write((char *)&compressed, sizeof(compressed)); + uint32_t nnodes = m_nodes.size(); + out_str.write((char *)&nnodes, sizeof(nnodes)); + if (nnodes == 0) return; + // save everything to a stream + std::stringstream aux_stream; + aux_stream.write((char *)&m_k, sizeof(m_k)); + aux_stream.write((char *)&m_L, sizeof(m_L)); + aux_stream.write((char *)&m_scoring, sizeof(m_scoring)); + aux_stream.write((char *)&m_weighting, sizeof(m_weighting)); + // nodes + std::vector parents = {0}; // root + + while (!parents.empty()) { + NodeId pid = parents.back(); + parents.pop_back(); + + const Node &parent = m_nodes[pid]; + + for (auto pit : parent.children) { + const Node &child = m_nodes[pit]; + aux_stream.write((char *)&child.id, sizeof(child.id)); + aux_stream.write((char *)&pid, sizeof(pid)); + aux_stream.write((char *)&child.weight, sizeof(child.weight)); + DescManip::toStream(child.descriptor, aux_stream); + // add to parent list + if (!child.isLeaf()) parents.push_back(pit); + } + } + // words + // save size + uint32_t m_words_size = m_words.size(); + aux_stream.write((char *)&m_words_size, sizeof(m_words_size)); + for (auto wit = m_words.begin(); wit != m_words.end(); wit++) { + WordId id = wit - m_words.begin(); + aux_stream.write((char *)&id, sizeof(id)); + aux_stream.write((char *)&(*wit)->id, sizeof((*wit)->id)); + } + + // now, decide if compress or not + if (compressed) { + qlz_state_compress state_compress; + memset(&state_compress, 0, sizeof(qlz_state_compress)); + // Create output buffer + int chunkSize = 10000; + std::vector compressed(chunkSize + size_t(400), 0); + std::vector input(chunkSize, 0); + int64_t total_size = static_cast(aux_stream.tellp()); + uint64_t total_compress_size = 0; + // calculate how many chunks will be written + uint32_t nChunks = total_size / chunkSize; + if (total_size % chunkSize != 0) nChunks++; + out_str.write((char *)&nChunks, sizeof(nChunks)); + // start compressing the chunks + while (total_size != 0) { + int readSize = chunkSize; + if (total_size < chunkSize) readSize = total_size; + aux_stream.read(&input[0], readSize); + uint64_t compressed_size = + qlz_compress(&input[0], &compressed[0], readSize, &state_compress); + total_size -= readSize; + out_str.write(&compressed[0], compressed_size); + total_compress_size += compressed_size; + } + } else { + out_str << aux_stream.rdbuf(); + } +} + +void Vocabulary::load_fromtxt(const std::string &filename) { + std::ifstream ifile(filename); + if (!ifile) + throw std::runtime_error( + "Vocabulary:: load_fromtxt Could not open file for reading:" + + filename); + int n1, n2; + { + std::string str; + getline(ifile, str); + std::stringstream ss(str); + ss >> m_k >> m_L >> n1 >> n2; + } + if (m_k < 0 || m_k > 20 || m_L < 1 || m_L > 10 || n1 < 0 || n1 > 5 || + n2 < 0 || n2 > 3) + throw std::runtime_error( + "Vocabulary loading failure: This is not a correct text file!"); + + m_scoring = (ScoringType)n1; + m_weighting = (WeightingType)n2; + createScoringObject(); + // nodes + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1) / (m_k - 1)); + m_nodes.reserve(expected_nodes); + + m_words.reserve(pow((double)m_k, (double)m_L + 1)); + + m_nodes.resize(1); + m_nodes[0].id = 0; + + int counter = 0; + while (!ifile.eof()) { + std::string snode; + getline(ifile, snode); + if (counter++ % 100 == 0) std::cerr << "."; + // std::cout<> pid; + m_nodes[nid].parent = pid; + m_nodes[pid].children.push_back(nid); + + int nIsLeaf; + ssnode >> nIsLeaf; + + // read until the end and add to data + std::vector data; + data.reserve(100); + float d; + while (ssnode >> d) data.push_back(d); + // the weight is the last + m_nodes[nid].weight = data.back(); + data.pop_back(); // remove + // the rest, to the descriptor + m_nodes[nid].descriptor.create(1, data.size(), CV_8UC1); + auto ptr = m_nodes[nid].descriptor.ptr(0); + for (auto d : data) *ptr++ = d; + + if (nIsLeaf > 0) { + int wid = m_words.size(); + m_words.resize(wid + 1); + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } else { + m_nodes[nid].children.reserve(m_k); + } + } +} +void Vocabulary::fromStream(std::istream &str) { + m_words.clear(); + m_nodes.clear(); + uint64_t sig = 0; // magic number describing the file + str.read((char *)&sig, sizeof(sig)); + if (sig != 88877711233) + throw std::runtime_error( + "Vocabulary::fromStream is not of appropriate type"); + bool compressed; + str.read((char *)&compressed, sizeof(compressed)); + uint32_t nnodes; + str.read((char *)&nnodes, sizeof(nnodes)); + if (nnodes == 0) return; + std::stringstream decompressed_stream; + std::istream *_used_str = 0; + if (compressed) { + qlz_state_decompress state_decompress; + memset(&state_decompress, 0, sizeof(qlz_state_decompress)); + int chunkSize = 10000; + std::vector decompressed(chunkSize); + std::vector input(chunkSize + 400); + // read how many chunks are there + uint32_t nChunks; + str.read((char *)&nChunks, sizeof(nChunks)); + for (uint32_t i = 0; i < nChunks; i++) { + str.read(&input[0], 9); + int c = qlz_size_compressed(&input[0]); + str.read(&input[9], c - 9); + size_t d = qlz_decompress(&input[0], &decompressed[0], &state_decompress); + decompressed_stream.write(&decompressed[0], d); + } + _used_str = &decompressed_stream; + } else { + _used_str = &str; + } + + _used_str->read((char *)&m_k, sizeof(m_k)); + _used_str->read((char *)&m_L, sizeof(m_L)); + _used_str->read((char *)&m_scoring, sizeof(m_scoring)); + _used_str->read((char *)&m_weighting, sizeof(m_weighting)); + + createScoringObject(); + m_nodes.resize(nnodes); + m_nodes[0].id = 0; + + for (size_t i = 1; i < m_nodes.size(); ++i) { + NodeId nid; + _used_str->read((char *)&nid, sizeof(NodeId)); + Node &child = m_nodes[nid]; + child.id = nid; + _used_str->read((char *)&child.parent, sizeof(child.parent)); + _used_str->read((char *)&child.weight, sizeof(child.weight)); + DescManip::fromStream(child.descriptor, *_used_str); + m_nodes[child.parent].children.push_back(child.id); + } + // // words + uint32_t m_words_size; + _used_str->read((char *)&m_words_size, sizeof(m_words_size)); + m_words.resize(m_words_size); + for (unsigned int i = 0; i < m_words.size(); ++i) { + WordId wid; + NodeId nid; + _used_str->read((char *)&wid, sizeof(wid)); + _used_str->read((char *)&nid, sizeof(nid)); + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } +} +// -------------------------------------------------------------------------- + +void Vocabulary::load(const cv::FileStorage &fs, const std::string &name) { + m_words.clear(); + m_nodes.clear(); + + cv::FileNode fvoc = fs[name]; + + m_k = (int)fvoc["k"]; + m_L = (int)fvoc["L"]; + m_scoring = (ScoringType)((int)fvoc["scoringType"]); + m_weighting = (WeightingType)((int)fvoc["weightingType"]); + + createScoringObject(); + + // nodes + cv::FileNode fn = fvoc["nodes"]; + + m_nodes.resize(fn.size() + 1); // +1 to include root + m_nodes[0].id = 0; + + for (unsigned int i = 0; i < fn.size(); ++i) { + NodeId nid = (int)fn[i]["nodeId"]; + NodeId pid = (int)fn[i]["parentId"]; + WordValue weight = (WordValue)fn[i]["weight"]; + std::string d = (std::string)fn[i]["descriptor"]; + + m_nodes[nid].id = nid; + m_nodes[nid].parent = pid; + m_nodes[nid].weight = weight; + m_nodes[pid].children.push_back(nid); + + DescManip::fromString(m_nodes[nid].descriptor, d); + } + + // words + fn = fvoc["words"]; + + m_words.resize(fn.size()); + + for (unsigned int i = 0; i < fn.size(); ++i) { + NodeId wid = (int)fn[i]["wordId"]; + NodeId nid = (int)fn[i]["nodeId"]; + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } +} + +// -------------------------------------------------------------------------- + +/** + * Writes printable information of the vocabulary + * @param os stream to write to + * @param voc + */ + +std::ostream &operator<<(std::ostream &os, const Vocabulary &voc) { + os << "Vocabulary: k = " << voc.getBranchingFactor() + << ", L = " << voc.getDepthLevels() << ", Weighting = "; + + switch (voc.getWeightingType()) { + case TF_IDF: + os << "tf-idf"; + break; + case TF: + os << "tf"; + break; + case IDF: + os << "idf"; + break; + case BINARY: + os << "binary"; + break; + } + + os << ", Scoring = "; + switch (voc.getScoringType()) { + case L1_NORM: + os << "L1-norm"; + break; + case L2_NORM: + os << "L2-norm"; + break; + case CHI_SQUARE: + os << "Chi square distance"; + break; + case KL: + os << "KL-divergence"; + break; + case BHATTACHARYYA: + os << "Bhattacharyya coefficient"; + break; + case DOT_PRODUCT: + os << "Dot product"; + break; + } + + os << ", Number of words = " << voc.size(); + + return os; +} +/** + * @brief Vocabulary::clear + */ +void Vocabulary::clear() { + delete m_scoring_object; + m_scoring_object = 0; + m_nodes.clear(); + m_words.clear(); +} +int Vocabulary::getDescritorSize() const { + if (m_words.size() == 0) + return -1; + else + return m_words[0]->descriptor.cols; +} +int Vocabulary::getDescritorType() const { + if (m_words.size() == 0) + return -1; + else + return m_words[0]->descriptor.type(); +} +} diff --git a/thirdparty/DBoW3/src/Vocabulary.h b/thirdparty/DBoW3/src/Vocabulary.h new file mode 100644 index 0000000..d2966f5 --- /dev/null +++ b/thirdparty/DBoW3/src/Vocabulary.h @@ -0,0 +1,468 @@ +/** + * File: Vocabulary.h + * Date: February 2011 + * Author: Dorian Galvez-Lopez + * Description: templated vocabulary + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T__VOCABULARY__ +#define __D_T__VOCABULARY__ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "BowVector.h" +#include "FeatureVector.h" +#include "ScoringObject.h" +#include "exports.h" +namespace DBoW3 { +/// Vocabulary +class DBOW_API Vocabulary { + friend class FastSearch; + + public: + /** + * Initiates an empty vocabulary + * @param k branching factor + * @param L depth levels + * @param weighting weighting type + * @param scoring scoring type + */ + Vocabulary(int k = 10, int L = 5, WeightingType weighting = TF_IDF, + ScoringType scoring = L1_NORM); + + /** + * Creates the vocabulary by loading a file + * @param filename + */ + Vocabulary(const std::string &filename); + + /** + * Creates the vocabulary by loading a file + * @param filename + */ + Vocabulary(const char *filename); + + /** + * Copy constructor + * @param voc + */ + Vocabulary(const Vocabulary &voc); + + /** + * Destructor + */ + virtual ~Vocabulary(); + + /** + * Assigns the given vocabulary to this by copying its data and removing + * all the data contained by this vocabulary before + * @param voc + * @return reference to this vocabulary + */ + Vocabulary &operator=(const Vocabulary &voc); + + /** + * Creates a vocabulary from the training features with the already + * defined parameters + * @param training_features + */ + virtual void create( + const std::vector> &training_features); + /** + * Creates a vocabulary from the training features with the already + * defined parameters + * @param training_features. Each row of a matrix is a feature + */ + virtual void create(const std::vector &training_features); + + /** + * Creates a vocabulary from the training features, setting the branching + * factor and the depth levels of the tree + * @param training_features + * @param k branching factor + * @param L depth levels + */ + virtual void create( + const std::vector> &training_features, int k, int L); + + /** + * Creates a vocabulary from the training features, setting the branching + * factor nad the depth levels of the tree, and the weighting and scoring + * schemes + */ + virtual void create( + const std::vector> &training_features, int k, int L, + WeightingType weighting, ScoringType scoring); + + /** + * Returns the number of words in the vocabulary + * @return number of words + */ + virtual inline unsigned int size() const { + return (unsigned int)m_words.size(); + } + + /** + * Returns whether the vocabulary is empty (i.e. it has not been trained) + * @return true iff the vocabulary is empty + */ + virtual inline bool empty() const { return m_words.empty(); } + + /** Clears the vocabulary object + */ + void clear(); + /** + * Transforms a set of descriptores into a bow vector + * @param features + * @param v (out) bow vector of weighted words + */ + virtual void transform(const std::vector &features, + BowVector &v) const; + + virtual void transform(const std::vector> &features, + BowVector &v) const; + + /** + * Transforms a set of descriptores into a bow vector + * @param features, one per row + * @param v (out) bow vector of weighted words + */ + virtual void transform(const cv::Mat &features, BowVector &v) const; + /** + * Transform a set of descriptors into a bow vector and a feature vector + * @param features + * @param v (out) bow vector + * @param fv (out) feature vector of nodes and feature indexes + * @param levelsup levels to go up the vocabulary tree to get the node index + */ + virtual void transform(const std::vector &features, BowVector &v, + FeatureVector &fv, int levelsup) const; + + /** + * Transforms a single feature into a word (without weight) + * @param feature + * @return word id + */ + virtual WordId transform(const cv::Mat &feature) const; + + void transform(const std::vector> &features, BowVector &v, + FeatureVector &fv, int levelsup) const; + + /** + * Returns the score of two vectors + * @param a vector + * @param b vector + * @return score between vectors + * @note the vectors must be already sorted and normalized if necessary + */ + double score(const BowVector &a, const BowVector &b) const { + return m_scoring_object->score(a, b); + } + + /** + * Returns the id of the node that is "levelsup" levels from the word given + * @param wid word id + * @param levelsup 0..L + * @return node id. if levelsup is 0, returns the node id associated to the + * word id + */ + virtual NodeId getParentNode(WordId wid, int levelsup) const; + + /** + * Returns the ids of all the words that are under the given node id, + * by traversing any of the branches that goes down from the node + * @param nid starting node id + * @param words ids of words + */ + void getWordsFromNode(NodeId nid, std::vector &words) const; + + /** + * Returns the branching factor of the tree (k) + * @return k + */ + inline int getBranchingFactor() const { return m_k; } + + /** + * Returns the depth levels of the tree (L) + * @return L + */ + inline int getDepthLevels() const { return m_L; } + + /** + * Returns the real depth levels of the tree on average + * @return average of depth levels of leaves + */ + float getEffectiveLevels() const; + + /** + * Returns the descriptor of a word + * @param wid word id + * @return descriptor + */ + virtual inline cv::Mat getWord(WordId wid) const; + + /** + * Returns the weight of a word + * @param wid word id + * @return weight + */ + virtual inline WordValue getWordWeight(WordId wid) const; + + /** + * Returns the weighting method + * @return weighting method + */ + inline WeightingType getWeightingType() const { return m_weighting; } + + /** + * Returns the scoring method + * @return scoring method + */ + inline ScoringType getScoringType() const { return m_scoring; } + + /** + * Changes the weighting method + * @param type new weighting type + */ + inline void setWeightingType(WeightingType type); + + /** + * Changes the scoring method + * @param type new scoring type + */ + void setScoringType(ScoringType type); + + /** + * Saves the vocabulary into a file. If filename extension contains .yml, + * opencv YALM format is used. Otherwise, binary format is employed + * @param filename + */ + void save(const std::string &filename, bool binary_compressed = true) const; + + /** + * Loads the vocabulary from a file created with save + * @param filename. + */ + void load(const std::string &filename); + + /** + * Saves the vocabulary to a file storage structure + * @param fn node in file storage + */ + virtual void save(cv::FileStorage &fs, + const std::string &name = "vocabulary") const; + + /** + * Loads the vocabulary from a file storage node + * @param fn first node + * @param subname name of the child node of fn where the tree is stored. + * If not given, the fn node is used instead + */ + virtual void load(const cv::FileStorage &fs, + const std::string &name = "vocabulary"); + + /** + * Stops those words whose weight is below minWeight. + * Words are stopped by setting their weight to 0. There are not returned + * later when transforming image features into vectors. + * Note that when using IDF or TF_IDF, the weight is the idf part, which + * is equivalent to -log(f), where f is the frequency of the word + * (f = Ni/N, Ni: number of training images where the word is present, + * N: number of training images). + * Note that the old weight is forgotten, and subsequent calls to this + * function with a lower minWeight have no effect. + * @return number of words stopped now + */ + virtual int stopWords(double minWeight); + + /** Returns the size of the descriptor employed. If the Vocabulary is empty, + * returns -1 + */ + int getDescritorSize() const; + /** Returns the type of the descriptor employed normally(8U_C1, 32F_C1) + */ + int getDescritorType() const; + // io to-from a stream + void toStream(std::ostream &str, bool compressed = true) const; + void fromStream(std::istream &str); + + protected: + /// reference to descriptor + typedef const cv::Mat pDescriptor; + + /// Tree node + struct Node { + /// Node id + NodeId id; + /// Weight if the node is a word + WordValue weight; + /// Children + std::vector children; + /// Parent node (undefined in case of root) + NodeId parent; + /// Node descriptor + cv::Mat descriptor; + + /// Word id if the node is a word + WordId word_id; + + /** + * Empty constructor + */ + Node() : id(0), weight(0), parent(0), word_id(0) {} + + /** + * Constructor + * @param _id node id + */ + Node(NodeId _id) : id(_id), weight(0), parent(0), word_id(0) {} + + /** + * Returns whether the node is a leaf node + * @return true iff the node is a leaf + */ + inline bool isLeaf() const { return children.empty(); } + }; + + protected: + /** + * Creates an instance of the scoring object accoring to m_scoring + */ + void createScoringObject(); + + /** + * Returns a set of pointers to descriptores + * @param training_features all the features + * @param features (out) pointers to the training features + */ + void getFeatures(const std::vector> &training_features, + std::vector &features) const; + + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + * @param weight (out) word weight + * @param nid (out) if given, id of the node "levelsup" levels up + * @param levelsup + */ + virtual void transform(const cv::Mat &feature, WordId &id, WordValue &weight, + NodeId *nid, int levelsup = 0) const; + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + * @param weight (out) word weight + * @param nid (out) if given, id of the node "levelsup" levels up + * @param levelsup + */ + virtual void transform(const cv::Mat &feature, WordId &id, + WordValue &weight) const; + + void transform(const std::bitset<256> &feature, WordId &word_id, + WordValue &weight) const; + + virtual void transform(const std::bitset<256> &feature, WordId &word_id, + WordValue &weight, NodeId *nid, int levelsup) const; + + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + */ + virtual void transform(const cv::Mat &feature, WordId &id) const; + + /** + * Creates a level in the tree, under the parent, by running kmeans with + * a descriptor set, and recursively creates the subsequent levels too + * @param parent_id id of parent node + * @param descriptors descriptors to run the kmeans on + * @param current_level current level in the tree + */ + void HKmeansStep(NodeId parent_id, const std::vector &descriptors, + int current_level); + + /** + * Creates k clusters from the given descriptors with some seeding algorithm. + * @note In this class, kmeans++ is used, but this function should be + * overriden by inherited classes. + */ + virtual void initiateClusters(const std::vector &descriptors, + std::vector &clusters) const; + + /** + * Creates k clusters from the given descriptor sets by running the + * initial step of kmeans++ + * @param descriptors + * @param clusters resulting clusters + */ + void initiateClustersKMpp(const std::vector &descriptors, + std::vector &clusters) const; + + /** + * Create the words of the vocabulary once the tree has been built + */ + void createWords(); + + /** + * Sets the weights of the nodes of tree according to the given features. + * Before calling this function, the nodes and the words must be already + * created (by calling HKmeansStep and createWords) + * @param features + */ + void setNodeWeights(const std::vector> &features); + + /** + * Writes printable information of the vocabulary + * @param os stream to write to + * @param voc + */ + DBOW_API friend std::ostream &operator<<(std::ostream &os, + const Vocabulary &voc); + + /**Loads from ORBSLAM txt files + */ + void load_fromtxt(const std::string &filename); + + protected: + /// Branching factor + int m_k; + + /// Depth levels + int m_L; + + /// Weighting method + WeightingType m_weighting; + + /// Scoring method + ScoringType m_scoring; + + /// Object for computing scores + GeneralScoring *m_scoring_object; + + /// Tree nodes + std::vector m_nodes; + + /// Words of the vocabulary (tree leaves) + /// this condition holds: m_words[wid]->word_id == wid + std::vector m_words; + + public: + // for debug (REMOVE) + inline Node *getNodeWord(uint32_t idx) { return m_words[idx]; } +}; + +} // namespace DBoW3 + +#endif diff --git a/thirdparty/DBoW3/src/exports.h b/thirdparty/DBoW3/src/exports.h new file mode 100644 index 0000000..c324953 --- /dev/null +++ b/thirdparty/DBoW3/src/exports.h @@ -0,0 +1,51 @@ +/***************************** +Copyright 2014 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. 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. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''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 Rafael Muñoz Salinas OR +CONTRIBUTORS 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. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ + + + +#ifndef __DBOW_CORE_TYPES_H__ +#define __DBOW_CORE_TYPES_H__ + +#if !defined _CRT_SECURE_NO_DEPRECATE && _MSC_VER > 1300 +#define _CRT_SECURE_NO_DEPRECATE /* to avoid multiple Visual Studio 2005 warnings */ +#endif + +#if (defined WIN32 || defined _WIN32 || defined WINCE) && defined DBOW_DSO_EXPORTS + #define DBOW_API __declspec(dllexport) + #pragma warning ( disable : 4251 ) //disable warning to templates with dll linkage. + #pragma warning ( disable : 4290 ) //disable warning due to exception specifications. + #pragma warning ( disable : 4996 ) //disable warning regarding unsafe vsprintf. + #pragma warning ( disable : 4244 ) //disable warning convesions with lost of data. + +#else + #define DBOW_API +#endif + + +#define DBOW_VERSION "3.0.0" +#endif diff --git a/thirdparty/DBoW3/src/quicklz.c b/thirdparty/DBoW3/src/quicklz.c new file mode 100644 index 0000000..3742129 --- /dev/null +++ b/thirdparty/DBoW3/src/quicklz.c @@ -0,0 +1,848 @@ +// Fast data compression library +// Copyright (C) 2006-2011 Lasse Mikkel Reinhold +// lar@quicklz.com +// +// QuickLZ can be used for free under the GPL 1, 2 or 3 license (where anything +// released into public must be open source) or under a commercial license if such +// has been acquired (see http://www.quicklz.com/order.html). The commercial license +// does not cover derived or ported versions created by third parties under GPL. + +// 1.5.0 final + +#include "quicklz.h" + +#if QLZ_VERSION_MAJOR != 1 || QLZ_VERSION_MINOR != 5 || QLZ_VERSION_REVISION != 0 + #error quicklz.c and quicklz.h have different versions +#endif + +#if (defined(__X86__) || defined(__i386__) || defined(i386) || defined(_M_IX86) || defined(__386__) || defined(__x86_64__) || defined(_M_X64)) + #define X86X64 +#endif + +#define MINOFFSET 2 +#define UNCONDITIONAL_MATCHLEN 6 +#define UNCOMPRESSED_END 4 +#define CWORD_LEN 4 + +#if QLZ_COMPRESSION_LEVEL == 1 && defined QLZ_PTR_64 && QLZ_STREAMING_BUFFER == 0 + #define OFFSET_BASE source + #define CAST (ui32)(size_t) +#else + #define OFFSET_BASE 0 + #define CAST +#endif + +int qlz_get_setting(int setting) +{ + switch (setting) + { + case 0: return QLZ_COMPRESSION_LEVEL; + case 1: return sizeof(qlz_state_compress); + case 2: return sizeof(qlz_state_decompress); + case 3: return QLZ_STREAMING_BUFFER; +#ifdef QLZ_MEMORY_SAFE + case 6: return 1; +#else + case 6: return 0; +#endif + case 7: return QLZ_VERSION_MAJOR; + case 8: return QLZ_VERSION_MINOR; + case 9: return QLZ_VERSION_REVISION; + } + return -1; +} + +#if QLZ_COMPRESSION_LEVEL == 1 +static int same(const unsigned char *src, size_t n) +{ + while(n > 0 && *(src + n) == *src) + n--; + return n == 0 ? 1 : 0; +} +#endif + +static void reset_table_compress(qlz_state_compress *state) +{ + int i; + for(i = 0; i < QLZ_HASH_VALUES; i++) + { +#if QLZ_COMPRESSION_LEVEL == 1 + state->hash[i].offset = 0; +#else + state->hash_counter[i] = 0; +#endif + } +} + +static void reset_table_decompress(qlz_state_decompress *state) +{ + int i; + (void)state; + (void)i; +#if QLZ_COMPRESSION_LEVEL == 2 + for(i = 0; i < QLZ_HASH_VALUES; i++) + { + state->hash_counter[i] = 0; + } +#endif +} + +static __inline ui32 hash_func(ui32 i) +{ +#if QLZ_COMPRESSION_LEVEL == 2 + return ((i >> 9) ^ (i >> 13) ^ i) & (QLZ_HASH_VALUES - 1); +#else + return ((i >> 12) ^ i) & (QLZ_HASH_VALUES - 1); +#endif +} + +static __inline ui32 fast_read(void const *src, ui32 bytes) +{ +#ifndef X86X64 + unsigned char *p = (unsigned char*)src; + switch (bytes) + { + case 4: + return(*p | *(p + 1) << 8 | *(p + 2) << 16 | *(p + 3) << 24); + case 3: + return(*p | *(p + 1) << 8 | *(p + 2) << 16); + case 2: + return(*p | *(p + 1) << 8); + case 1: + return(*p); + } + return 0; +#else + if (bytes >= 1 && bytes <= 4) + return *((ui32*)src); + else + return 0; +#endif +} + +static __inline ui32 hashat(const unsigned char *src) +{ + ui32 fetch, hash; + fetch = fast_read(src, 3); + hash = hash_func(fetch); + return hash; +} + +static __inline void fast_write(ui32 f, void *dst, size_t bytes) +{ +#ifndef X86X64 + unsigned char *p = (unsigned char*)dst; + + switch (bytes) + { + case 4: + *p = (unsigned char)f; + *(p + 1) = (unsigned char)(f >> 8); + *(p + 2) = (unsigned char)(f >> 16); + *(p + 3) = (unsigned char)(f >> 24); + return; + case 3: + *p = (unsigned char)f; + *(p + 1) = (unsigned char)(f >> 8); + *(p + 2) = (unsigned char)(f >> 16); + return; + case 2: + *p = (unsigned char)f; + *(p + 1) = (unsigned char)(f >> 8); + return; + case 1: + *p = (unsigned char)f; + return; + } +#else + switch (bytes) + { + case 4: + *((ui32*)dst) = f; + return; + case 3: + *((ui32*)dst) = f; + return; + case 2: + *((ui16 *)dst) = (ui16)f; + return; + case 1: + *((unsigned char*)dst) = (unsigned char)f; + return; + } +#endif +} + + +size_t qlz_size_decompressed(const char *source) +{ + ui32 n, r; + n = (((*source) & 2) == 2) ? 4 : 1; + r = fast_read(source + 1 + n, n); + r = r & (0xffffffff >> ((4 - n)*8)); + return r; +} + +size_t qlz_size_compressed(const char *source) +{ + ui32 n, r; + n = (((*source) & 2) == 2) ? 4 : 1; + r = fast_read(source + 1, n); + r = r & (0xffffffff >> ((4 - n)*8)); + return r; +} + +size_t qlz_size_header(const char *source) +{ + size_t n = 2*((((*source) & 2) == 2) ? 4 : 1) + 1; + return n; +} + + +static __inline void memcpy_up(unsigned char *dst, const unsigned char *src, ui32 n) +{ + // Caution if modifying memcpy_up! Overlap of dst and src must be special handled. +#ifndef X86X64 + unsigned char *end = dst + n; + while(dst < end) + { + *dst = *src; + dst++; + src++; + } +#else + ui32 f = 0; + do + { + *(ui32 *)(dst + f) = *(ui32 *)(src + f); + f += MINOFFSET + 1; + } + while (f < n); +#endif +} + +static __inline void update_hash(qlz_state_decompress *state, const unsigned char *s) +{ +#if QLZ_COMPRESSION_LEVEL == 1 + ui32 hash; + hash = hashat(s); + state->hash[hash].offset = s; + state->hash_counter[hash] = 1; +#elif QLZ_COMPRESSION_LEVEL == 2 + ui32 hash; + unsigned char c; + hash = hashat(s); + c = state->hash_counter[hash]; + state->hash[hash].offset[c & (QLZ_POINTERS - 1)] = s; + c++; + state->hash_counter[hash] = c; +#endif + (void)state; + (void)s; +} + +#if QLZ_COMPRESSION_LEVEL <= 2 +static void update_hash_upto(qlz_state_decompress *state, unsigned char **lh, const unsigned char *max) +{ + while(*lh < max) + { + (*lh)++; + update_hash(state, *lh); + } +} +#endif + +static size_t qlz_compress_core(const unsigned char *source, unsigned char *destination, size_t size, qlz_state_compress *state) +{ + const unsigned char *last_byte = source + size - 1; + const unsigned char *src = source; + unsigned char *cword_ptr = destination; + unsigned char *dst = destination + CWORD_LEN; + ui32 cword_val = 1U << 31; + const unsigned char *last_matchstart = last_byte - UNCONDITIONAL_MATCHLEN - UNCOMPRESSED_END; + ui32 fetch = 0; + unsigned int lits = 0; + + (void) lits; + + if(src <= last_matchstart) + fetch = fast_read(src, 3); + + while(src <= last_matchstart) + { + if ((cword_val & 1) == 1) + { + // store uncompressed if compression ratio is too low + if (src > source + (size >> 1) && dst - destination > src - source - ((src - source) >> 5)) + return 0; + + fast_write((cword_val >> 1) | (1U << 31), cword_ptr, CWORD_LEN); + + cword_ptr = dst; + dst += CWORD_LEN; + cword_val = 1U << 31; + fetch = fast_read(src, 3); + } +#if QLZ_COMPRESSION_LEVEL == 1 + { + const unsigned char *o; + ui32 hash, cached; + + hash = hash_func(fetch); + cached = fetch ^ state->hash[hash].cache; + state->hash[hash].cache = fetch; + + o = state->hash[hash].offset + OFFSET_BASE; + state->hash[hash].offset = CAST(src - OFFSET_BASE); + +#ifdef X86X64 + if ((cached & 0xffffff) == 0 && o != OFFSET_BASE && (src - o > MINOFFSET || (src == o + 1 && lits >= 3 && src > source + 3 && same(src - 3, 6)))) + { + if(cached != 0) + { +#else + if (cached == 0 && o != OFFSET_BASE && (src - o > MINOFFSET || (src == o + 1 && lits >= 3 && src > source + 3 && same(src - 3, 6)))) + { + if (*(o + 3) != *(src + 3)) + { +#endif + hash <<= 4; + cword_val = (cword_val >> 1) | (1U << 31); + fast_write((3 - 2) | hash, dst, 2); + src += 3; + dst += 2; + } + else + { + const unsigned char *old_src = src; + size_t matchlen; + hash <<= 4; + + cword_val = (cword_val >> 1) | (1U << 31); + src += 4; + + if(*(o + (src - old_src)) == *src) + { + src++; + if(*(o + (src - old_src)) == *src) + { + size_t q = last_byte - UNCOMPRESSED_END - (src - 5) + 1; + size_t remaining = q > 255 ? 255 : q; + src++; + while(*(o + (src - old_src)) == *src && (size_t)(src - old_src) < remaining) + src++; + } + } + + matchlen = src - old_src; + if (matchlen < 18) + { + fast_write((ui32)(matchlen - 2) | hash, dst, 2); + dst += 2; + } + else + { + fast_write((ui32)(matchlen << 16) | hash, dst, 3); + dst += 3; + } + } + fetch = fast_read(src, 3); + lits = 0; + } + else + { + lits++; + *dst = *src; + src++; + dst++; + cword_val = (cword_val >> 1); +#ifdef X86X64 + fetch = fast_read(src, 3); +#else + fetch = (fetch >> 8 & 0xffff) | (*(src + 2) << 16); +#endif + } + } +#elif QLZ_COMPRESSION_LEVEL >= 2 + { + const unsigned char *o, *offset2; + ui32 hash, matchlen, k, m, best_k = 0; + unsigned char c; + size_t remaining = (last_byte - UNCOMPRESSED_END - src + 1) > 255 ? 255 : (last_byte - UNCOMPRESSED_END - src + 1); + (void)best_k; + + + //hash = hashat(src); + fetch = fast_read(src, 3); + hash = hash_func(fetch); + + c = state->hash_counter[hash]; + + offset2 = state->hash[hash].offset[0]; + if(offset2 < src - MINOFFSET && c > 0 && ((fast_read(offset2, 3) ^ fetch) & 0xffffff) == 0) + { + matchlen = 3; + if(*(offset2 + matchlen) == *(src + matchlen)) + { + matchlen = 4; + while(*(offset2 + matchlen) == *(src + matchlen) && matchlen < remaining) + matchlen++; + } + } + else + matchlen = 0; + for(k = 1; k < QLZ_POINTERS && c > k; k++) + { + o = state->hash[hash].offset[k]; +#if QLZ_COMPRESSION_LEVEL == 3 + if(((fast_read(o, 3) ^ fetch) & 0xffffff) == 0 && o < src - MINOFFSET) +#elif QLZ_COMPRESSION_LEVEL == 2 + if(*(src + matchlen) == *(o + matchlen) && ((fast_read(o, 3) ^ fetch) & 0xffffff) == 0 && o < src - MINOFFSET) +#endif + { + m = 3; + while(*(o + m) == *(src + m) && m < remaining) + m++; +#if QLZ_COMPRESSION_LEVEL == 3 + if ((m > matchlen) || (m == matchlen && o > offset2)) +#elif QLZ_COMPRESSION_LEVEL == 2 + if (m > matchlen) +#endif + { + offset2 = o; + matchlen = m; + best_k = k; + } + } + } + o = offset2; + state->hash[hash].offset[c & (QLZ_POINTERS - 1)] = src; + c++; + state->hash_counter[hash] = c; + +#if QLZ_COMPRESSION_LEVEL == 3 + if(matchlen > 2 && src - o < 131071) + { + ui32 u; + size_t offset = src - o; + + for(u = 1; u < matchlen; u++) + { + hash = hashat(src + u); + c = state->hash_counter[hash]++; + state->hash[hash].offset[c & (QLZ_POINTERS - 1)] = src + u; + } + + cword_val = (cword_val >> 1) | (1U << 31); + src += matchlen; + + if(matchlen == 3 && offset <= 63) + { + *dst = (unsigned char)(offset << 2); + dst++; + } + else if (matchlen == 3 && offset <= 16383) + { + ui32 f = (ui32)((offset << 2) | 1); + fast_write(f, dst, 2); + dst += 2; + } + else if (matchlen <= 18 && offset <= 1023) + { + ui32 f = ((matchlen - 3) << 2) | ((ui32)offset << 6) | 2; + fast_write(f, dst, 2); + dst += 2; + } + + else if(matchlen <= 33) + { + ui32 f = ((matchlen - 2) << 2) | ((ui32)offset << 7) | 3; + fast_write(f, dst, 3); + dst += 3; + } + else + { + ui32 f = ((matchlen - 3) << 7) | ((ui32)offset << 15) | 3; + fast_write(f, dst, 4); + dst += 4; + } + } + else + { + *dst = *src; + src++; + dst++; + cword_val = (cword_val >> 1); + } +#elif QLZ_COMPRESSION_LEVEL == 2 + + if(matchlen > 2) + { + cword_val = (cword_val >> 1) | (1U << 31); + src += matchlen; + + if (matchlen < 10) + { + ui32 f = best_k | ((matchlen - 2) << 2) | (hash << 5); + fast_write(f, dst, 2); + dst += 2; + } + else + { + ui32 f = best_k | (matchlen << 16) | (hash << 5); + fast_write(f, dst, 3); + dst += 3; + } + } + else + { + *dst = *src; + src++; + dst++; + cword_val = (cword_val >> 1); + } +#endif + } +#endif + } + while (src <= last_byte) + { + if ((cword_val & 1) == 1) + { + fast_write((cword_val >> 1) | (1U << 31), cword_ptr, CWORD_LEN); + cword_ptr = dst; + dst += CWORD_LEN; + cword_val = 1U << 31; + } +#if QLZ_COMPRESSION_LEVEL < 3 + if (src <= last_byte - 3) + { +#if QLZ_COMPRESSION_LEVEL == 1 + ui32 hash, fetch; + fetch = fast_read(src, 3); + hash = hash_func(fetch); + state->hash[hash].offset = CAST(src - OFFSET_BASE); + state->hash[hash].cache = fetch; +#elif QLZ_COMPRESSION_LEVEL == 2 + ui32 hash; + unsigned char c; + hash = hashat(src); + c = state->hash_counter[hash]; + state->hash[hash].offset[c & (QLZ_POINTERS - 1)] = src; + c++; + state->hash_counter[hash] = c; +#endif + } +#endif + *dst = *src; + src++; + dst++; + cword_val = (cword_val >> 1); + } + + while((cword_val & 1) != 1) + cword_val = (cword_val >> 1); + + fast_write((cword_val >> 1) | (1U << 31), cword_ptr, CWORD_LEN); + + // min. size must be 9 bytes so that the qlz_size functions can take 9 bytes as argument + return dst - destination < 9 ? 9 : dst - destination; +} + +static size_t qlz_decompress_core(const unsigned char *source, unsigned char *destination, size_t size, qlz_state_decompress *state, const unsigned char *history) +{ + const unsigned char *src = source + qlz_size_header((const char *)source); + unsigned char *dst = destination; + const unsigned char *last_destination_byte = destination + size - 1; + ui32 cword_val = 1; + const unsigned char *last_matchstart = last_destination_byte - UNCONDITIONAL_MATCHLEN - UNCOMPRESSED_END; + unsigned char *last_hashed = destination - 1; + const unsigned char *last_source_byte = source + qlz_size_compressed((const char *)source) - 1; + static const ui32 bitlut[16] = {4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0}; + + (void) last_source_byte; + (void) last_hashed; + (void) state; + (void) history; + + for(;;) + { + ui32 fetch; + + if (cword_val == 1) + { +#ifdef QLZ_MEMORY_SAFE + if(src + CWORD_LEN - 1 > last_source_byte) + return 0; +#endif + cword_val = fast_read(src, CWORD_LEN); + src += CWORD_LEN; + } + +#ifdef QLZ_MEMORY_SAFE + if(src + 4 - 1 > last_source_byte) + return 0; +#endif + + fetch = fast_read(src, 4); + + if ((cword_val & 1) == 1) + { + ui32 matchlen; + const unsigned char *offset2; + +#if QLZ_COMPRESSION_LEVEL == 1 + ui32 hash; + cword_val = cword_val >> 1; + hash = (fetch >> 4) & 0xfff; + offset2 = (const unsigned char *)(size_t)state->hash[hash].offset; + + if((fetch & 0xf) != 0) + { + matchlen = (fetch & 0xf) + 2; + src += 2; + } + else + { + matchlen = *(src + 2); + src += 3; + } + +#elif QLZ_COMPRESSION_LEVEL == 2 + ui32 hash; + unsigned char c; + cword_val = cword_val >> 1; + hash = (fetch >> 5) & 0x7ff; + c = (unsigned char)(fetch & 0x3); + offset2 = state->hash[hash].offset[c]; + + if((fetch & (28)) != 0) + { + matchlen = ((fetch >> 2) & 0x7) + 2; + src += 2; + } + else + { + matchlen = *(src + 2); + src += 3; + } + +#elif QLZ_COMPRESSION_LEVEL == 3 + ui32 offset; + cword_val = cword_val >> 1; + if ((fetch & 3) == 0) + { + offset = (fetch & 0xff) >> 2; + matchlen = 3; + src++; + } + else if ((fetch & 2) == 0) + { + offset = (fetch & 0xffff) >> 2; + matchlen = 3; + src += 2; + } + else if ((fetch & 1) == 0) + { + offset = (fetch & 0xffff) >> 6; + matchlen = ((fetch >> 2) & 15) + 3; + src += 2; + } + else if ((fetch & 127) != 3) + { + offset = (fetch >> 7) & 0x1ffff; + matchlen = ((fetch >> 2) & 0x1f) + 2; + src += 3; + } + else + { + offset = (fetch >> 15); + matchlen = ((fetch >> 7) & 255) + 3; + src += 4; + } + + offset2 = dst - offset; +#endif + +#ifdef QLZ_MEMORY_SAFE + if(offset2 < history || offset2 > dst - MINOFFSET - 1) + return 0; + + if(matchlen > (ui32)(last_destination_byte - dst - UNCOMPRESSED_END + 1)) + return 0; +#endif + + memcpy_up(dst, offset2, matchlen); + dst += matchlen; + +#if QLZ_COMPRESSION_LEVEL <= 2 + update_hash_upto(state, &last_hashed, dst - matchlen); + last_hashed = dst - 1; +#endif + } + else + { + if (dst < last_matchstart) + { + unsigned int n = bitlut[cword_val & 0xf]; +#ifdef X86X64 + *(ui32 *)dst = *(ui32 *)src; +#else + memcpy_up(dst, src, 4); +#endif + cword_val = cword_val >> n; + dst += n; + src += n; +#if QLZ_COMPRESSION_LEVEL <= 2 + update_hash_upto(state, &last_hashed, dst - 3); +#endif + } + else + { + while(dst <= last_destination_byte) + { + if (cword_val == 1) + { + src += CWORD_LEN; + cword_val = 1U << 31; + } +#ifdef QLZ_MEMORY_SAFE + if(src >= last_source_byte + 1) + return 0; +#endif + *dst = *src; + dst++; + src++; + cword_val = cword_val >> 1; + } + +#if QLZ_COMPRESSION_LEVEL <= 2 + update_hash_upto(state, &last_hashed, last_destination_byte - 3); // todo, use constant +#endif + return size; + } + + } + } +} + +size_t qlz_compress(const void *source, char *destination, size_t size, qlz_state_compress *state) +{ + size_t r; + ui32 compressed; + size_t base; + + if(size == 0 || size > 0xffffffff - 400) + return 0; + + if(size < 216) + base = 3; + else + base = 9; + +#if QLZ_STREAMING_BUFFER > 0 + if (state->stream_counter + size - 1 >= QLZ_STREAMING_BUFFER) +#endif + { + reset_table_compress(state); + r = base + qlz_compress_core((const unsigned char *)source, (unsigned char*)destination + base, size, state); +#if QLZ_STREAMING_BUFFER > 0 + reset_table_compress(state); +#endif + if(r == base) + { + memcpy(destination + base, source, size); + r = size + base; + compressed = 0; + } + else + { + compressed = 1; + } + state->stream_counter = 0; + } +#if QLZ_STREAMING_BUFFER > 0 + else + { + unsigned char *src = state->stream_buffer + state->stream_counter; + + memcpy(src, source, size); + r = base + qlz_compress_core(src, (unsigned char*)destination + base, size, state); + + if(r == base) + { + memcpy(destination + base, src, size); + r = size + base; + compressed = 0; + reset_table_compress(state); + } + else + { + compressed = 1; + } + state->stream_counter += size; + } +#endif + if(base == 3) + { + *destination = (unsigned char)(0 | compressed); + *(destination + 1) = (unsigned char)r; + *(destination + 2) = (unsigned char)size; + } + else + { + *destination = (unsigned char)(2 | compressed); + fast_write((ui32)r, destination + 1, 4); + fast_write((ui32)size, destination + 5, 4); + } + + *destination |= (QLZ_COMPRESSION_LEVEL << 2); + *destination |= (1 << 6); + *destination |= ((QLZ_STREAMING_BUFFER == 0 ? 0 : (QLZ_STREAMING_BUFFER == 100000 ? 1 : (QLZ_STREAMING_BUFFER == 1000000 ? 2 : 3))) << 4); + +// 76543210 +// 01SSLLHC + + return r; +} + +size_t qlz_decompress(const char *source, void *destination, qlz_state_decompress *state) +{ + size_t dsiz = qlz_size_decompressed(source); + +#if QLZ_STREAMING_BUFFER > 0 + if (state->stream_counter + qlz_size_decompressed(source) - 1 >= QLZ_STREAMING_BUFFER) +#endif + { + if((*source & 1) == 1) + { + reset_table_decompress(state); + dsiz = qlz_decompress_core((const unsigned char *)source, (unsigned char *)destination, dsiz, state, (const unsigned char *)destination); + } + else + { + memcpy(destination, source + qlz_size_header(source), dsiz); + } + state->stream_counter = 0; + reset_table_decompress(state); + } +#if QLZ_STREAMING_BUFFER > 0 + else + { + unsigned char *dst = state->stream_buffer + state->stream_counter; + if((*source & 1) == 1) + { + dsiz = qlz_decompress_core((const unsigned char *)source, dst, dsiz, state, (const unsigned char *)state->stream_buffer); + } + else + { + memcpy(dst, source + qlz_size_header(source), dsiz); + reset_table_decompress(state); + } + memcpy(destination, dst, dsiz); + state->stream_counter += dsiz; + } +#endif + return dsiz; +} + diff --git a/thirdparty/DBoW3/src/quicklz.h b/thirdparty/DBoW3/src/quicklz.h new file mode 100644 index 0000000..6a710f1 --- /dev/null +++ b/thirdparty/DBoW3/src/quicklz.h @@ -0,0 +1,150 @@ +#ifndef QLZ_HEADER +#define QLZ_HEADER + +// Fast data compression library +// Copyright (C) 2006-2011 Lasse Mikkel Reinhold +// lar@quicklz.com +// +// QuickLZ can be used for free under the GPL 1, 2 or 3 license (where anything +// released into public must be open source) or under a commercial license if such +// has been acquired (see http://www.quicklz.com/order.html). The commercial license +// does not cover derived or ported versions created by third parties under GPL. + +// You can edit following user settings. Data must be decompressed with the same +// setting of QLZ_COMPRESSION_LEVEL and QLZ_STREAMING_BUFFER as it was compressed +// (see manual). If QLZ_STREAMING_BUFFER > 0, scratch buffers must be initially +// zeroed out (see manual). First #ifndef makes it possible to define settings from +// the outside like the compiler command line. + +// 1.5.0 final + +#ifndef QLZ_COMPRESSION_LEVEL + + // 1 gives fastest compression speed. 3 gives fastest decompression speed and best + // compression ratio. + #define QLZ_COMPRESSION_LEVEL 1 + //#define QLZ_COMPRESSION_LEVEL 2 + //#define QLZ_COMPRESSION_LEVEL 3 + + // If > 0, zero out both states prior to first call to qlz_compress() or qlz_decompress() + // and decompress packets in the same order as they were compressed + #define QLZ_STREAMING_BUFFER 0 + //#define QLZ_STREAMING_BUFFER 100000 + //#define QLZ_STREAMING_BUFFER 1000000 + + // Guarantees that decompression of corrupted data cannot crash. Decreases decompression + // speed 10-20%. Compression speed not affected. + //#define QLZ_MEMORY_SAFE +#endif + +#define QLZ_VERSION_MAJOR 1 +#define QLZ_VERSION_MINOR 5 +#define QLZ_VERSION_REVISION 0 + +// Using size_t, memset() and memcpy() +#include + +// Verify compression level +#if QLZ_COMPRESSION_LEVEL != 1 && QLZ_COMPRESSION_LEVEL != 2 && QLZ_COMPRESSION_LEVEL != 3 +#error QLZ_COMPRESSION_LEVEL must be 1, 2 or 3 +#endif + +typedef unsigned int ui32; +typedef unsigned short int ui16; + +// Decrease QLZ_POINTERS for level 3 to increase compression speed. Do not touch any other values! +#if QLZ_COMPRESSION_LEVEL == 1 +#define QLZ_POINTERS 1 +#define QLZ_HASH_VALUES 4096 +#elif QLZ_COMPRESSION_LEVEL == 2 +#define QLZ_POINTERS 4 +#define QLZ_HASH_VALUES 2048 +#elif QLZ_COMPRESSION_LEVEL == 3 +#define QLZ_POINTERS 16 +#define QLZ_HASH_VALUES 4096 +#endif + +// Detect if pointer size is 64-bit. It's not fatal if some 64-bit target is not detected because this is only for adding an optional 64-bit optimization. +#if defined _LP64 || defined __LP64__ || defined __64BIT__ || _ADDR64 || defined _WIN64 || defined __arch64__ || __WORDSIZE == 64 || (defined __sparc && defined __sparcv9) || defined __x86_64 || defined __amd64 || defined __x86_64__ || defined _M_X64 || defined _M_IA64 || defined __ia64 || defined __IA64__ + #define QLZ_PTR_64 +#endif + +// hash entry +typedef struct +{ +#if QLZ_COMPRESSION_LEVEL == 1 + ui32 cache; +#if defined QLZ_PTR_64 && QLZ_STREAMING_BUFFER == 0 + unsigned int offset; +#else + const unsigned char *offset; +#endif +#else + const unsigned char *offset[QLZ_POINTERS]; +#endif + +} qlz_hash_compress; + +typedef struct +{ +#if QLZ_COMPRESSION_LEVEL == 1 + const unsigned char *offset; +#else + const unsigned char *offset[QLZ_POINTERS]; +#endif +} qlz_hash_decompress; + + +// states +typedef struct +{ + #if QLZ_STREAMING_BUFFER > 0 + unsigned char stream_buffer[QLZ_STREAMING_BUFFER]; + #endif + size_t stream_counter; + qlz_hash_compress hash[QLZ_HASH_VALUES]; + unsigned char hash_counter[QLZ_HASH_VALUES]; +} qlz_state_compress; + + +#if QLZ_COMPRESSION_LEVEL == 1 || QLZ_COMPRESSION_LEVEL == 2 + typedef struct + { +#if QLZ_STREAMING_BUFFER > 0 + unsigned char stream_buffer[QLZ_STREAMING_BUFFER]; +#endif + qlz_hash_decompress hash[QLZ_HASH_VALUES]; + unsigned char hash_counter[QLZ_HASH_VALUES]; + size_t stream_counter; + } qlz_state_decompress; +#elif QLZ_COMPRESSION_LEVEL == 3 + typedef struct + { +#if QLZ_STREAMING_BUFFER > 0 + unsigned char stream_buffer[QLZ_STREAMING_BUFFER]; +#endif +#if QLZ_COMPRESSION_LEVEL <= 2 + qlz_hash_decompress hash[QLZ_HASH_VALUES]; +#endif + size_t stream_counter; + } qlz_state_decompress; +#endif + + +#if defined (__cplusplus) +extern "C" { +#endif + +// Public functions of QuickLZ +size_t qlz_size_decompressed(const char *source); +size_t qlz_size_compressed(const char *source); +size_t qlz_compress(const void *source, char *destination, size_t size, qlz_state_compress *state); +size_t qlz_decompress(const char *source, void *destination, qlz_state_decompress *state); +int qlz_get_setting(int setting); + +#if defined (__cplusplus) +} +#endif + +#endif + diff --git a/thirdparty/DBoW3/src/timers.h b/thirdparty/DBoW3/src/timers.h new file mode 100644 index 0000000..98fd0fc --- /dev/null +++ b/thirdparty/DBoW3/src/timers.h @@ -0,0 +1,159 @@ +#ifndef DBoW3_TIMERS_H +#define DBoW3_TIMERS_H + +#include +#include +#include +#include +namespace DBoW3 { + +// timer +struct ScopeTimer { + std::chrono::high_resolution_clock::time_point begin, end; + + std::string name; + bool use; + enum SCALE { NSEC, MSEC, SEC }; + SCALE sc; + ScopeTimer(std::string name_, bool use_ = true, SCALE _sc = MSEC) { + name = name_; + use = use_; + sc = _sc; + begin = std::chrono::high_resolution_clock::now(); + } + ~ScopeTimer() { + if (use) { + end = std::chrono::high_resolution_clock::now(); + double fact = 1; + std::string str; + switch (sc) { + case NSEC: + fact = 1; + str = "ns"; + break; + case MSEC: + fact = 1e6; + str = "ms"; + break; + case SEC: + fact = 1e9; + str = "s"; + break; + }; + + std::cout << "Time (" << name << ")= " + << double(std::chrono::duration_cast( + end - begin) + .count()) / + fact + << str << std::endl; + ; + } + } +}; + +struct ScopedTimerEvents { + enum SCALE { NSEC, MSEC, SEC }; + SCALE sc; + std::vector vtimes; + std::vector names; + std::string _name; + + ScopedTimerEvents(std::string name = "", bool start = true, + SCALE _sc = MSEC) { + if (start) add("start"); + sc = _sc; + _name = name; + } + + void add(std::string name) { + vtimes.push_back(std::chrono::high_resolution_clock::now()); + names.push_back(name); + } + void addspaces(std::vector &str) { + // get max size + size_t m = -1; + for (auto &s : str) m = std::max(s.size(), m); + for (auto &s : str) { + while (s.size() < m) s.push_back(' '); + } + } + + ~ScopedTimerEvents() { + double fact = 1; + std::string str; + switch (sc) { + case NSEC: + fact = 1; + str = "ns"; + break; + case MSEC: + fact = 1e6; + str = "ms"; + break; + case SEC: + fact = 1e9; + str = "s"; + break; + }; + + add("total"); + addspaces(names); + for (size_t i = 1; i < vtimes.size(); i++) { + std::cout << "Time(" << _name << ")-" << names[i] << " " + << double(std::chrono::duration_cast( + vtimes[i] - vtimes[i - 1]) + .count()) / + fact + << str << " " + << double(std::chrono::duration_cast( + vtimes[i] - vtimes[0]) + .count()) / + fact + << str << std::endl; + } + } +}; + +struct Timer { + enum SCALE { NSEC, MSEC, SEC }; + + std::chrono::high_resolution_clock::time_point _s; + double sum = 0, n = 0; + std::string _name; + Timer() {} + + Timer(std::string name) : _name(name) {} + void setName(std::string name) { _name = name; } + void start() { _s = std::chrono::high_resolution_clock::now(); } + void end() { + auto e = std::chrono::high_resolution_clock::now(); + sum += double( + std::chrono::duration_cast(e - _s).count()); + n++; + } + + void print(SCALE sc = MSEC) { + double fact = 1; + std::string str; + switch (sc) { + case NSEC: + fact = 1; + str = "ns"; + break; + case MSEC: + fact = 1e6; + str = "ms"; + break; + case SEC: + fact = 1e9; + str = "s"; + break; + }; + std::cout << "Time(" << _name << ")= " << (sum / n) / fact << str + << std::endl; + } +}; +} + +#endif diff --git a/thirdparty/Pangolin b/thirdparty/Pangolin new file mode 160000 index 0000000..ad8b5f8 --- /dev/null +++ b/thirdparty/Pangolin @@ -0,0 +1 @@ +Subproject commit ad8b5f83222291c51b4800d5a5873b0e90a0cf81 diff --git a/thirdparty/apriltag/CMakeLists.txt b/thirdparty/apriltag/CMakeLists.txt new file mode 100644 index 0000000..0c1a28e --- /dev/null +++ b/thirdparty/apriltag/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.2) + + +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") + +#file(GLOB APRILTAG_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/apriltag-2016-12-01/*.c" "${CMAKE_CURRENT_SOURCE_DIR}/apriltag-2016-12-01/common/*.c") +#include_directories(apriltag-2016-12-01) + +include_directories(${OpenCV_INCLUDE_DIR}) + +file(GLOB APRILTAG_SRCS "ethz_apriltag2/src/*.cc") +include_directories(ethz_apriltag2/include) + + +include_directories(../../include) +include_directories(../basalt-headers/include) +include_directories(../basalt-headers/thirdparty/Sophus) + +add_library(apriltag STATIC ${APRILTAG_SRCS} src/apriltag.cpp) + +target_include_directories(apriltag PUBLIC include) +target_link_libraries(apriltag PUBLIC ${OpenCV_LIBS} pangolin) + + diff --git a/thirdparty/apriltag/ethz_apriltag2/CMakeLists.txt b/thirdparty/apriltag/ethz_apriltag2/CMakeLists.txt new file mode 100644 index 0000000..58f985c --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8) + +project(ethz_apriltag2) + +find_package(catkin REQUIRED COMPONENTS cmake_modules) +include_directories(${catkin_INCLUDE_DIRS}) + +find_package(Eigen REQUIRED) + +catkin_package( + DEPENDS eigen opencv + INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} + LIBRARIES ${PROJECT_NAME} +) + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake/) + +find_package(Eigen REQUIRED) +find_package(OpenCV REQUIRED) + +add_definitions(-fPIC -O3) +include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +#library +file(GLOB SOURCE_FILES "src/*.cc") +add_library(${PROJECT_NAME} ${SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${catkin_LIBRARIES}) + +#demo +if(NOT APPLE) + add_executable(apriltags_demo src/example/apriltags_demo.cpp src/example/Serial.cpp) + target_link_libraries(apriltags_demo ${PROJECT_NAME} v4l2) +endif() + + diff --git a/thirdparty/apriltag/ethz_apriltag2/LICENSE b/thirdparty/apriltag/ethz_apriltag2/LICENSE new file mode 100644 index 0000000..ff17f84 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/LICENSE @@ -0,0 +1,34 @@ + +Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, + ETH Zurich, Switzerland +Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland +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. + + All advertising materials mentioning features or use of this software must + display the following acknowledgement: This product includes software developed + by the Autonomous Systems Lab and Skybotix AG. + + Neither the name of the Autonomous Systems Lab and Skybotix AG 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 AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''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 the AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG 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. diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Edge.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Edge.h new file mode 100644 index 0000000..34fff13 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Edge.h @@ -0,0 +1,59 @@ +#ifndef EDGE_H +#define EDGE_H + +#include + +#include "apriltags/FloatImage.h" + +namespace AprilTags { + +class FloatImage; +class UnionFindSimple; + +using std::min; +using std::max; + +//! Represents an edge between adjacent pixels in the image. +/*! The edge is encoded by the indices of the two pixels. Edge cost + * is proportional to the difference in local orientations. + */ +class Edge { +public: + static float const minMag; //!< minimum intensity gradient for an edge to be recognized + static float const maxEdgeCost; //!< 30 degrees = maximum acceptable difference in local orientations + static int const WEIGHT_SCALE; // was 10000 + static float const thetaThresh; //!< theta threshold for merging edges + static float const magThresh; //!< magnitude threshold for merging edges + + int pixelIdxA; + int pixelIdxB; + int cost; + + //! Constructor + Edge() : pixelIdxA(), pixelIdxB(), cost() {} + + //! Compare edges based on cost + inline bool operator< (const Edge &other) const { return (cost < other.cost); } + + //! Cost of an edge between two adjacent pixels; -1 if no edge here + /*! An edge exists between adjacent pixels if the magnitude of the + intensity gradient at both pixels is above threshold. The edge + cost is proportional to the difference in the local orientation at + the two pixels. Lower cost is better. A cost of -1 means there + is no edge here (intensity gradien fell below threshold). + */ + static int edgeCost(float theta0, float theta1, float mag1); + + //! Calculates and inserts up to four edges into 'edges', a vector of Edges. + static void calcEdges(float theta0, int x, int y, + const FloatImage& theta, const FloatImage& mag, + std::vector &edges, size_t &nEdges); + + //! Process edges in order of increasing cost, merging clusters if we can do so without exceeding the thetaThresh. + static void mergeEdges(std::vector &edges, UnionFindSimple &uf, float tmin[], float tmax[], float mmin[], float mmax[]); + +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/FloatImage.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/FloatImage.h new file mode 100644 index 0000000..e630ff5 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/FloatImage.h @@ -0,0 +1,61 @@ +#ifndef FLOATIMAGE_H +#define FLOATIMAGE_H + +#include +#include + +namespace DualCoding { + typedef unsigned char uchar; + template class Sketch; +} + +namespace AprilTags { + +//! Represent an image as a vector of floats in [0,1] +class FloatImage { +private: + int width; + int height; + std::vector pixels; + +public: + + //! Default constructor + FloatImage(); + + //! Construct an empty image + FloatImage(int widthArg, int heightArg); + + //! Constructor that copies pixels from an array + FloatImage(int widthArg, int heightArg, const std::vector& pArg); + + FloatImage& operator=(const FloatImage& other); + + float get(int x, int y) const { return pixels[y*width + x]; } + void set(int x, int y, float v) { pixels[y*width + x] = v; } + + int getWidth() const { return width; } + int getHeight() const { return height; } + int getNumFloatImagePixels() const { return width*height; } + const std::vector& getFloatImagePixels() const { return pixels; } + + //! TODO: Fix decimateAvg function. DO NOT USE! + void decimateAvg(); + + //! Rescale all values so that they are between [0,1] + void normalize(); + + void filterFactoredCentered(const std::vector& fhoriz, const std::vector& fvert); + + template + void copyToSketch(DualCoding::Sketch& sketch) { + for (int i = 0; i < getNumFloatImagePixels(); i++) + sketch[i] = (T)(255.0f * getFloatImagePixels()[i]); + } + + void printMinMax() const; +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLine2D.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLine2D.h new file mode 100644 index 0000000..46d8390 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLine2D.h @@ -0,0 +1,74 @@ +#ifndef GLINE2D_H +#define GLINE2D_H + +#include +#include +#include + +#include "apriltags/MathUtil.h" +#include "apriltags/XYWeight.h" + +namespace AprilTags { + +//! A 2D line +class GLine2D { +public: + + //! Create a new line. + GLine2D(); + + //! Create a new line. + /* @param slope the slope + * @param b the y intercept + */ + GLine2D(float slope, float b); + + //! Create a new line. + /* @param dx A change in X corresponding to dy + * @param dy A change in Y corresponding to dx + * @param p A point that the line passes through + */ + GLine2D(float dX, float dY, const std::pair& pt); + + //! Create a new line through two points. + /* @param p1 the first point + * @param p2 the second point + */ + GLine2D(const std::pair& p1, const std::pair& p2); + + //! Get the coordinate of a point (on this line), with zero corresponding to the point + //! on the that is perpendicular toa line passing through the origin and the line. + /* This allows easy computation if one point is between two other points on the line: + * compute the line coordinate of all three points and test if a<=b<=c. This is + * implemented by computing the dot product of the vector 'p' with the + * line's direct unit vector. + */ + float getLineCoordinate(const std::pair& p); + + //! The inverse of getLineCoordinate. + std::pair getPointOfCoordinate(float coord); + + //!Compute the point where two lines intersect, or (-1,0) if the lines are parallel. + std::pair intersectionWith(const GLine2D& line) const; + + static GLine2D lsqFitXYW(const std::vector& xyweights); + + inline float getDx() const { return dx; } + inline float getDy() const { return dy; } + inline float getFirst() const { return p.first; } + inline float getSecond() const { return p.second; } + +protected: + void normalizeSlope(); + void normalizeP(); + +private: + float dx, dy; + std::pair p; //!< A point the line passes through; when normalized, it is the point closest to the origin (hence perpendicular to the line) + bool didNormalizeSlope; + bool didNormalizeP; +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLineSegment2D.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLineSegment2D.h new file mode 100644 index 0000000..8b55a10 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLineSegment2D.h @@ -0,0 +1,31 @@ +#ifndef GLINESEGMENT2D_H +#define GLINESEGMENT2D_H + +#include +#include +#include +#include + +#include "apriltags/GLine2D.h" +#include "apriltags/XYWeight.h" + +namespace AprilTags { + +//! A 2D line with endpoints. +class GLineSegment2D { +public: + GLineSegment2D(const std::pair &p0Arg, const std::pair &p1Arg); + static GLineSegment2D lsqFitXYW(const std::vector& xyweight); + std::pair getP0() const { return p0; } + std::pair getP1() const { return p1; } + +private: + GLine2D line; + std::pair p0; + std::pair p1; + int weight; +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gaussian.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gaussian.h new file mode 100644 index 0000000..8d8d46f --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gaussian.h @@ -0,0 +1,37 @@ +#ifndef GAUSSIAN_H +#define GAUSSIAN_H + +#include +#include + +namespace AprilTags { + +class Gaussian { + +public: + static bool warned; + + //! Returns a Gaussian filter of size n. + /*! @param sigma standard deviation of the Gaussian + * @param n length of the Gaussian (must be odd) + */ + static std::vector makeGaussianFilter(float sigma, int n); + + //! Convolve the input 'a' (which begins at offset aoff and is alen elements in length) with the filter 'f'. + /*! The result is deposited in 'r' at offset 'roff'. f.size() should be odd. + * The output is shifted by -f.size()/2, so that there is no net time delay. + * @param a input vector of pixels + * @param aoff + * @param alen + * @param f + * @param r the resultant array of pixels + * @param roff + */ + static void convolveSymmetricCentered(const std::vector& a, unsigned int aoff, unsigned int alen, + const std::vector& f, std::vector& r, unsigned int roff); + +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GrayModel.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GrayModel.h new file mode 100644 index 0000000..baade0b --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/GrayModel.h @@ -0,0 +1,46 @@ +//-*-c++-*- + +#ifndef GRAYMODEL_H +#define GRAYMODEL_H + +#include +#include + +namespace AprilTags { + +//! Fits a grayscale model over an area of pixels. +/*! The model is of the form: c1*x + c2*y + c3*x*y + c4 = value + * + * We use this model to compute spatially-varying thresholds for + * reading bits. + */ +class GrayModel { +public: + GrayModel(); + + void addObservation(float x, float y, float gray); + + inline int getNumObservations() { return nobs; } + + float interpolate(float x, float y); + +private: + void compute(); + +// We're solving Av = b. +// +// For each observation, we add a row to A of the form [x y xy 1] +// and to b of the form gray*[x y xy 1]. v is the vector [c1 c2 c3 c4]. +// +// The least-squares solution to the system is v = inv(A'A)A'b + + Eigen::Matrix4d A; + Eigen::Vector4d v; + Eigen::Vector4d b; + int nobs; + bool dirty; //!< True if we've added an observation and need to recompute v +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gridder.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gridder.h new file mode 100644 index 0000000..3798d0e --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gridder.h @@ -0,0 +1,180 @@ +#ifndef GRIDDER_H +#define GRIDDER_H + +#include +#include +#include + +#include "apriltags/Segment.h" + +namespace AprilTags { + +//! A lookup table in 2D for implementing nearest neighbor. +template +class Gridder { + private: + Gridder(const Gridder&); //!< don't call + Gridder& operator=(const Gridder&); //!< don't call + + struct Cell { + T* object; + Cell *next; + + Cell() : object(NULL), next(NULL) {} + + Cell(const Cell& c) : object(c.object), next(c.next) {} + + // Destructor + ~Cell() { + delete next; + } + + Cell& operator=(const Cell &other) { + if (this == &other) + return *this; + + object = other.object; + next = other.next; + return *this; + } + }; + + //! Initializes Gridder constructor + void gridderInit(float x0Arg, float y0Arg, float x1Arg, float y1Arg, float ppCell) { + width = (int) ((x1Arg - x0Arg)/ppCell + 1); + height = (int) ((y1Arg - y0Arg)/ppCell + 1); + + x1 = x0Arg + ppCell*width; + y1 = y0Arg + ppCell*height; + cells = std::vector< std::vector >(height, std::vector(width,(Cell*)NULL)); + } + + float x0,y0,x1,y1; + int width, height; + float pixelsPerCell; //pixels per cell + std::vector< std::vector > cells; + +public: + Gridder(float x0Arg, float y0Arg, float x1Arg, float y1Arg, float ppCell) + : x0(x0Arg), y0(y0Arg), x1(), y1(), width(), height(), pixelsPerCell(ppCell), + cells() { gridderInit(x0Arg, y0Arg, x1Arg, y1Arg, ppCell); } + + // Destructor + ~Gridder() { + for (unsigned int i = 0; i < cells.size(); i++) { + for (unsigned int j = 0; j < cells[i].size(); j++) + delete cells[i][j]; + } + } + + void add(float x, float y, T* object) { + int ix = (int) ((x - x0)/pixelsPerCell); + int iy = (int) ((y - y0)/pixelsPerCell); + + if (ix>=0 && iy>=0 && ixobject = object; + c->next = cells[iy][ix]; + cells[iy][ix] = c; + // cout << "Gridder placed seg " << o->getId() << " at (" << ix << "," << iy << ")" << endl; + } + } + + // iterator begin(); + // iterator end(); + + //! Iterator for Segment class. + class Iterator { + public: + Iterator(Gridder* grid, float x, float y, float range) + : outer(grid), ix0(), ix1(), iy0(), iy1(), ix(), iy(), c(NULL) { iteratorInit(x,y,range); } + + Iterator(const Iterator& it) + : outer(it.outer), ix0(it.ix0), ix1(it.ix1), iy0(it.iy0), iy1(it.iy1), ix(it.ix), iy(it.iy), c(it.c) {} + + Iterator& operator=(const Iterator& it) { + outer = it.outer; + ix0 = it.ix0; + ix1 = it.ix1; + iy0 = it.iy0; + iy1 = it.iy1; + ix = it.ix; + iy = it.iy; + c = it.c; + } + + bool hasNext() { + if (c == NULL) + findNext(); + return (c != NULL); + } + + T& next() { + T* thisObj = c->object; + findNext(); + return *thisObj; // return Segment + } + + private: + void findNext() { + if (c != NULL) + c = c->next; + if (c != NULL) + return; + + ix++; + while (true) { + if (ix > ix1) { + iy++; + ix = ix0; + } + if (iy > iy1) + break; + + c = outer->cells[iy][ix]; + + if (c != NULL) + break; + ix++; + } + } + + //! Initializes Iterator constructor + void iteratorInit(float x, float y, float range) { + ix0 = (int) ((x - range - outer->x0)/outer->pixelsPerCell); + iy0 = (int) ((y - range - outer->y0)/outer->pixelsPerCell); + + ix1 = (int) ((x + range - outer->x0)/outer->pixelsPerCell); + iy1 = (int) ((y + range - outer->y0)/outer->pixelsPerCell); + + ix0 = std::max(0, ix0); + ix0 = std::min(outer->width-1, ix0); + + ix1 = std::max(0, ix1); + ix1 = std::min(outer->width-1, ix1); + + iy0 = std::max(0, iy0); + iy0 = std::min(outer->height-1, iy0); + + iy1 = std::max(0, iy1); + iy1 = std::min(outer->height-1, iy1); + + ix = ix0; + iy = iy0; + + c = outer->cells[iy][ix]; + } + + Gridder* outer; + int ix0, ix1, iy0, iy1; + int ix, iy; + Cell *c; + }; + + typedef Iterator iterator; + iterator find(float x, float y, float range) { return Iterator(this,x,y,range); } +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h new file mode 100644 index 0000000..055f74d --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h @@ -0,0 +1,77 @@ +//-*-c++-*- + +#ifndef HOMOGRAPHY33_H +#define HOMOGRAPHY33_H + +#include +#include + +#include + +// NOTE: In Basalt we use homography, since for fisheye-distorted +// images, interpolation is not good enough, resulting in a lot less +// valid detections. At the same time we don't case about speed too +// much. + +// interpolate points instead of using homography +//#define INTERPOLATE +// use stable version of homography recover (opencv, includes refinement step) +#define STABLE_H + +//! Compute 3x3 homography using Direct Linear Transform +/* + * + * DEPRECATED - DEPRECATED - DEPRECATED - DEPRECATED + * + * use TagDetection::getRelativeTransform() instead + * + * + * y = Hx (y = image coordinates in homogeneous coordinates, H = 3x3 + * homography matrix, x = homogeneous 2D world coordinates) + * + * For each point correspondence, constrain y x Hx = 0 (where x is + * cross product). This means that they have the same direction, and + * ignores scale factor. + * + * We rewrite this as Ah = 0, where h is the 9x1 column vector of the + * elements of H. Each point correspondence gives us 3 equations of + * rank 2. The solution h is the minimum right eigenvector of A, + * which we can obtain via SVD, USV' = A. h is the right-most column + * of V'. + * + * We will actually maintain A'A internally, which is 9x9 regardless + * of how many correspondences we're given, and has the same + * eigenvectors as A. + */ +class Homography33 { +public: + //! Constructor + Homography33(const std::pair &opticalCenter); + +#ifdef STABLE_H + void setCorrespondences(const std::vector< std::pair > &srcPts, + const std::vector< std::pair > &dstPts); +#else + void addCorrespondence(float worldx, float worldy, float imagex, float imagey); +#endif + + //! Note that the returned H matrix does not reflect cxy. + Eigen::Matrix3d& getH(); + + const std::pair getCXY() const { return cxy; } + + void compute(); + + std::pair project(float worldx, float worldy); + +private: + std::pair cxy; + Eigen::Matrix fA; + Eigen::Matrix3d H; + bool valid; +#ifdef STABLE_H + std::vector< std::pair > srcPts, dstPts; +#endif +}; + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/MathUtil.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/MathUtil.h new file mode 100644 index 0000000..4779dad --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/MathUtil.h @@ -0,0 +1,69 @@ +//-*-c++-*- + +#ifndef MATHUTIL_H +#define MATHUTIL_H + +#include +#include +#include +#include +#include + +namespace AprilTags { + +std::ostream& operator<<(std::ostream &os, const std::pair &pt); + +//! Miscellaneous math utilities and fast exp functions. +class MathUtil { +public: + + //! Returns the square of a value. + static inline float square(float x) { return x*x; } + + static inline float distance2D(const std::pair &p0, const std::pair &p1) { + float dx = p0.first - p1.first; + float dy = p0.second - p1.second; + return std::sqrt(dx*dx + dy*dy); + } + + //! Returns a result in [-Pi, Pi] + static inline float mod2pi(float vin) { + const float twopi = 2 * (float)M_PI; + const float twopi_inv = 1.f / (2.f * (float)M_PI); + float absv = std::abs(vin); + float q = absv*twopi_inv + 0.5f; + int qi = (int) q; + float r = absv - qi*twopi; + return (vin<0) ? -r : r; + } + + //! Returns a value of v wrapped such that ref and v differ by no more than +/- Pi + static inline float mod2pi(float ref, float v) { return ref + mod2pi(v-ref); } + +// lousy approximation of arctan function, but good enough for our purposes (about 4 degrees) + static inline double fast_atan2(double y, double x) { + double coeff_1 = M_PI/4; + double coeff_2 = 3*coeff_1; + double abs_y = fabs(y)+1e-10; // kludge to prevent 0/0 condition + + double angle; + + if (x >= 0) { + double r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + double r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + + if (y < 0) + return -angle; // negate if in quad III or IV + else + return angle; + } + +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Quad.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Quad.h new file mode 100644 index 0000000..ca5c10a --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Quad.h @@ -0,0 +1,72 @@ +#ifndef QUAD_H +#define QUAD_H + +#include +#include + +#include + +#include "apriltags/Homography33.h" + +namespace AprilTags { + +class FloatImage; +class Segment; + +using std::min; +using std::max; + +//! Represents four segments that form a loop, and might be a tag. +class Quad { +public: + static const int minimumEdgeLength = 6; //!< Minimum size of a tag (in pixels) as measured along edges and diagonals + static float const maxQuadAspectRatio; //!< Early pruning of quads with insane ratios. + + //! Constructor + /*! (x,y) are the optical center of the camera, which is + * needed to correctly compute the homography. */ + Quad(const std::vector< std::pair >& p, const std::pair& opticalCenter); + + //! Interpolate given that the lower left corner of the lower left cell is at (-1,-1) and the upper right corner of the upper right cell is at (1,1). + std::pair interpolate(float x, float y); + + //! Same as interpolate, except that the coordinates are interpreted between 0 and 1, instead of -1 and 1. + std::pair interpolate01(float x, float y); + + //! Points for the quad (in pixel coordinates), in counter clockwise order. These points are the intersections of segments. + std::vector< std::pair > quadPoints; + + //! Segments composing this quad + std::vector segments; + + //! Total length (in pixels) of the actual perimeter observed for the quad. + /*! This is in contrast to the geometric perimeter, some of which + * may not have been directly observed but rather inferred by + * intersecting segments. Quads with more observed perimeter are + * preferred over others. */ + float observedPerimeter; + + //! Given that the whole quad spans from (0,0) to (1,1) in "quad space", compute the pixel coordinates for a given point within that quad. + /*! Note that for most of the Quad's existence, we will not know the correct orientation of the tag. */ + Homography33 homography; + + //! Searches through a vector of Segments to form Quads. + /* @param quads any discovered quads will be added to this list + * @param path the segments currently part of the search + * @param parent the first segment in the quad + * @param depth how deep in the search are we? + */ + static void search(const FloatImage& fImage, std::vector& path, + Segment& parent, int depth, std::vector& quads, + const std::pair& opticalCenter); + +#ifdef INTERPOLATE + private: + Eigen::Vector2f p0, p3, p01, p32; +#endif + +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Segment.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Segment.h new file mode 100644 index 0000000..ac76560 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Segment.h @@ -0,0 +1,56 @@ +#ifndef SEGMENT_H +#define SEGMENT_H + +#include +#include + +namespace AprilTags { + +//! Represents a line fit to a set of pixels whose gradients are similiar. +class Segment { +public: + Segment(); + + static int const minimumSegmentSize = 4; //!< Minimum number of pixels in a segment before we'll fit a line to it. + static float const minimumLineLength; //!< In pixels. Calculated based on minimum plausible decoding size for Tag9 family. + + float getX0() const { return x0; } + void setX0(float newValue) { x0 = newValue; } + + float getY0() const { return y0; } + void setY0(float newValue) { y0 = newValue; } + + float getX1() const { return x1; } + void setX1(float newValue) { x1 = newValue; } + + float getY1() const { return y1; } + void setY1(float newValue) { y1 = newValue; } + + float getTheta() const { return theta; } + void setTheta(float newValue) { theta = newValue; } + + float getLength() const { return length; } + void setLength(float newValue) { length = newValue; } + + //! Returns the length of the Segment. + float segmentLength(); + + //! Print endpoint coordinates of this segment. + void printSegment(); + + //! ID of Segment. + int getId() const { return segmentId; } + + std::vector children; + +private: + float x0, y0, x1, y1; + float theta; // gradient direction (points towards white) + float length; // length of line segment in pixels + int segmentId; + static int idCounter; +}; + +} // namsepace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5.h new file mode 100644 index 0000000..ea7f947 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5.h @@ -0,0 +1,41 @@ +/** Tag family with 30 distinct codes. + bits: 16, minimum hamming: 5, minimum complexity: 5 + + Max bits corrected False positive rate + 0 0.045776 % + 1 0.778198 % + 2 6.271362 % + + Generation time: 0.309000 s + + Hamming distance between pairs of codes (accounting for rotation): + + 0 0 + 1 0 + 2 0 + 3 0 + 4 0 + 5 120 + 6 172 + 7 91 + 8 33 + 9 13 + 10 6 + 11 0 + 12 0 + 13 0 + 14 0 + 15 0 + 16 0 +**/ + +#pragma once + +namespace AprilTags { + +const unsigned long long t16h5[] = + { 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x79a6LL, 0x7f6bLL, 0xb358LL, 0xe745LL, 0xfe59LL, 0x156dLL, 0x380bLL, 0xf0abLL, 0x0d84LL, 0x4736LL, 0x8c72LL, 0xaf10LL, 0x093cLL, 0x93b4LL, 0xa503LL, 0x468fLL, 0xe137LL, 0x5795LL, 0xdf42LL, 0x1c1dLL, 0xe9dcLL, 0x73adLL, 0xad5fLL, 0xd530LL, 0x07caLL, 0xaf2eLL }; + +static const TagCodes tagCodes16h5 = TagCodes(16, 5, t16h5, sizeof(t16h5)/sizeof(t16h5[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5_other.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5_other.h new file mode 100644 index 0000000..7657c0c --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5_other.h @@ -0,0 +1,17 @@ +// These codes were generated by TagFamilyGenerator.java from Ed Olson + +// tag16h5Codes : 16 bits, minimum Hamming distance 5, minimum complexity 5 + +#pragma once + +namespace AprilTags { + +const unsigned long long t16h5_other[] = + { 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x6857LL, 0x7f6bLL, 0xad93LL, 0xb358LL, + 0xb91dLL, 0xe745LL, 0x156dLL, 0xd3d2LL, 0xdf5cLL, 0x4736LL, 0x8c72LL, 0x5a02LL, + 0xd32bLL, 0x1867LL, 0x468fLL, 0xdc91LL, 0x4940LL, 0xa9edLL, 0x2bd5LL, 0x599aLL, + 0x9009LL, 0x61f6LL, 0x3850LL, 0x8157LL, 0xbfcaLL, 0x987cLL}; + +static const TagCodes tagCodes16h5_other = TagCodes(16, 5, t16h5_other, sizeof(t16h5_other)/sizeof(t16h5_other[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h7.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h7.h new file mode 100644 index 0000000..d476133 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h7.h @@ -0,0 +1,51 @@ +/** Tag family with 242 distinct codes. + bits: 25, minimum hamming: 7, minimum complexity: 8 + + Max bits corrected False positive rate + 0 0.000721 % + 1 0.018752 % + 2 0.235116 % + 3 1.893914 % + + Generation time: 72.585000 s + + Hamming distance between pairs of codes (accounting for rotation): + + 0 0 + 1 0 + 2 0 + 3 0 + 4 0 + 5 0 + 6 0 + 7 2076 + 8 4161 + 9 5299 + 10 6342 + 11 5526 + 12 3503 + 13 1622 + 14 499 + 15 114 + 16 16 + 17 3 + 18 0 + 19 0 + 20 0 + 21 0 + 22 0 + 23 0 + 24 0 + 25 0 +**/ + +#pragma once + +namespace AprilTags { + +const unsigned long long t25h7[] = + { 0x4b770dLL, 0x11693e6LL, 0x1a599abLL, 0xc3a535LL, 0x152aafaLL, 0xaccd98LL, 0x1cad922LL, 0x2c2fadLL, 0xbb3572LL, 0x14a3b37LL, 0x186524bLL, 0xc99d4cLL, 0x23bfeaLL, 0x141cb74LL, 0x1d0d139LL, 0x1670aebLL, 0x851675LL, 0x150334eLL, 0x6e3ed8LL, 0xfd449dLL, 0xaa55ecLL, 0x1c86176LL, 0x15e9b28LL, 0x7ca6b2LL, 0x147c38bLL, 0x1d6c950LL, 0x8b0e8cLL, 0x11a1451LL, 0x1562b65LL, 0x13f53c8LL, 0xd58d7aLL, 0x829ec9LL, 0xfaccf1LL, 0x136e405LL, 0x7a2f06LL, 0x10934cbLL, 0x16a8b56LL, 0x1a6a26aLL, 0xf85545LL, 0x195c2e4LL, 0x24c8a9LL, 0x12bfc96LL, 0x16813aaLL, 0x1a42abeLL, 0x1573424LL, 0x1044573LL, 0xb156c2LL, 0x5e6811LL, 0x1659bfeLL, 0x1d55a63LL, 0x5bf065LL, 0xe28667LL, 0x1e9ba54LL, 0x17d7c5aLL, 0x1f5aa82LL, 0x1a2bbd1LL, 0x1ae9f9LL, 0x1259e51LL, 0x134062bLL, 0xe1177aLL, 0xed07a8LL, 0x162be24LL, 0x59128bLL, 0x1663e8fLL, 0x1a83cbLL, 0x45bb59LL, 0x189065aLL, 0x4bb370LL, 0x16fb711LL, 0x122c077LL, 0xeca17aLL, 0xdbc1f4LL, 0x88d343LL, 0x58ac5dLL, 0xba02e8LL, 0x1a1d9dLL, 0x1c72eecLL, 0x924bc5LL, 0xdccab3LL, 0x886d15LL, 0x178c965LL, 0x5bc69aLL, 0x1716261LL, 0x174e2ccLL, 0x1ed10f4LL, 0x156aa8LL, 0x3e2a8aLL, 0x2752edLL, 0x153c651LL, 0x1741670LL, 0x765b05LL, 0x119c0bbLL, 0x172a783LL, 0x4faca1LL, 0xf31257LL, 0x12441fcLL, 0x0d3748LL, 0xc21f15LL, 0xac5037LL, 0x180e592LL, 0x7d3210LL, 0xa27187LL, 0x2beeafLL, 0x26ff57LL, 0x690e82LL, 0x77765cLL, 0x1a9e1d7LL, 0x140be1aLL, 0x1aa1e3aLL, 0x1944f5cLL, 0x19b5032LL, 0x169897LL, 0x1068eb9LL, 0xf30dbcLL, 0x106a151LL, 0x1d53e95LL, 0x1348ceeLL, 0xcf4fcaLL, 0x1728bb5LL, 0xdc1eecLL, 0x69e8dbLL, 0x16e1523LL, 0x105fa25LL, 0x18abb0cLL, 0xc4275dLL, 0x6d8e76LL, 0xe8d6dbLL, 0xe16fd7LL, 0x1ac2682LL, 0x77435bLL, 0xa359ddLL, 0x3a9c4eLL, 0x123919aLL, 0x1e25817LL, 0x02a836LL, 0x1545a4LL, 0x1209c8dLL, 0xbb5f69LL, 0x1dc1f02LL, 0x5d5f7eLL, 0x12d0581LL, 0x13786c2LL, 0xe15409LL, 0x1aa3599LL, 0x139aad8LL, 0xb09d2aLL, 0x54488fLL, 0x13c351cLL, 0x976079LL, 0xb25b12LL, 0x1addb34LL, 0x1cb23aeLL, 0x1175738LL, 0x1303bb8LL, 0xd47716LL, 0x188ceeaLL, 0xbaf967LL, 0x1226d39LL, 0x135e99bLL, 0x34adc5LL, 0x2e384dLL, 0x90d3faLL, 0x232713LL, 0x17d49b1LL, 0xaa84d6LL, 0xc2ddf8LL, 0x1665646LL, 0x4f345fLL, 0x2276b1LL, 0x1255dd7LL, 0x16f4cccLL, 0x4aaffcLL, 0xc46da6LL, 0x85c7b3LL, 0x1311fcbLL, 0x9c6c4fLL, 0x187d947LL, 0x8578e4LL, 0xe2bf0bLL, 0xa01b4cLL, 0xa1493bLL, 0x7ad766LL, 0xccfe82LL, 0x1981b5bLL, 0x1cacc85LL, 0x562cdbLL, 0x15b0e78LL, 0x8f66c5LL, 0x3332bfLL, 0x12ce754LL, 0x096a76LL, 0x1d5e3baLL, 0x27ea41LL, 0x14412dfLL, 0x67b9b4LL, 0xdaa51aLL, 0x1dcb17LL, 0x4d4afdLL, 0x6335d5LL, 0xee2334LL, 0x17d4e55LL, 0x1b8b0f0LL, 0x14999e3LL, 0x1513dfaLL, 0x765cf2LL, 0x56af90LL, 0x12e16acLL, 0x1d3d86cLL, 0xff279bLL, 0x18822ddLL, 0x99d478LL, 0x8dc0d2LL, 0x34b666LL, 0xcf9526LL, 0x186443dLL, 0x7a8e29LL, 0x19c6aa5LL, 0x1f2a27dLL, 0x12b2136LL, 0xd0cd0dLL, 0x12cb320LL, 0x17ddb0bLL, 0x05353bLL, 0x15b2cafLL, 0x1e5a507LL, 0x120f1e5LL, 0x114605aLL, 0x14efe4cLL, 0x568134LL, 0x11b9f92LL, 0x174d2a7LL, 0x692b1dLL, 0x39e4feLL, 0xaaff3dLL, 0x96224cLL, 0x13c9f77LL, 0x110ee8fLL, 0xf17beaLL, 0x99fb5dLL, 0x337141LL, 0x02b54dLL, 0x1233a70LL }; + +static const TagCodes tagCodes25h7 = TagCodes(25, 7, t25h7, sizeof(t25h7)/sizeof(t25h7[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h9.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h9.h new file mode 100644 index 0000000..90185bb --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h9.h @@ -0,0 +1,52 @@ +/** Tag family with 35 distinct codes. + bits: 25, minimum hamming: 9, minimum complexity: 8 + + Max bits corrected False positive rate + 0 0.000104 % + 1 0.002712 % + 2 0.034004 % + 3 0.273913 % + 4 1.593411 % + + Generation time: 31.283000 s + + Hamming distance between pairs of codes (accounting for rotation): + + 0 0 + 1 0 + 2 0 + 3 0 + 4 0 + 5 0 + 6 0 + 7 0 + 8 0 + 9 156 + 10 214 + 11 120 + 12 64 + 13 29 + 14 11 + 15 1 + 16 0 + 17 0 + 18 0 + 19 0 + 20 0 + 21 0 + 22 0 + 23 0 + 24 0 + 25 0 +**/ + +#pragma once + +namespace AprilTags { + +const unsigned long long t25h9[] = + { 0x155cbf1LL, 0x1e4d1b6LL, 0x17b0b68LL, 0x1eac9cdLL, 0x12e14ceLL, 0x3548bbLL, 0x7757e6LL, 0x1065dabLL, 0x1baa2e7LL, 0xdea688LL, 0x81d927LL, 0x51b241LL, 0xdbc8aeLL, 0x1e50e19LL, 0x15819d2LL, 0x16d8282LL, 0x163e035LL, 0x9d9b81LL, 0x173eec4LL, 0xae3a09LL, 0x5f7c51LL, 0x1a137fcLL, 0xdc9562LL, 0x1802e45LL, 0x1c3542cLL, 0x870fa4LL, 0x914709LL, 0x16684f0LL, 0xc8f2a5LL, 0x833ebbLL, 0x59717fLL, 0x13cd050LL, 0xfa0ad1LL, 0x1b763b0LL, 0xb991ceLL }; + +static const TagCodes tagCodes25h9 = TagCodes(25, 9, t25h9, sizeof(t25h9)/sizeof(t25h9[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11.h new file mode 100644 index 0000000..7c7e896 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11.h @@ -0,0 +1,64 @@ +/** Tag family with 587 distinct codes. + bits: 36, minimum hamming: 11, minimum complexity: 10 + + Max bits corrected False positive rate + 0 0.000001 % + 1 0.000032 % + 2 0.000570 % + 3 0.006669 % + 4 0.056985 % + 5 0.379011 % + + Generation time: 210507.523000 s + + Hamming distance between pairs of codes (accounting for rotation): + + 0 0 + 1 0 + 2 0 + 3 0 + 4 0 + 5 0 + 6 0 + 7 0 + 8 0 + 9 0 + 10 0 + 11 7191 + 12 14401 + 13 20567 + 14 29161 + 15 31975 + 16 29179 + 17 21104 + 18 11447 + 19 4923 + 20 1590 + 21 372 + 22 73 + 23 8 + 24 0 + 25 0 + 26 0 + 27 0 + 28 0 + 29 0 + 30 0 + 31 0 + 32 0 + 33 0 + 34 0 + 35 0 + 36 0 +**/ + +#pragma once + +namespace AprilTags { + +const unsigned long long t36h11[] = + { 0xd5d628584LL, 0xd97f18b49LL, 0xdd280910eLL, 0xe479e9c98LL, 0xebcbca822LL, 0xf31dab3acLL, 0x056a5d085LL, 0x10652e1d4LL, 0x22b1dfeadLL, 0x265ad0472LL, 0x34fe91b86LL, 0x3ff962cd5LL, 0x43a25329aLL, 0x474b4385fLL, 0x4e9d243e9LL, 0x5246149aeLL, 0x5997f5538LL, 0x683bb6c4cLL, 0x6be4a7211LL, 0x7e3158eeaLL, 0x81da494afLL, 0x858339a74LL, 0x8cd51a5feLL, 0x9f21cc2d7LL, 0xa2cabc89cLL, 0xadc58d9ebLL, 0xb16e7dfb0LL, 0xb8c05eb3aLL, 0xd25ef139dLL, 0xd607e1962LL, 0xe4aba3076LL, 0x2dde6a3daLL, 0x43d40c678LL, 0x5620be351LL, 0x64c47fa65LL, 0x686d7002aLL, 0x6c16605efLL, 0x6fbf50bb4LL, 0x8d06d39dcLL, 0x9f53856b5LL, 0xadf746dc9LL, 0xbc9b084ddLL, 0xd290aa77bLL, 0xd9e28b305LL, 0xe4dd5c454LL, 0xfad2fe6f2LL, 0x181a8151aLL, 0x26be42c2eLL, 0x2e10237b8LL, 0x405cd5491LL, 0x7742eab1cLL, 0x85e6ac230LL, 0x8d388cdbaLL, 0x9f853ea93LL, 0xc41ea2445LL, 0xcf1973594LL, 0x14a34a333LL, 0x31eacd15bLL, 0x6c79d2dabLL, 0x73cbb3935LL, 0x89c155bd3LL, 0x8d6a46198LL, 0x91133675dLL, 0xa708d89fbLL, 0xae5ab9585LL, 0xb9558a6d4LL, 0xb98743ab2LL, 0xd6cec68daLL, 0x1506bcaefLL, 0x4becd217aLL, 0x4f95c273fLL, 0x658b649ddLL, 0xa76c4b1b7LL, 0xecf621f56LL, 0x1c8a56a57LL, 0x3628e92baLL, 0x53706c0e2LL, 0x5e6b3d231LL, 0x7809cfa94LL, 0xe97eead6fLL, 0x5af40604aLL, 0x7492988adLL, 0xed5994712LL, 0x5eceaf9edLL, 0x7c1632815LL, 0xc1a0095b4LL, 0xe9e25d52bLL, 0x3a6705419LL, 0xa8333012fLL, 0x4ce5704d0LL, 0x508e60a95LL, 0x877476120LL, 0xa864e950dLL, 0xea45cfce7LL, 0x19da047e8LL, 0x24d4d5937LL, 0x6e079cc9bLL, 0x99f2e11d7LL, 0x33aa50429LL, 0x499ff26c7LL, 0x50f1d3251LL, 0x66e7754efLL, 0x96ad633ceLL, 0x9a5653993LL, 0xaca30566cLL, 0xc298a790aLL, 0x8be44b65dLL, 0xdc68f354bLL, 0x16f7f919bLL, 0x4dde0e826LL, 0xd548cbd9fLL, 0xe0439ceeeLL, 0xfd8b1fd16LL, 0x76521bb7bLL, 0xd92375742LL, 0xcab16d40cLL, 0x730c9dd72LL, 0xad9ba39c2LL, 0xb14493f87LL, 0x52b15651fLL, 0x185409cadLL, 0x77ae2c68dLL, 0x94f5af4b5LL, 0x0a13bad55LL, 0x61ea437cdLL, 0xa022399e2LL, 0x203b163d1LL, 0x7bba8f40eLL, 0x95bc9442dLL, 0x41c0b5358LL, 0x8e9c6cc81LL, 0x0eb549670LL, 0x9da3a0b51LL, 0xd832a67a1LL, 0xdcd4350bcLL, 0x4aa05fdd2LL, 0x60c7bb44eLL, 0x4b358b96cLL, 0x067299b45LL, 0xb9c89b5faLL, 0x6975acaeaLL, 0x62b8f7afaLL, 0x33567c3d7LL, 0xbac139950LL, 0xa5927c62aLL, 0x5c916e6a4LL, 0x260ecb7d5LL, 0x29b7bbd9aLL, 0x903205f26LL, 0xae72270a4LL, 0x3d2ec51a7LL, 0x82ea55324LL, 0x11a6f3427LL, 0x1ca1c4576LL, 0xa40c81aefLL, 0xbddccd730LL, 0x0e617561eLL, 0x969317b0fLL, 0x67f781364LL, 0x610912f96LL, 0xb2549fdfcLL, 0x06e5aaa6bLL, 0xb6c475339LL, 0xc56836a4dLL, 0x844e351ebLL, 0x4647f83b4LL, 0x0908a04f5LL, 0x7f51034c9LL, 0xaee537fcaLL, 0x5e92494baLL, 0xd445808f4LL, 0x28d68b563LL, 0x04d25374bLL, 0x2bc065f65LL, 0x96dc3ea0cLL, 0x4b2ade817LL, 0x07c3fd502LL, 0xe768b5cafLL, 0x17605cf6cLL, 0x182741ee4LL, 0x62846097cLL, 0x72b5ebf80LL, 0x263da6e13LL, 0xfa841bcb5LL, 0x7e45e8c69LL, 0x653c81fa0LL, 0x7443b5e70LL, 0x0a5234afdLL, 0x74756f24eLL, 0x157ebf02aLL, 0x82ef46939LL, 0x80d420264LL, 0x2aeed3e98LL, 0xb0a1dd4f8LL, 0xb5436be13LL, 0x7b7b4b13bLL, 0x1ce80d6d3LL, 0x16c08427dLL, 0xee54462ddLL, 0x1f7644cceLL, 0x9c7b5cc92LL, 0xe369138f8LL, 0x5d5a66e91LL, 0x485d62f49LL, 0xe6e819e94LL, 0xb1f340eb5LL, 0x09d198ce2LL, 0xd60717437LL, 0x0196b856cLL, 0xf0a6173a5LL, 0x12c0e1ec6LL, 0x62b82d5cfLL, 0xad154c067LL, 0xce3778832LL, 0x6b0a7b864LL, 0x4c7686694LL, 0x5058ff3ecLL, 0xd5e21ea23LL, 0x9ff4a76eeLL, 0x9dd981019LL, 0x1bad4d30aLL, 0xc601896d1LL, 0x973439b48LL, 0x1ce7431a8LL, 0x57a8021d6LL, 0xf9dba96e6LL, 0x83a2e4e7cLL, 0x8ea585380LL, 0xaf6c0e744LL, 0x875b73babLL, 0xda34ca901LL, 0x2ab9727efLL, 0xd39f21b9aLL, 0x8a10b742fLL, 0x5f8952dbaLL, 0xf8da71ab0LL, 0xc25f9df96LL, 0x06f8a5d94LL, 0xe42e63e1aLL, 0xb78409d1bLL, 0x792229addLL, 0x5acf8c455LL, 0x2fc29a9b0LL, 0xea486237bLL, 0xb0c9685a0LL, 0x1ad748a47LL, 0x03b4712d5LL, 0xf29216d30LL, 0x8dad65e49LL, 0x0a2cf09ddLL, 0x0b5f174c6LL, 0xe54f57743LL, 0xb9cf54d78LL, 0x4a312a88aLL, 0x27babc962LL, 0xb86897111LL, 0xf2ff6c116LL, 0x82274bd8aLL, 0x97023505eLL, 0x52d46edd1LL, 0x585c1f538LL, 0xbddd00e43LL, 0x5590b74dfLL, 0x729404a1fLL, 0x65320855eLL, 0xd3d4b6956LL, 0x7ae374f14LL, 0x2d7a60e06LL, 0x315cd9b5eLL, 0xfd36b4eacLL, 0xf1df7642bLL, 0x55db27726LL, 0x8f15ebc19LL, 0x992f8c531LL, 0x62dea2a40LL, 0x928275cabLL, 0x69c263cb9LL, 0xa774cca9eLL, 0x266b2110eLL, 0x1b14acbb8LL, 0x624b8a71bLL, 0x1c539406bLL, 0x3086d529bLL, 0x0111dd66eLL, 0x98cd630bfLL, 0x8b9d1ffdcLL, 0x72b2f61e7LL, 0x9ed9d672bLL, 0x96cdd15f3LL, 0x6366c2504LL, 0x6ca9df73aLL, 0xa066d60f0LL, 0xe7a4b8addLL, 0x8264647efLL, 0xaa195bf81LL, 0x9a3db8244LL, 0x014d2df6aLL, 0x0b63265b7LL, 0x2f010de73LL, 0x97e774986LL, 0x248affc29LL, 0xfb57dcd11LL, 0x0b1a7e4d9LL, 0x4bfa2d07dLL, 0x54e5cdf96LL, 0x4c15c1c86LL, 0xcd9c61166LL, 0x499380b2aLL, 0x540308d09LL, 0x8b63fe66fLL, 0xc81aeb35eLL, 0x86fe0bd5cLL, 0xce2480c2aLL, 0x1ab29ee60LL, 0x8048daa15LL, 0xdbfeb2d39LL, 0x567c9858cLL, 0x2b6edc5bcLL, 0x2078fca82LL, 0xadacc22aaLL, 0xb92486f49LL, 0x51fac5964LL, 0x691ee6420LL, 0xf63b3e129LL, 0x39be7e572LL, 0xda2ce6c74LL, 0x20cf17a5cLL, 0xee55f9b6eLL, 0xfb8572726LL, 0xb2c2de548LL, 0xcaa9bce92LL, 0xae9182db3LL, 0x74b6e5bd1LL, 0x137b252afLL, 0x51f686881LL, 0xd672f6c02LL, 0x654146ce4LL, 0xf944bc825LL, 0xe8327f809LL, 0x76a73fd59LL, 0xf79da4cb4LL, 0x956f8099bLL, 0x7b5f2655cLL, 0xd06b114a6LL, 0xd0697ca50LL, 0x27c390797LL, 0xbc61ed9b2LL, 0xcc12dd19bLL, 0xeb7818d2cLL, 0x092fcecdaLL, 0x89ded4ea1LL, 0x256a0ba34LL, 0xb6948e627LL, 0x1ef6b1054LL, 0x8639294a2LL, 0xeda3780a4LL, 0x39ee2af1dLL, 0xcd257edc5LL, 0x2d9d6bc22LL, 0x121d3b47dLL, 0x37e23f8adLL, 0x119f31cf6LL, 0x2c97f4f09LL, 0xd502abfe0LL, 0x10bc3ca77LL, 0x53d7190efLL, 0x90c3e62a6LL, 0x7e9ebf675LL, 0x979ce23d1LL, 0x27f0c98e9LL, 0xeafb4ae59LL, 0x7ca7fe2bdLL, 0x1490ca8f6LL, 0x9123387baLL, 0xb3bc73888LL, 0x3ea87e325LL, 0x4888964aaLL, 0xa0188a6b9LL, 0xcd383c666LL, 0x40029a3fdLL, 0xe1c00ac5cLL, 0x39e6f2b6eLL, 0xde664f622LL, 0xe979a75e8LL, 0x7c6b4c86cLL, 0xfd492e071LL, 0x8fbb35118LL, 0x40b4a09b7LL, 0xaf80bd6daLL, 0x70e0b2521LL, 0x2f5c54d93LL, 0x3f4a118d5LL, 0x09c1897b9LL, 0x079776eacLL, 0x084b00b17LL, 0x3a95ad90eLL, 0x28c544095LL, 0x39d457c05LL, 0x7a3791a78LL, 0xbb770e22eLL, 0x9a822bd6cLL, 0x68a4b1fedLL, 0xa5fd27b3bLL, 0x0c3995b79LL, 0xd1519dff1LL, 0x8e7eee359LL, 0xcd3ca50b1LL, 0xb73b8b793LL, 0x57aca1c43LL, 0xec2655277LL, 0x785a2c1b3LL, 0x75a07985aLL, 0xa4b01eb69LL, 0xa18a11347LL, 0xdb1f28ca3LL, 0x877ec3e25LL, 0x31f6341b8LL, 0x1363a3a4cLL, 0x075d8b9baLL, 0x7ae0792a9LL, 0xa83a21651LL, 0x7f08f9fb5LL, 0x0d0cf73a9LL, 0xb04dcc98eLL, 0xf65c7b0f8LL, 0x65ddaf69aLL, 0x2cf9b86b3LL, 0x14cb51e25LL, 0xf48027b5bLL, 0x0ec26ea8bLL, 0x44bafd45cLL, 0xb12c7c0c4LL, 0x959fd9d82LL, 0xc77c9725aLL, 0x48a22d462LL, 0x8398e8072LL, 0xec89b05ceLL, 0xbb682d4c9LL, 0xe5a86d2ffLL, 0x358f01134LL, 0x8556ddcf6LL, 0x67584b6e2LL, 0x11609439fLL, 0x08488816eLL, 0xaaf1a2c46LL, 0xf879898cfLL, 0x8bbe5e2f7LL, 0x101eee363LL, 0x690f69377LL, 0xf5bd93cd9LL, 0xcea4c2bf6LL, 0x9550be706LL, 0x2c5b38a60LL, 0xe72033547LL, 0x4458b0629LL, 0xee8d9ed41LL, 0xd2f918d72LL, 0x78dc39fd3LL, 0x8212636f6LL, 0x7450a72a7LL, 0xc4f0cf4c6LL, 0x367bcddcdLL, 0xc1caf8cc6LL, 0xa7f5b853dLL, 0x9d536818bLL, 0x535e021b0LL, 0xa7eb8729eLL, 0x422a67b49LL, 0x929e928a6LL, 0x48e8aefccLL, 0xa9897393cLL, 0x5eb81d37eLL, 0x1e80287b7LL, 0x34770d903LL, 0x2eef86728LL, 0x59266ccb6LL, 0x0110bba61LL, 0x1dfd284efLL, 0x447439d1bLL, 0xfece0e599LL, 0x9309f3703LL, 0x80764d1ddLL, 0x353f1e6a0LL, 0x2c1c12dccLL, 0xc1d21b9d7LL, 0x457ee453eLL, 0xd66faf540LL, 0x44831e652LL, 0xcfd49a848LL, 0x9312d4133LL, 0x3f097d3eeLL, 0x8c9ebef7aLL, 0xa99e29e88LL, 0x0e9fab22cLL, 0x4e748f4fbLL, 0xecdee4288LL, 0xabce5f1d0LL, 0xc42f6876cLL, 0x7ed402ea0LL, 0xe5c4242c3LL, 0xd5b2c31aeLL, 0x286863be6LL, 0x160444d94LL, 0x5f0f5808eLL, 0xae3d44b2aLL, 0x9f5c5d109LL, 0x8ad9316d7LL, 0x3422ba064LL, 0x2fed11d56LL, 0xbea6e3e04LL, 0x04b029eecLL, 0x6deed7435LL, 0x3718ce17cLL, 0x55857f5e2LL, 0x2edac7b62LL, 0x085d6c512LL, 0xd6ca88e0fLL, 0x2b7e1fc69LL, 0xa699d5c1bLL, 0xf05ad74deLL, 0x4cf5fb56dLL, 0x5725e07e1LL, 0x72f18a2deLL, 0x1cec52609LL, 0x48534243cLL, 0x2523a4d69LL, 0x35c1b80d1LL, 0xa4d7338a7LL, 0x0db1af012LL, 0xe61a9475dLL, 0x05df03f91LL, 0x97ae260bbLL, 0x32d627fefLL, 0xb640f73c2LL, 0x45a1ac9c6LL, 0x6a2202de1LL, 0x57d3e25f2LL, 0x5aa9f986eLL, 0x0cc859d8aLL, 0xe3ec6cca8LL, 0x54e95e1aeLL, 0x446887b06LL, 0x7516732beLL, 0x3817ac8f5LL, 0x3e26d938cLL, 0xaa81bc235LL, 0xdf387ca1bLL, 0x0f3a3b3f2LL, 0xb4bf69677LL, 0xae21868edLL, 0x81e1d2d9dLL, 0xa0a9ea14cLL, 0x8eee297a9LL, 0x4740c0559LL, 0xe8b141837LL, 0xac69e0a3dLL, 0x9ed83a1e1LL, 0x5edb55ecbLL, 0x07340fe81LL, 0x50dfbc6bfLL, 0x4f583508aLL, 0xcb1fb78bcLL, 0x4025ced2fLL, 0x39791ebecLL, 0x53ee388f1LL, 0x7d6c0bd23LL, 0x93a995fbeLL, 0x8a41728deLL, 0x2fe70e053LL, 0xab3db443aLL, 0x1364edb05LL, 0x47b6eeed6LL, 0x12e71af01LL, 0x52ff83587LL, 0x3a1575dd8LL, 0x3feaa3564LL, 0xeacf78ba7LL, 0x0872b94f8LL, 0xda8ddf9a2LL, 0x9aa920d2bLL, 0x1f350ed36LL, 0x18a5e861fLL, 0x2c35b89c3LL, 0x3347ac48aLL, 0x7f23e022eLL, 0x2459068fbLL, 0xe83be4b73LL }; + +static const TagCodes tagCodes36h11 = TagCodes(36, 11, t36h11, sizeof(t36h11)/sizeof(t36h11[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11_other.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11_other.h new file mode 100644 index 0000000..073aa00 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11_other.h @@ -0,0 +1,88 @@ +// These codes were generated by TagFamilyGenerator.java from Ed Olson + +// tag36h11Codes : 36 bits, minimum Hamming distance 11, minimum complexity 10 + +#pragma once + +namespace AprilTags { + +const unsigned long long t36h11_other[] = + { 0xd5d628584LL, 0xd97f18b49LL, 0xdd280910eLL, 0xe479e9c98LL, 0xebcbca822LL, 0xf31dab3acLL, 0x056a5d085LL, 0x10652e1d4LL, + 0x17b70ed5eLL, 0x22b1dfeadLL, 0x265ad0472LL, 0x34fe91b86LL, 0x3ff962cd5LL, 0x43a25329aLL, 0x474b4385fLL, 0x4e9d243e9LL, + 0x5246149aeLL, 0x5997f5538LL, 0x683bb6c4cLL, 0x6be4a7211LL, 0x7e3158eeaLL, 0x81da494afLL, 0x858339a74LL, 0x8cd51a5feLL, + 0x9f21cc2d7LL, 0xa2cabc89cLL, 0xadc58d9ebLL, 0xb16e7dfb0LL, 0xb8c05eb3aLL, 0xd25ef139dLL, 0xd607e1962LL, 0xe4aba3076LL, + 0x2dde6a3daLL, 0x43d40c678LL, 0x5620be351LL, 0x64c47fa65LL, 0x686d7002aLL, 0x6c16605efLL, 0x6fbf50bb4LL, 0x8d06d39dcLL, + 0x9baa950f0LL, 0x9f53856b5LL, 0xadf746dc9LL, 0xbc9b084ddLL, 0xd290aa77bLL, 0xd9e28b305LL, 0xe4dd5c454LL, 0xfad2fe6f2LL, + 0x181a8151aLL, 0x26be42c2eLL, 0x2e10237b8LL, 0x405cd5491LL, 0x6ff109f92LL, 0x7742eab1cLL, 0x85e6ac230LL, 0x8d388cdbaLL, + 0x9f853ea93LL, 0xc41ea2445LL, 0xcf1973594LL, 0x14a34a333LL, 0x31eacd15bLL, 0x44377ee34LL, 0x6c79d2dabLL, 0x73cbb3935LL, + 0x89c155bd3LL, 0x8d6a46198LL, 0x91133675dLL, 0xa708d89fbLL, 0xae5ab9585LL, 0xb9558a6d4LL, 0xb98743ab2LL, 0xd6cec68daLL, + 0x1506bcaefLL, 0x4becd217aLL, 0x4f95c273fLL, 0x658b649ddLL, 0xa76c4b1b7LL, 0xecf621f56LL, 0x1c8a56a57LL, 0x3628e92baLL, + 0x53706c0e2LL, 0x7809cfa94LL, 0xc88e77982LL, 0xe97eead6fLL, 0x5af40604aLL, 0xffa6463ebLL, 0x5eceaf9edLL, 0x7c1632815LL, + 0xc1a0095b4LL, 0xe9e25d52bLL, 0x3a6705419LL, 0xa8333012fLL, 0x4ce5704d0LL, 0x508e60a95LL, 0x877476120LL, 0xa864e950dLL, + 0xea45cfce7LL, 0x19da047e8LL, 0x24d4d5937LL, 0x54690a438LL, 0x6e079cc9bLL, 0x99f2e11d7LL, 0x499ff26c7LL, 0x50f1d3251LL, + 0x66e7754efLL, 0x49d1abaa5LL, 0x96ad633ceLL, 0x9a5653993LL, 0xaca30566cLL, 0x8be44b65dLL, 0xb4269f5d4LL, 0xdc68f354bLL, + 0x4dde0e826LL, 0xd548cbd9fLL, 0xfd8b1fd16LL, 0x76521bb7bLL, 0xd1d194bb8LL, 0xd57a8517dLL, 0xd92375742LL, 0xcab16d40cLL, + 0x730c9dd72LL, 0xad9ba39c2LL, 0xb14493f87LL, 0x185409cadLL, 0x77ae2c68dLL, 0x94f5af4b5LL, 0x0a13bad55LL, 0x61ea437cdLL, + 0xa022399e2LL, 0xbd69bc80aLL, 0x203b163d1LL, 0x7bba8f40eLL, 0x784358227LL, 0xc92b728d1LL, 0x92a8cfa02LL, 0x9da3a0b51LL, + 0xdcd4350bcLL, 0x4aa05fdd2LL, 0x60c7bb44eLL, 0x4b358b96cLL, 0x612b2dc0aLL, 0x775289286LL, 0x7ea469e10LL, 0x09e9d0d2cLL, + 0x067299b45LL, 0xb9c89b5faLL, 0x9560f1026LL, 0x62b8f7afaLL, 0xbac139950LL, 0x58b6c4d01LL, 0xa5927c62aLL, 0xe77362e04LL, + 0xf29fed331LL, 0x903205f26LL, 0xc36f2afecLL, 0xae72270a4LL, 0x3d2ec51a7LL, 0x82ea55324LL, 0x1ca1c4576LL, 0xa40c81aefLL, + 0xbddccd730LL, 0x0e617561eLL, 0x585b218faLL, 0x969317b0fLL, 0x588cdacd8LL, 0x67309c3ecLL, 0x8c5f2b938LL, 0x4142f72ddLL, + 0x06e5aaa6bLL, 0x626523aa8LL, 0xb6c475339LL, 0xc56836a4dLL, 0x4647f83b4LL, 0x0908a04f5LL, 0x7862950fbLL, 0xd445808f4LL, + 0x28d68b563LL, 0x04d25374bLL, 0xc8bd52fc0LL, 0x06f5491d5LL, 0x27e5bc5c2LL, 0x2bc065f65LL, 0x96dc3ea0cLL, 0xfdb9fb354LL, + 0x47b3a7630LL, 0xd7372a6abLL, 0x372678c25LL, 0xe768b5cafLL, 0x437d5a886LL, 0x2b091f757LL, 0x91b522cc1LL, 0x62846097cLL, + 0x3aa57f1c1LL, 0x263da6e13LL, 0xfa841bcb5LL, 0x157ebf02aLL, 0xf586e9f93LL, 0xecaf0e8ceLL, 0x82ef46939LL, 0x847d10829LL, + 0x68919e513LL, 0x2aeed3e98LL, 0x11265760cLL, 0x1ce80d6d3LL, 0x0e4c1b374LL, 0x68b0db3e7LL, 0x1c389627aLL, 0xfb79dc26bLL, + 0x9379975a4LL, 0x064ac3391LL, 0x706dfdae2LL, 0x44edfb117LL, 0x86a4f78c8LL, 0xaebd61816LL, 0xf53fd690bLL, 0xb8f91cda2LL, + 0xf0a6173a5LL, 0x12c0e1ec6LL, 0xd1a6e0664LL, 0x6bf37b450LL, 0x62b82d5cfLL, 0xe5f46d153LL, 0x0d1438d4bLL, 0x5af82d134LL, + 0x6ea0ef91fLL, 0x9ff4a76eeLL, 0xde5e56ce1LL, 0xb5c82ed18LL, 0x5b50f279bLL, 0xd7f297fa3LL, 0xef444ad53LL, 0xa8c9a5013LL, + 0x3d300e4f1LL, 0x33fc8fa25LL, 0x43d277c22LL, 0x9d2c1d435LL, 0x5f8952dbaLL, 0xf0cc59103LL, 0xd2f779af6LL, 0xb5e97f461LL, + 0x7f0b3918bLL, 0xe42e63e1aLL, 0x769bc1897LL, 0x97bdee062LL, 0x792229addLL, 0x816ca89bdLL, 0xd41446335LL, 0x3f572b065LL, + 0x2a93af8b0LL, 0xcadde9ac9LL, 0x7176b93c1LL, 0x84b15c29bLL, 0x9b9f9c88fLL, 0x537511febLL, 0xee8891d4fLL, 0x5dc83b096LL, + 0x05bd1b4a0LL, 0x2e073e7ccLL, 0x0b5f174c6LL, 0xb184d5e98LL, 0xd0ef4e74aLL, 0x0ddad25b7LL, 0xbd16e63f6LL, 0x4aa64f025LL, + 0xa252eda74LL, 0xd940d24b4LL, 0x9745a041bLL, 0x055322c79LL, 0x7022f6a83LL, 0xa31416eacLL, 0x96b2880f6LL, 0x48d385322LL, + 0x14d46c8f9LL, 0x11e4d63b7LL, 0x379614f93LL, 0x71eda5cc5LL, 0xaa05e1e39LL, 0xcee09d333LL, 0x52c976570LL, 0x023252178LL, + 0x599fac8f4LL, 0xbb0a48854LL, 0x98cd630bfLL, 0x2d8f6f9a4LL, 0xf5c05a72dLL, 0x9ed9d672bLL, 0x50d8b8ce3LL, 0xe59ac55c8LL, + 0xe09d938a6LL, 0x4ada4f38bLL, 0xbb85a794eLL, 0x5544e5f55LL, 0x9a3db8244LL, 0xe3784e95dLL, 0x796c81d2bLL, 0x6cebb60a1LL, + 0x27b2d55b4LL, 0x6de945a0cLL, 0x4a69d0af9LL, 0x6afea0adfLL, 0x158987899LL, 0x1b528fb48LL, 0x6d183275eLL, 0x73afeed3aLL, + 0x1a7a77a10LL, 0x4be59d2feLL, 0x2ad522b12LL, 0xa82d445fdLL, 0xbbcb59c93LL, 0xe71e94895LL, 0x75b14896fLL, 0xb0afb721aLL, + 0x065d8e6c8LL, 0x372810d9cLL, 0xb77603728LL, 0xad78c1f44LL, 0x90ca91da0LL, 0x2e74184b4LL, 0xc2964c0aaLL, 0xb07f7a899LL, + 0x8ee694eddLL, 0x1ad7caf87LL, 0x2035916c5LL, 0xcd1670631LL, 0x1611c2a77LL, 0x8a1a06962LL, 0xdb970149aLL, 0x5778c6bb4LL, + 0x3fab695feLL, 0x014b9cc35LL, 0x604be4377LL, 0xfd49501f1LL, 0xe2b710c4dLL, 0x6bf7f4a88LL, 0x0adf98124LL, 0xc5ee49adeLL, + 0xe4c34b0eaLL, 0x9b5e0047dLL, 0x4002b5929LL, 0x4e9a35492LL, 0x908aedae9LL, 0xa0bc790edLL, 0xd12b583baLL, 0x431b08264LL, + 0xb7b33afc8LL, 0xd115672f8LL, 0x253296b16LL, 0xbd5e4f6edLL, 0xf1276fc55LL, 0x5feaa426bLL, 0x2d0955cbfLL, 0xcb2ade90eLL, + 0x08b0fe749LL, 0x2709d6730LL, 0x0edc7ec97LL, 0xa7c74d431LL, 0x1536402eaLL, 0x61936f66dLL, 0x7ec973bc9LL, 0xa00a12d3dLL, + 0xc6ed2ccf6LL, 0x8f87c4b9aLL, 0xf049ee52bLL, 0x0d1fa9777LL, 0x85175a497LL, 0x2d917c5c5LL, 0xfc61287b4LL, 0x63ce55156LL, + 0x659cac663LL, 0xbb4b8174fLL, 0x70bd5be0bLL, 0xfb3da5f18LL, 0x917b001e3LL, 0x516870f16LL, 0x03bb5ac33LL, 0x2a510ec0cLL, + 0x07ecd1ae2LL, 0x06642c91aLL, 0xcc7c83662LL, 0xb88d2c60eLL, 0x40d35e87eLL, 0x452f5656eLL, 0xf8c5e5640LL, 0xc68372145LL, + 0x6b61cb49eLL, 0x066ce5035LL, 0x151c05dd0LL, 0xad92f9119LL, 0x8fa874156LL, 0xd7d545982LL, 0x2602c7a8eLL, 0x0d9054ac8LL, + 0xb85332ce0LL, 0xa8e7f583cLL, 0xa2534a713LL, 0x77ce78732LL, 0x6b605c6c5LL, 0x8101603e6LL, 0x9573cd8f0LL, 0xa18bba0abLL, + 0xf5d224ae1LL, 0x9ecb4dfd4LL, 0x2e48c9e03LL, 0x79b4c6ae0LL, 0x60e6b0713LL, 0xea6a8420dLL, 0xa22971a8fLL, 0x605c053fdLL, + 0x57678633aLL, 0xea85c3395LL, 0x7fda1da74LL, 0x9824459caLL, 0xb2eee31f2LL, 0x4a34b0db1LL, 0xb5bbbd933LL, 0x583d9c190LL, + 0xc93e6091cLL, 0xdca7c6e3bLL, 0x214d69b74LL, 0x525894f7fLL, 0x21be0e083LL, 0xf0dbd2784LL, 0x0ffac88d9LL, 0x57f7e33e5LL, + 0x4a7301d85LL, 0x8887af6f6LL, 0x1b8ccb3a1LL, 0x68c1b2878LL, 0x78b6bf950LL, 0x63b9aa851LL, 0x7ed12f23aLL, 0x350eb35b2LL, + 0x561503189LL, 0x3f16ac63cLL, 0xd2fd4b06cLL, 0xa7c49627eLL, 0x36b9f5d0aLL, 0xbca21c149LL, 0xba5bc28efLL, 0xce2c2b89cLL, + 0x776bc0448LL, 0xce170f268LL, 0x8f57303c3LL, 0x74e5fcc9eLL, 0x46de67b7eLL, 0x2b98bd7aaLL, 0xb5c41dc2eLL, 0x12e1e50f8LL, + 0x875f6fcdaLL, 0xf90ea702dLL, 0x7ed051595LL, 0x9d8da07b8LL, 0xbc30d09edLL, 0x77ad8306bLL, 0x82d4a0885LL, 0xf4e1b7a04LL, + 0xb427eabdaLL, 0xdb1b28f5eLL, 0xe5f911de5LL, 0xb8ff4c115LL, 0x3185fbcf4LL, 0xefda16bdfLL, 0xeaa3f6c63LL, 0x9a3f4f520LL, + 0x6317c6e21LL, 0xde1ac8909LL, 0xb962e4d06LL, 0x8a8cc1536LL, 0x0abebf2d5LL, 0x6a3787f5fLL, 0x62cc2622fLL, 0x3196aa59fLL, + 0x9b6816c6bLL, 0x95f398661LL, 0x2b1673eb7LL, 0xc9cf19ba7LL, 0x44394782aLL, 0xd02e2d199LL, 0xe517f16dcLL, 0x433df5666LL, + 0xdbfb6521fLL, 0x6316ed9fbLL, 0x0681b072aLL, 0x24e7e3614LL, 0x3049f22daLL, 0x245b47d67LL, 0x032e59d5dLL, 0x78f512121LL, + 0xf76e98aacLL, 0x20e313ad6LL, 0x9947bd319LL, 0x0719aab9fLL, 0xabe40e6b2LL, 0x9bbec96c6LL, 0xcf05e6446LL, 0xee3c76b79LL, + 0xe9317cc92LL, 0x9bf9aa92bLL, 0x13fe98495LL, 0xd931239a6LL, 0x7264aced9LL, 0x04ed957b4LL, 0xbb7021cbbLL, 0x4609308b5LL, + 0x2d5c52c38LL, 0xceb22ad4fLL, 0x82a47a446LL, 0x04d68909aLL, 0x832cdf368LL, 0x242ed1118LL, 0xd1dda2014LL, 0x901b6a04eLL, + 0x19c1e9514LL, 0x3e9c0c97fLL, 0x1e814bce3LL, 0x55b40b44dLL, 0xeff162399LL, 0x4012da58aLL, 0x1d4d90e43LL, 0x50e79facdLL, + 0x35ef088baLL, 0x774709d00LL, 0xd32e575b3LL, 0x54c5cfff2LL, 0x59bbd73fbLL, 0x3b2dc7e8bLL, 0x13a74d09fLL, 0xc5a21e37fLL, + 0x7aab49b28LL, 0x2793aa17aLL, 0x3a6673575LL, 0x78c27371eLL, 0x1788da29cLL, 0x8b5bb6078LL, 0xa5c506a80LL, 0xc2c487d6aLL, + 0xf238647acLL, 0x601f5e6e2LL, 0x2db5d412aLL, 0xd7c46f24aLL, 0xe4e0b67f5LL, 0xe94793093LL, 0xe07e85846LL, 0xa04f6f205LL, + 0xe47cbc125LL, 0x9022fb419LL, 0xc2127ead8LL, 0xc670f0e63LL, 0xd282518cfLL, 0x63e8d8335LL, 0x2aa63408eLL, 0xb011851aaLL, + 0x2df8c686fLL, 0xa31f8c5f1LL, 0xe8c09cecbLL, 0x4cd645fb9LL, 0xa63b5d9f5LL, 0xd74dd32d6LL, 0xf6869a32aLL, 0xb725dae3eLL, + 0xc5b27c981LL, 0x67d0118b8LL, 0xb3fdb04faLL, 0xf11838f31LL, 0x7e73b5fb9LL, 0x24ec2067aLL, 0x8aaf3bceaLL, 0x04a06dca0LL, + 0x70a03ed6bLL, 0x70dc29b18LL, 0x4d75699a9LL, 0xaedc558d9LL, 0x62590b9afLL, 0x5f9258453LL, 0xf04a9a9ceLL, 0xcd1feb280LL, + 0x7300b05a7LL, 0xadb7ab52cLL, 0x59afeb236LL, 0xf1de62e03LL, 0xf103ba210LL, 0x7cf6472d5LL, 0xf84bf1908LL, 0xa4952dc03LL, + 0x43d506f47LL, 0x8e90eed24LL, 0x9c04974e9LL, 0x953aef583LL, 0x3d839c1bcLL, 0x0348ac64fLL, 0x2a1284fc1LL, 0x9fc565ccdLL, + 0x57118e8c4LL}; + +static const TagCodes tagCodes36h11_other = TagCodes(36, 11, t36h11_other, sizeof(t36h11_other)/sizeof(t36h11_other[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h9.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h9.h new file mode 100644 index 0000000..c3746f2 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h9.h @@ -0,0 +1,63 @@ +/** Tag family with 5329 distinct codes. + bits: 36, minimum hamming: 9, minimum complexity: 10 + + Max bits corrected False positive rate + 0 0.000008 % + 1 0.000287 % + 2 0.005172 % + 3 0.060541 % + 4 0.517333 % + + Generation time: 1121504.896000 s + + Hamming distance between pairs of codes (accounting for rotation): + + 0 0 + 1 0 + 2 0 + 3 0 + 4 0 + 5 0 + 6 0 + 7 0 + 8 0 + 9 88978 + 10 237109 + 11 493264 + 12 979401 + 13 1619560 + 14 2297837 + 15 2681251 + 16 2470697 + 17 1772734 + 18 969816 + 19 406950 + 20 134210 + 21 35419 + 22 7666 + 23 1342 + 24 199 + 25 20 + 26 3 + 27 0 + 28 0 + 29 0 + 30 0 + 31 0 + 32 0 + 33 0 + 34 0 + 35 0 + 36 0 +**/ + +#pragma once + +namespace AprilTags { + +const unsigned long long t36h9[] = + { 0x131b29edaLL, 0x16c41a49fLL, 0x1a6d0aa64LL, 0x1e15fb029LL, 0x21beeb5eeLL, 0x2567dbbb3LL, 0x2910cc178LL, 0x340b9d2c7LL, 0x37b48d88cLL, 0x42af5e9dbLL, 0x4a013f565LL, 0x54fc106b4LL, 0x58a500c79LL, 0x6748c238dLL, 0x6e9aa2f17LL, 0x75ec83aa1LL, 0x799574066LL, 0x7d3e6462bLL, 0x80e754bf0LL, 0x88393577aLL, 0x8be225d3fLL, 0x9a85e7453LL, 0x9e2ed7a18LL, 0xa580b85a2LL, 0xa929a8b67LL, 0xacd29912cLL, 0xb07b896f1LL, 0xb42479cb6LL, 0xbb765a840LL, 0xbf1f4ae05LL, 0xc6712b98fLL, 0xca1a1bf54LL, 0xd514ed0a3LL, 0xd8bddd668LL, 0xf60560490LL, 0xf9ae50a55LL, 0x085212169LL, 0x0bfb0272eLL, 0x1e47b4407LL, 0x294285556LL, 0x2ceb75b1bLL, 0x37e646c6aLL, 0x4a32f8943LL, 0x552dc9a92LL, 0x58d6ba057LL, 0x677a7b76bLL, 0x6b236bd30LL, 0x72754c8baLL, 0x81190dfceLL, 0x84c1fe593LL, 0x886aeeb58LL, 0x8c13df11dLL, 0x970eb026cLL, 0xad045250aLL, 0xb45633094LL, 0xc6a2e4d6dLL, 0xca4bd5332LL, 0xd19db5ebcLL, 0xd546a6481LL, 0xdc988700bLL, 0xe3ea67b95LL, 0xe7935815aLL, 0xeee538ce4LL, 0x0131ea9bdLL, 0x0c2cbbb0cLL, 0x0fd5ac0d1LL, 0x137e9c696LL, 0x17278cc5bLL, 0x22225ddaaLL, 0x2d1d2eef9LL, 0x346f0fa83LL, 0x3f69e0bd2LL, 0x4312d1197LL, 0x4e0da22e6LL, 0x51b6928abLL, 0x590873435LL, 0x5cb1639faLL, 0x605a53fbfLL, 0x67ac34b49LL, 0x6b552510eLL, 0x6efe156d3LL, 0x764ff625dLL, 0x889ca7f36LL, 0x8c45984fbLL, 0x939779085LL, 0x97406964aLL, 0xa5e42ad5eLL, 0xc6d49e14bLL, 0xce267ecd5LL, 0xd9214fe24LL, 0xdcca403e9LL, 0xe7c511538LL, 0xf668d2c4cLL, 0x13b055a74LL, 0x29a5f7d12LL, 0x2d4ee82d7LL, 0x3849b9426LL, 0x3bf2a99ebLL, 0x43448a575LL, 0x46ed7ab3aLL, 0x51e84bc89LL, 0x6b86de4ecLL, 0x6f2fceab1LL, 0x72d8bf076LL, 0x7681af63bLL, 0x852570d4fLL, 0x8c77518d9LL, 0x93c932463LL, 0x9ec4035b2LL, 0xad67c4cc6LL, 0xb110b528bLL, 0xb4b9a5850LL, 0xcaaf47aeeLL, 0xce58380b3LL, 0xd5aa18c3dLL, 0xe0a4e9d8cLL, 0xe44dda351LL, 0xe7f6ca916LL, 0xef48ab4a0LL, 0xf2f19ba65LL, 0xf69a8c02aLL, 0x10391e88dLL, 0x1b33ef9dcLL, 0x262ec0b2bLL, 0x29d7b10f0LL, 0x2d80a16b5LL, 0x312991c7aLL, 0x3c2462dc9LL, 0x437643953LL, 0x521a05067LL, 0x55c2f562cLL, 0x6466b6d40LL, 0x730a78454LL, 0x76b368a19LL, 0x7a5c58fdeLL, 0x7e05495a3LL, 0x85572a12dLL, 0x9051fb27cLL, 0x97a3dbe06LL, 0x9ef5bc990LL, 0xa29eacf55LL, 0xa6479d51aLL, 0xb1426e669LL, 0xb4eb5ec2eLL, 0xb8944f1f3LL, 0xce89f1491LL, 0xd5dbd201bLL, 0xd984c25e0LL, 0xe0d6a316aLL, 0xf32354e43LL, 0xfa75359cdLL, 0x0918f70e1LL, 0x106ad7c6bLL, 0x17bcb87f5LL, 0x22b789944LL, 0x38ad2bbe2LL, 0x4750ed2f6LL, 0x4af9dd8bbLL, 0x6f934126dLL, 0x8931d3ad0LL, 0x8cdac4095LL, 0x9083b465aLL, 0x9b7e857a9LL, 0xa2d066333LL, 0xa679568f8LL, 0xadcb37482LL, 0xb8c6085d1LL, 0xc017e915bLL, 0xc769c9ce5LL, 0xe85a3d0d2LL, 0xec032d697LL, 0xefac1dc5cLL, 0xf3550e221LL, 0xfaa6eedabLL, 0x01f8cf935LL, 0x22e942d22LL, 0x4782a66d4LL, 0x4b2b96c99LL, 0x4ed48725eLL, 0x5d7848972LL, 0x64ca294fcLL, 0x687319ac1LL, 0x6fc4fa64bLL, 0x736deac10LL, 0x7abfcb79aLL, 0x85ba9c8e9LL, 0x89638ceaeLL, 0xa6ab0fcd6LL, 0xaa540029bLL, 0xb1a5e0e25LL, 0xbca0b1f74LL, 0xc049a2539LL, 0xcb4473688LL, 0xceed63c4dLL, 0xd29654212LL, 0xdd9125361LL, 0xe13a15926LL, 0xe88bf64b0LL, 0xec34e6a75LL, 0xfad8a8189LL, 0xfe819874eLL, 0x097c6989dLL, 0x26c3ec6c5LL, 0x2a6cdcc8aLL, 0x2e15cd24fLL, 0x31bebd814LL, 0x4b5d50077LL, 0x5a011178bLL, 0x5daa01d50LL, 0x6c4dc3464LL, 0x6ff6b3a29LL, 0x7748945b3LL, 0x89954628cLL, 0x90e726e16LL, 0xa333d8aefLL, 0xa6dcc90b4LL, 0xc07b5b917LL, 0xc4244bedcLL, 0xcb762ca66LL, 0xd670fdbb5LL, 0xe16bced04LL, 0xf76170fa2LL, 0x14a8f3dcaLL, 0x1bfad4954LL, 0x2a9e96068LL, 0x2e478662dLL, 0x31f076bf2LL, 0x39425777cLL, 0x52e0e9fdfLL, 0x5689da5a4LL, 0x5ddbbb12eLL, 0x68d68c27dLL, 0x94c1d07b9LL, 0x986ac0d7eLL, 0x9fbca1908LL, 0xa36591ecdLL, 0xaab772a57LL, 0xc0ad14cf5LL, 0xcba7e5e44LL, 0xd2f9c69ceLL, 0xd6a2b6f93LL, 0xda4ba7558LL, 0xddf497b1dLL, 0xec9859231LL, 0xfee50af0aLL, 0x0636eba94LL, 0x14daad1a8LL, 0x397410b5aLL, 0x40c5f16e4LL, 0x4817d226eLL, 0x655f55096LL, 0x69084565bLL, 0x94f389b97LL, 0x989c7a15cLL, 0x9fee5ace6LL, 0xaae92be35LL, 0xb98ced549LL, 0xcbd99f222LL, 0xd32b7fdacLL, 0xecca1260fLL, 0xf07302bd4LL, 0x02bfb48adLL, 0x0a1195437LL, 0x0dba859fcLL, 0x18b556b4bLL, 0x2eaaf8de9LL, 0x35fcd9973LL, 0x3d4eba4fdLL, 0x4bf27bc11LL, 0x56ed4cd60LL, 0x61e81deafLL, 0x6939fea39LL, 0x82d89129cLL, 0x917c529b0LL, 0x952542f75LL, 0xa3c904689LL, 0xa771f4c4eLL, 0xb26cc5d9dLL, 0xc4b977a76LL, 0xcfb448bc5LL, 0xdaaf19d14LL, 0xe952db428LL, 0xf44dac577LL, 0x153e1f964LL, 0x278ad163dLL, 0x3285a278cLL, 0x39d783316LL, 0x487b44a2aLL, 0x5e70e6cc8LL, 0x6219d728dLL, 0x696bb7e17LL, 0x746688f66LL, 0x830a4a67aLL, 0x91ae0bd8eLL, 0xa051cd4a2LL, 0xab4c9e5f1LL, 0xb6476f740LL, 0xbd99502caLL, 0xc89421419LL, 0xcc3d119deLL, 0xd737e2b2dLL, 0xe5dba4241LL, 0xf82855f1aLL, 0x156fd8d42LL, 0x1cc1b98ccLL, 0x2b657afe0LL, 0x32b75bb6aLL, 0x36604c12fLL, 0x53a7cef57LL, 0x5ea2a00a6LL, 0x86e4f401dLL, 0x9931a5cf6LL, 0xa42c76e45LL, 0xaf2747f94LL, 0xb67928b1eLL, 0xbdcb096a8LL, 0xc8c5da7f7LL, 0xcc6ecadbcLL, 0xd3c0ab946LL, 0xdebb7ca95LL, 0xe2646d05aLL, 0x11f8a1b5bLL, 0x244553834LL, 0x3de3e6097LL, 0x48deb71e6LL, 0x53d988335LL, 0x5782788faLL, 0x69cf2a5d3LL, 0x71210b15dLL, 0x8716ad3fbLL, 0x9d0c4f699LL, 0xb301f1937LL, 0xc1a5b304bLL, 0x122a5af39LL, 0x2bc8ed79cLL, 0x331ace326LL, 0x4cb960b89LL, 0x540b41713LL, 0x62af02e27LL, 0x6657f33ecLL, 0x6da9d3f76LL, 0xa0e6f903cLL, 0xabe1ca18bLL, 0xaf8aba750LL, 0xc1d76c429LL, 0xc9294cfb3LL, 0xf8bd81ab4LL, 0x0b0a3378dLL, 0x3e4758853LL, 0x5f37cbc40LL, 0x6689ac7caLL, 0x6a329cd8fLL, 0x6ddb8d354LL, 0xb365640f3LL, 0xb70e546b8LL, 0xbe6035242LL, 0xc5b215dccLL, 0xc95b06391LL, 0xcd03f6956LL, 0xd0ace6f1bLL, 0xe6a2891b9LL, 0xf19d5a308LL, 0x128dcd6f5LL, 0x1636bdcbaLL, 0x2c2c5ff58LL, 0x3727310a7LL, 0x5bc094a59LL, 0x5f698501eLL, 0x6312755e3LL, 0x87abd8f95LL, 0x8b54c955aLL, 0x99f88ac6eLL, 0xac453c947LL, 0xc23adebe5LL, 0xd487908beLL, 0xea7d32b5cLL, 0x0072d4dfaLL, 0x07c4b5984LL, 0x12bf86ad3LL, 0x250c387acLL, 0x2c5e19336LL, 0x3b01daa4aLL, 0x50f77cce8LL, 0x54a06d2adLL, 0x58495d872LL, 0x71e7f00d5LL, 0x8434a1daeLL, 0x87dd92373LL, 0xa5251519bLL, 0xb01fe62eaLL, 0xc61588588LL, 0xf952ad64eLL, 0x0f484f8ecLL, 0x253df1b8aLL, 0x28e6e214fLL, 0x4285749b2LL, 0x462e64f77LL, 0x54d22668bLL, 0x5c2407215LL, 0x6ac7c8929LL, 0x80bd6abc7LL, 0x9a5bfd42aLL, 0xa1adddfb4LL, 0xa556ce579LL, 0xb3fa8fc8dLL, 0xc64741966LL, 0xcd99224f0LL, 0xeae0a5318LL, 0xf5db76467LL, 0x306a7c0b7LL, 0x4a090e91aLL, 0x58acd002eLL, 0x6ea2722ccLL, 0xacda684e1LL, 0xcdcadb8ceLL, 0xe7696e131LL, 0xf60d2f845LL, 0x25a164346LL, 0x344525a5aLL, 0x3b97065e4LL, 0x42e8e716eLL, 0x4de3b82bdLL, 0x553598e47LL, 0x5c87799d1LL, 0x6b2b3b0e5LL, 0x8120dd383LL, 0x84c9cd948LL, 0xbf58d3598LL, 0xdca0563c0LL, 0xeeed08099LL, 0x1ad84c5d5LL, 0x46c390b11LL, 0x606223374LL, 0x7a00b5bd7LL, 0xa994ea6d8LL, 0xb0e6cb262LL, 0xce2e4e08aLL, 0x2d56b768cLL, 0x434c5992aLL, 0x643cccd17LL, 0x6f379de66LL, 0x76897e9f0LL, 0x88d6306c9LL, 0xa9c6a3ab6LL, 0xb86a651caLL, 0xd208f7a2dLL, 0x29df804a5LL, 0x646e860f5LL, 0x6f6957244LL, 0x7a6428393LL, 0xa9f85ce94LL, 0xada14d459LL, 0xe830530a9LL, 0x2a1139883LL, 0x4758bc6abLL, 0x8939a2e85LL, 0x8ce29344aLL, 0x908b83a0fLL, 0xadd306837LL, 0xb524e73c1LL, 0xc3c8a8ad5LL, 0x353dc3db0LL, 0x6129082ecLL, 0x687ae8e76LL, 0x771eaa58aLL, 0x94662d3b2LL, 0x9f60fe501LL, 0xd29e235c7LL, 0x147f09da1LL, 0x2322cb4b5LL, 0x406a4e2ddLL, 0x4b651f42cLL, 0x615ac16caLL, 0x68aca2254LL, 0x9840d6d55LL, 0xb9314a142LL, 0xd678ccf6aLL, 0xddcaadaf4LL, 0xf76940357LL, 0x1859b3744LL, 0x1fab942ceLL, 0x2aa66541dLL, 0x4b96d880aLL, 0x5691a9959LL, 0x6c874bbf7LL, 0xae68323d1LL, 0xc0b4e40aaLL, 0xcf58a57beLL, 0xd6aa86348LL, 0xe54e47a5cLL, 0x40cdc0a99LL, 0x5a6c532fcLL, 0x82aea7273LL, 0x94fb58f4cLL, 0xa7480ac25LL, 0xaaf0fb1eaLL, 0xb5ebcc339LL, 0xb994bc8feLL, 0xd3334f161LL, 0xda852fcebLL, 0xe1d710875LL, 0x0dc254db1LL, 0x2760e7614LL, 0x2b09d7bd9LL, 0x3604a8d28LL, 0x61efed264LL, 0x6598dd829LL, 0x6941cddeeLL, 0x98d6028efLL, 0xa3d0d3a3eLL, 0xab22b45c8LL, 0xb27495152LL, 0x06a22d605LL, 0x1545eed19LL, 0x4fd4f4969LL, 0x746e5831bLL, 0x7f692946aLL, 0x955ecb708LL, 0xa4028ce1cLL, 0xab546d9a6LL, 0xb2a64e530LL, 0xc89bf07ceLL, 0xe5e3735f6LL, 0x4162ec633LL, 0x5006add47LL, 0x57588e8d1LL, 0x8a95b3997LL, 0x959084ae6LL, 0xa434461faLL, 0xba29e8498LL, 0xbdd2d8a5dLL, 0x27f6131aeLL, 0x2b9f03773LL, 0x32f0e42fdLL, 0x4194a5a11LL, 0x48e68659bLL, 0x787abb09cLL, 0x7c23ab661LL, 0x83758c1ebLL, 0xaf60d0727LL, 0xd7a32469eLL, 0xe646e5db2LL, 0x0ae049764LL, 0x3e1d6e82aLL, 0x665fc27a1LL, 0x6db1a332bLL, 0x715a938f0LL, 0x750383eb5LL, 0x83a7455c9LL, 0xa0eec83f1LL, 0xb33b7a0caLL, 0xc5882bda3LL, 0xc9311c368LL, 0xccda0c92dLL, 0xe6789f190LL, 0xedca7fd1aLL, 0x1263e36ccLL, 0x19b5c4256LL, 0x2c0275f2fLL, 0x4949f8d57LL, 0x509bd98e1LL, 0xa4c971d94LL, 0xd0b4b62d0LL, 0x079acb95bLL, 0x12959caaaLL, 0x21395e1beLL, 0x33860fe97LL, 0x5f71543d3LL, 0x7566f6671LL, 0x840ab7d85LL, 0x8f0588ed4LL, 0xa4fb2b172LL, 0xb39eec886LL, 0xbe99bd9d5LL, 0xc9948eb24LL, 0xe33321387LL, 0x1a1936a12LL, 0x425b8a989LL, 0x5bfa1d1ecLL, 0xe70dcad2aLL, 0xee5fab8b4LL, 0x4d8814eb6LL, 0x7973593f2LL, 0x9311ebc55LL, 0x21ce89d58LL, 0x5162be859LL, 0x58b49f3e3LL, 0x6eaa41681LL, 0xbb85f8faaLL, 0xd5248b80dLL, 0x4a42970adLL, 0x678a19ed5LL, 0x6b330a49aLL, 0x762ddb5e9LL, 0xa5c2100eaLL, 0xca5b73a9cLL, 0xeb4be6e89LL, 0xf29dc7a13LL, 0xfd9898b62LL, 0x17372b3c5LL, 0x25daecad9LL, 0x556f215daLL, 0x975007db4LL, 0xa5f3c94c8LL, 0xa99cb9a8dLL, 0x0c6e13654LL, 0x29b59647cLL, 0x55a0da9b8LL, 0x7a3a3e36aLL, 0x9781c1192LL, 0x9b2ab1757LL, 0x9ed3a1d1cLL, 0xad7763430LL, 0xcabee6258LL, 0xef5849c0aLL, 0x1eec7e70bLL, 0x4ad7c2c47LL, 0x731a16bbeLL, 0x8566c8897LL, 0xe0e6418d4LL, 0xf332f35adLL, 0x267018673LL, 0x3513d9d87LL, 0x3c65ba911LL, 0x4eb26c5eaLL, 0x56044d174LL, 0x64a80e888LL, 0x7e46a10ebLL, 0xbc7e97300LL, 0x4040642b4LL, 0x43e954879LL, 0x4b3b35403LL, 0xaa639ea05LL, 0x232a9a86aLL, 0x47c3fe21cLL, 0x825303e6cLL, 0xa34377259LL, 0xc433ea646LL, 0xddd27cea9LL, 0xe5245da33LL, 0x0d66b19aaLL, 0x5deb59898LL, 0x653d3a422LL, 0xa37530637LL, 0xb5c1e2310LL, 0xcbb7845aeLL, 0xd6b2556fdLL, 0xeca7f799bLL, 0xfef4a9674LL, 0x06468a1feLL, 0x4f7951562LL, 0x98ac188c6LL, 0xff2662a52LL, 0x0a2133ba1LL, 0x11731472bLL, 0x326387b18LL, 0x3d5e58c67LL, 0x69499d1a3LL, 0x77ed5e8b7LL, 0xb27c64507LL, 0xc4c9161e0LL, 0x11a4cdb09LL, 0x329540ef6LL, 0x572ea48a8LL, 0xa40a5c1d1LL, 0xaf052d320LL, 0xb2ae1d8e5LL, 0xb6570deaaLL, 0xd39e90cd2LL, 0xd74781297LL, 0x157f774acLL, 0x4c658cb37LL, 0xed6edc913LL, 0x0e5f4fd00LL, 0x74d999e8cLL, 0x7c2b7aa16LL, 0xba6370c2bLL, 0x2bd88bf06LL, 0x2f817c4cbLL, 0x750b5326aLL, 0x95fbc6657LL, 0xe6806e545LL, 0x2fb3358a9LL, 0x335c25e6eLL, 0x7c8eed1d2LL, 0xafcc12298LL, 0xb3750285dLL, 0x19ef4c9e9LL, 0x3736cf811LL, 0x3e88b039bLL, 0x582742bfeLL, 0x5f7923788LL, 0x8f0d58289LL, 0x9db11999dLL, 0xa15a09f62LL, 0xaffdcb676LL, 0xc99c5ded9LL, 0xee35c188bLL, 0x376888befLL, 0xa8dda3ecaLL, 0xd4c8e8406LL, 0x0baefda91LL, 0x21a49fd2fLL, 0x3eec22b57LL, 0x797b287a7LL, 0x9a6b9bb94LL, 0xc9ffd0695LL, 0xdc4c8236eLL, 0x37cbfb3abLL, 0x3b74eb970LL, 0x516a8dc0eLL, 0x84a7b2cd4LL, 0xb7e4d7d9aLL, 0xd8d54b187LL, 0x1ab631961LL, 0x1e5f21f26LL, 0x25b102ab0LL, 0xa220eeedaLL, 0x13960a1b5LL, 0x34867d5a2LL, 0x3bd85e12cLL, 0x1ec2946e2LL, 0x26147526cLL, 0x88e5cee33LL, 0xa62d51c5bLL, 0xb12822daaLL, 0xb87a03934LL, 0xcac6b560dLL, 0xce6fa5bd2LL, 0x34e9efd5eLL, 0x3fe4c0eadLL, 0x6f78f59aeLL, 0x76cad6538LL, 0x97bb49925LL, 0xcea15efb0LL, 0xd24a4f575LL, 0xd5f33fb3aLL, 0x01de84076LL, 0x3c6d89cc6LL, 0x4b114b3daLL, 0x6858ce202LL, 0x8949415efLL, 0x9b95f32c8LL, 0xd27c08953LL, 0xd624f8f18LL, 0xd9cde94ddLL, 0x4b43047b8LL, 0x90ccdb557LL, 0xbcb81fa93LL, 0xe4fa73a0aLL, 0x0993d73bcLL, 0x39280bebdLL, 0x89acb3dabLL, 0x90fe94935LL, 0xda315bc99LL, 0xddda4c25eLL, 0x40aba5e25LL, 0xb5c9b16c5LL, 0xb972a1c8aLL, 0xcbbf53963LL, 0xda6315077LL, 0x02a568feeLL, 0x09f749b78LL, 0x1fecebe16LL, 0x2e90ad52aLL, 0x4bd830352LL, 0x5a7bf1a66LL, 0x950af76b6LL, 0xaea989f19LL, 0xf08a706f3LL, 0x201ea51f4LL, 0x23c7957b9LL, 0x4c09e9730LL, 0x5704ba87fLL, 0x77f52dc6cLL, 0xa7896276dLL, 0xb62d23e81LL, 0x0a5abc334LL, 0x623144dacLL, 0x9cc04a9fcLL, 0x242b07f75LL, 0x32cec9689LL, 0xa7ecd4f29LL, 0xc18b6778cLL, 0x07153e52bLL, 0x0e671f0b5LL, 0x8ad70b4dfLL, 0xbe14305a5LL, 0xc1bd20b6aLL, 0xe9ff74ae1LL, 0x4cd0ce6a8LL, 0x62c670946LL, 0x7c65031a9LL, 0x960395a0cLL, 0xafa22826fLL, 0x0eca91871LL, 0x3363f5223LL, 0x5ba64919aLL, 0xd816355c4LL, 0x164e2b7d9LL, 0x2148fc928LL, 0x87c346ab4LL, 0xd49efe3ddLL, 0x9a41b1b6bLL, 0xa193926f5LL, 0xb03753e09LL, 0x37a211382LL, 0x4645d2a96LL, 0x673645e83LL, 0xb411fd7acLL, 0x4a207c439LL, 0x96fc33d62LL, 0x9e4e148ecLL, 0x3f57646c8LL, 0x46a945252LL, 0x81384aea2LL, 0x972ded140LL, 0xa97a9ee19LL, 0xca6b12206LL, 0x29937b808LL, 0x6f1d525a7LL, 0x766f33131LL, 0x177882f0dLL, 0x1eca63a97LL, 0x55b079122LL, 0x8c968e7adLL, 0xb881d2ce9LL, 0xbc2ac32aeLL, 0xd2206554cLL, 0x22a50d43aLL, 0x2d9fde589LL, 0x3fec90262LL, 0x60dd0364fLL, 0xbc5c7c68cLL, 0x64b7acff2LL, 0x9f46b2c42LL, 0x59ee95281LL, 0x6c3b46f5aLL, 0xaa733d16fLL, 0x2a8c19b5eLL, 0x31ddfa6e8LL, 0xb59fc769cLL, 0xf780ade76LL, 0x06246f58aLL, 0x56a917478LL, 0x7047a9cdbLL, 0x91381d0c8LL, 0xda6ae442cLL, 0x61d5a19a5LL, 0xa3b68817fLL, 0xb25a49893LL, 0xc4a6fb56cLL, 0xd6f3ad245LL, 0xde458ddcfLL, 0x1182b2e95LL, 0x2eca35cbdLL, 0x327326282LL, 0x4c11b8ae5LL, 0x570c89c34LL, 0x5ab57a1f9LL, 0xcfd385a99LL, 0xcc5c4e8b2LL, 0xff9973978LL, 0xa7f4a42deLL, 0xdedab9969LL, 0x1d12afb7eLL, 0x74e9385f6LL, 0x99829bfa8LL, 0xc916d0aa9LL, 0xd7ba921bdLL, 0x0af7b7283LL, 0x2f911ac35LL, 0x6dc910e4aLL, 0x8767a36adLL, 0xb352e7be9LL, 0xd4435afd6LL, 0x2c19e3a4eLL, 0x2fc2d4013LL, 0x6a51d9c63LL, 0xea6ab6652LL, 0xf1bc971dcLL, 0xfcb76832bLL, 0x49931fc54LL, 0xa8bb89256LL, 0x3b211791eLL, 0x4272f84a8LL, 0xbee2e48d2LL, 0xd4d886b70LL, 0x2caf0f5e8LL, 0x3b52d0cfcLL, 0x5fec346aeLL, 0x932959774LL, 0x16eb26728LL, 0x76138fd2aLL, 0xd53bf932cLL, 0x9adeacabaLL, 0x3f90ece5bLL, 0x4a8bbdfaaLL, 0x901594d49LL, 0xbfdb82c28LL, 0x17b20b6a0LL, 0x143ad44b9LL, 0x2dd966d1cLL, 0x61168bde2LL, 0xa6a062b81LL, 0xcee2b6af8LL, 0xe8814935bLL, 0x0d1aacd0dLL, 0x7ae6d7a23LL, 0xd2bd6049bLL, 0xe8b302739LL, 0xf3add3888LL, 0x05fa85561LL, 0x567f2d44fLL, 0xdde9ea9c8LL, 0xe192daf8dLL, 0xf7887d22bLL, 0x77a159c1aLL, 0x94e8dca42LL, 0xa38c9e156LL, 0x7b7c035bdLL, 0xc857baee6LL, 0x44c7a7310LL, 0x6d09fb287LL, 0x745bdbe11LL, 0x8a517e0afLL, 0xa79900ed7LL, 0xb66e7b9c9LL, 0xcc641dc67LL, 0x1596e4fcbLL, 0x661b8ceb9LL, 0x8ab4f086bLL, 0xa453830ceLL, 0x8ae6a9c49LL, 0x0ea876bfdLL, 0x3341da5afLL, 0x5f2d1eaebLL, 0xea40cc629LL, 0x5bb5e7904LL, 0xa4e8aec68LL, 0xac3a8f7f2LL, 0xb73560941LL, 0xbade50f06LL, 0xbe87414cbLL, 0xc23031a90LL, 0xcd2b02bdfLL, 0x6a8b623f6LL, 0x6e34529bbLL, 0x8f24c5da8LL, 0x9dc8874bcLL, 0xa17177a81LL, 0xdfa96dc96LL, 0x511e88f71LL, 0x96a85fd10LL, 0xd4e055f25LL, 0xead5f81c3LL, 0x933128b29LL, 0xcdc02e779LL, 0x0fa114f53LL, 0x72726eb1aLL, 0x970bd24ccLL, 0x0880ed7a7LL, 0x973d8b8aaLL, 0xeb6b23d5dLL, 0x1aff5885eLL, 0x1ea848e23LL, 0x29a319f72LL, 0x852292fafLL, 0x88cb83574LL, 0x0c8d50528LL, 0x3126b3edaLL, 0xf320770a3LL, 0x4af6ffb1bLL, 0x4e9ff00e0LL, 0x5d43b17f4LL, 0x733953a92LL, 0x9080d68baLL, 0xa67678b58LL, 0xdd5c8e1e3LL, 0x22e664f82LL, 0x05d09b538LL, 0x14745cc4cLL, 0x8d3b58ab1LL, 0xd66e1fe15LL, 0x40915a566LL, 0x4b8c2b6b5LL, 0x911602454LL, 0xe5439a907LL, 0xe1cc63720LL, 0x5a935f585LL, 0x7431f1de8LL, 0x7b83d2972LL, 0x8dd08464bLL, 0xa76f16eaeLL, 0xde552c539LL, 0x153b41bc4LL, 0x2787f389dLL, 0x98fd0eb78LL, 0xaef2b0e16LL, 0x2b629d240LL, 0x0aa3e3231LL, 0x836adf096LL, 0xb2ff13b97LL, 0xd79877549LL, 0xf888ea936LL, 0x540863973LL, 0x62ac25087LL, 0xc9266f213LL, 0xd42140362LL, 0x877741e17LL, 0x3e7633e91LL, 0x50c2e5b6aLL, 0x12bca8d33LL, 0x758e028faLL, 0x92d585722LL, 0x12ee62111LL, 0xbb4992a77LL, 0xc9ed5418bLL, 0xd4e8252daLL, 0x6af6a3f67LL, 0x76232e494LL, 0xb45b246a9LL, 0xc6a7d6382LL, 0x3bc5e1c22LL, 0x640835b99LL, 0x8c4a89b10LL, 0x8ff37a0d5LL, 0xa2402bdaeLL, 0x437b34f68LL, 0x646ba8355LL, 0x6bbd88edfLL, 0xc39411957LL, 0xcae5f24e1LL, 0xf32846458LL, 0x8cdfb56aaLL, 0x9088a5c6fLL, 0xadd028a97LL, 0xe4b63e122LL, 0x094fa1ad4LL, 0x17f3631e8LL, 0x40677053dLL, 0x0d5c04855LL, 0xa71373aa7LL, 0x0292ecae4LL, 0x1136ae1f8LL, 0x8dd853a00LL, 0xb61aa7977LL, 0x363384366LL, 0x5723f7753LL, 0xf48456f6aLL, 0xdec06e0aaLL, 0x0aabb25e6LL, 0xa80c11dfdLL, 0xf890b9cebLL, 0x3a71a04c5LL, 0x7157b5b50LL, 0xdb7af02a1LL, 0x57eadc6cbLL, 0xac1874b7eLL, 0x3ad512c81LL, 0x2c630a94bLL, 0x87e283988LL, 0x169f21a8bLL, 0x7d196bc17LL, 0x1e22bb9f3LL, 0xd8ca9e032LL, 0x1e862e1afLL, 0xbbe68d9c6LL, 0xce64f8a7dLL, 0x8cb5cb681LL, 0xce96b1e5bLL, 0x057cc74e6LL, 0x64d6e9ec6LL, 0x687fda48bLL, 0xd64c051a1LL, 0x2327bcacaLL, 0x31cb7e1deLL, 0x7afe45542LL, 0x7ea735b07LL, 0xddcf9f109LL, 0x2aab56a32LL, 0x56969af6eLL, 0x029abbe99LL, 0x61c32549bLL, 0x656c15a60LL, 0xcf8f501b1LL, 0xde64caca3LL, 0xf45a6cf41LL, 0x9cb59d8a7LL, 0x5b06704abLL, 0x6d5322184LL, 0xbdd7ca072LL, 0x48eb77bb0LL, 0x4c9468175LL, 0xc55b63fdaLL, 0xd7a815cb3LL, 0x20dadd017LL, 0x6a0da437bLL, 0xea2680d6aLL, 0x19bab586bLL, 0xdbb478a34LL, 0x50d2842d4LL, 0x6350ef38bLL, 0x54dee7055LL, 0x2d000589aLL, 0x4a47886c2LL, 0xad18e2289LL, 0xf9f499bb2LL, 0x1e8dfd564LL, 0x2988ce6b3LL, 0x9afde998eLL, 0x76963f3baLL, 0xebb44ac5aLL, 0xe83d13a73LL, 0x0cd677425LL, 0x52604e1c4LL, 0x9f3c05aedLL, 0xaa36d6c3cLL, 0xbc8388915LL, 0xcb274a029LL, 0x7b06148f7LL, 0xc46a95039LL, 0x9c59fa4a0LL, 0xb9a17d2c8LL, 0x39ba59cb7LL, 0xc4ce077f5LL, 0xf0b94bd31LL, 0x3a1dcc473LL, 0x57654f29bLL, 0x2f866dae0LL, 0x57c8c1a57LL, 0xdb8a8ea0bLL, 0x423691f75LL, 0x66cff5927LL, 0xcd4a3fab3LL, 0xe71a8b6f4LL, 0x21a991344LL, 0x304d52a58LL, 0x6733680e3LL, 0x307f0be36LL, 0xb440d8deaLL, 0xfd73a014eLL, 0x3459b57d9LL, 0x3f5486928LL, 0xe46a39485LL, 0xd9d2daaf2LL, 0xe8a8555e4LL, 0xa35037c23LL, 0xc472643eeLL, 0x239acd9f0LL, 0xb9db05a5bLL, 0xcc27b7734LL, 0x364af1e85LL, 0xc8b08054dLL, 0x8e84ed0b9LL, 0x36e01da1fLL, 0xafa719884LL, 0xbb055d18fLL, 0xf93d533a4LL, 0x6ab26e67fLL, 0xe3796a4e4LL, 0xf21d2bbf8LL, 0x464ac40abLL, 0xe75413e87LL, 0x676cf0876LL, 0xe785cd265LL, 0xdcbcb54f4LL, 0x5cd591ee3LL, 0xb1032a396LL, 0xa9e302beaLL, 0xb886c42feLL, 0x523e33550LL, 0xb50f8d117LL, 0x1b89d72a3LL, 0x1bbb90681LL, 0xf753e60adLL, 0x4f2a6eb25LL, 0x03159c174LL, 0x06be8c739LL, 0xd03be986aLL, 0x0acaef4baLL, 0x7fe8fad5aLL, 0x50867f637LL, 0x78c8d35aeLL, 0x2875e4a9eLL, 0x78fa8c98cLL, 0xac37b1a52LL, 0xb0125b3f5LL, 0xa54943684LL, 0xa1d20c49dLL, 0xd8e9daf06LL, 0xdcc4848a9LL, 0xf6631710cLL, 0x851fb520fLL, 0xa9b918bc1LL, 0xb13cb2b29LL, 0xef74a8d3eLL, 0xf6c6898c8LL, 0xaa1c8b37dLL, 0xb5175c4ccLL, 0x64c46d9bcLL, 0xcee7a810dLL, 0x05cdbd798LL, 0xfb04a5a27LL, 0x73fd5ac6aLL, 0xecc456acfLL, 0x56e791220LL, 0x693442ef9LL, 0xc10acb971LL, 0x44cc98925LL, 0x5ac23abc3LL, 0x7bb2adfb0LL, 0x2064ee351LL, 0x4c81ebc6bLL, 0xaf84fec10LL, 0xa112f68daLL, 0x078d40a66LL, 0xa144afcb8LL, 0xb73a51f56LL, 0xe6ce86a57LL, 0xf94cf1b0eLL, 0x7213ed973LL, 0xfd595488fLL, 0xb45846909LL, 0x1b3603251LL, 0x90540eaf1LL, 0xebd387b2eLL, 0x8933e7345LL, 0xb8c81be46LL, 0x4b2daa50eLL, 0xe13c2919bLL, 0x4c2648864LL, 0xb64982fb5LL, 0xa0e90c8b1LL, 0x5f39df4b5LL, 0xac1596ddeLL, 0xf5485e142LL, 0xc9f2457a0LL, 0x60327d80bLL, 0x225df9db2LL, 0x35a349de1LL, 0xb5bc267d0LL, 0x9c8106729LL, 0xa029f6ceeLL, 0xaecdb8402LL, 0x44dc3708fLL, 0x0e27dade2LL, 0x48b6e0a32LL, 0x9d16322c3LL, 0x5f0ff548cLL, 0x3010ec525LL, 0x0f83eb8f4LL, 0x1e27ad008LL, 0x9345b88a8LL, 0xa24cec778LL, 0x4e510d6a3LL, 0xb8d7ba5b0LL, 0x1f83bdb1aLL, 0x53560e77aLL, 0xed0d7d9ccLL, 0x78b6570a4LL, 0x07a4ae585LL, 0x498594d5fLL, 0x9a0a3cc4dLL, 0xdc1cdc805LL, 0xe717ad954LL, 0xa1bf8ff93LL, 0xb063516a7LL, 0xc6bc66101LL, 0xd93ad11b8LL, 0xe435a2307LL, 0xe7de928ccLL, 0x60a58e731LL, 0x769b309cfLL, 0x0ca9af65cLL, 0x97bd5d19aLL, 0x560e2fd9eLL, 0x1f59d3af1LL, 0xbcebec6e6LL, 0x14f42e53cLL, 0xb62f376f6LL, 0xe31319f88LL, 0x66d4e6f3cLL, 0x6e26c7ac6LL, 0xee3fa44b5LL, 0x80a532b7dLL, 0xe3768c744LL, 0xd15b93e49LL, 0xb09cd9e3aLL, 0x642494ccdLL, 0xf6ed95b51LL, 0x7769e4cfcLL, 0x779b9e0daLL, 0x3995612a3LL, 0xbd572e257LL, 0x0a32e5b80LL, 0x5ae946e4cLL, 0x8e266bf12LL, 0x36819c878LL, 0x41ae26da5LL, 0x0af9caaf8LL, 0x542c91e5cLL, 0x57d582421LL, 0x6a22340faLL, 0xac668d090LL, 0x6e6050259LL, 0xbb3c07b82LL, 0xee792cc48LL, 0xb072efe11LL, 0xbf486a903LL, 0x88940e656LL, 0xa98481a43LL, 0x7a53bf6feLL, 0x40bd57e04LL, 0x2ea25f509LL, 0x745def686LL, 0xc88b87b39LL, 0xc8bd40f17LL, 0x9d356f197LL, 0xf8b4e81d4LL, 0x0b0199eadLL, 0x1d4e4bb86LL, 0x78cdc4bc3LL, 0x54661a5efLL, 0x79313737fLL, 0x3ed3eab0dLL, 0x6f2f04586LL, 0x9b4c01ea0LL, 0x4b2acc76eLL, 0x52e01fab4LL, 0x9c12e6e18LL, 0x2acf84f1bLL, 0xc0de03ba8LL, 0x362dc8826LL, 0x15a0c7bf5LL, 0xd07a63612LL, 0x57e520b8bLL, 0x2c2b95a2dLL, 0x0f47853c1LL, 0x465f53e2aLL, 0xa96266dcfLL, 0xf32a59ccdLL, 0xd6a9bbe1dLL, 0x5e1479396LL, 0x39accedc2LL, 0xbd6e9bd76LL, 0x5ea9a4f30LL, 0x4c8eac635LL, 0xa83fdea50LL, 0xafc3789b8LL, 0x0b42f19f5LL, 0x1dc15caacLL, 0x6acecd7b3LL, 0x135bb74f7LL, 0x3b9e0b46eLL, 0x34af9d0a0LL, 0x76908387aLL, 0xb9384efccLL, 0x4b9ddd694LL, 0x8a078cc87LL, 0xd7aa29528LL, 0x073e5e029LL, 0x24b79a22fLL, 0x3f4ecade8LL, 0xd9063a03aLL, 0xce3d222c9LL, 0x226aba77cLL, 0x93dfd5a57LL, 0xb1bc84419LL, 0xb90e64fa3LL, 0x52f78d5d3LL, 0xbd4c81102LL, 0x2ec19c3ddLL, 0x8352a704cLL, 0xbde1acc9cLL, 0x875f09dcdLL, 0xd49e33eb2LL, 0x5fe39adceLL, 0x638c8b393LL, 0xb099fc09aLL, 0xdc85405d6LL, 0x42ff8a762LL, 0x6ba550e95LL, 0xd9717bbabLL, 0x10894a614LL, 0x5d6501f3dLL, 0x445b9b274LL, 0x89e572013LL, 0x11502f58cLL, 0x533115d66LL, 0x98baecb05LL, 0x95a7280daLL, 0xdfa0d43b6LL, 0xc28b0a96cLL, 0xe7246e31eLL, 0x046bf1146LL, 0x3b52067d1LL, 0x601d23561LL, 0x08dbc6683LL, 0x13d6977d2LL, 0x29cc39a70LL, 0x105f605ebLL, 0x3c4aa4b27LL, 0x736273590LL, 0x985f496feLL, 0xec8ce1bb1LL, 0x9fe2e3666LL, 0x065d2d7f2LL, 0x91d44daecLL, 0xa7c9efd8aLL, 0x3a2f7e452LL, 0x6d9e5c8f6LL, 0xa0db819bcLL, 0xf50919e6fLL, 0x667e3514aLL, 0x9393d0dbaLL, 0x614f4a04aLL, 0x77da17e82LL, 0xc8f3eb90aLL, 0xa4bdfa714LL, 0x967dab7bcLL, 0xf22eddbd7LL, 0x8be64ce29LL, 0xb1dbc12edLL, 0xd6a6de07dLL, 0x690c6c745LL, 0xb5e82406eLL, 0x484db2736LL, 0x46328c061LL, 0xad10489a9LL, 0x9af5500aeLL, 0xd9c271e5dLL, 0xe4bd42facLL, 0x3198fa8d5LL, 0xd9f42b23bLL, 0x7ea66b5dcLL, 0xcf2b134caLL, 0xe1a97e581LL, 0x535052c3aLL, 0x37016e168LL, 0x7cbcfe2e5LL, 0xa1881b075LL, 0x42916ae51LL, 0x884cfafceLL, 0x3bd4b5e61LL, 0x72ec848caLL, 0xb187ed29bLL, 0x279e96e91LL, 0x07434f63eLL, 0xf17f6677eLL, 0x19f373ad3LL, 0x58c095882LL, 0xc315893b1LL, 0x0559e2347LL, 0x38970740dLL, 0xfecee6735LL, 0x2054856bcLL, 0x994d3a8ffLL, 0x9607bcaf6LL, 0xe00168dd2LL, 0x25eeb232dLL, 0x1ece8ab81LL, 0x2ece5cda7LL, 0x82fbf525aLL, 0xb388c80b1LL, 0xf569ae88bLL, 0x8f52d6ebbLL, 0x58d033fecLL, 0xfd827438dLL, 0x3f9513f45LL, 0xf6c5bf39dLL, 0x6491ea0b3LL, 0xebfca762cLL, 0x736764ba5LL, 0xa707fc427LL, 0x44685bc3eLL, 0x3a02b6689LL, 0x5404bb6a8LL, 0xf50e0b484LL, 0x2505b2741LL, 0xb0ae8be19LL, 0xf2c12b9d1LL, 0x46eec3e84LL, 0x053f96a88LL, 0xd5dd1b365LL, 0x2a6e25fd4LL, 0x3911e76e8LL, 0xe1d08a80aLL, 0x39d8cc660LL, 0x3a0a85a3eLL, 0x91e10e4b6LL, 0xf85b58642LL, 0xe2976f782LL, 0x3e16e87bfLL, 0xccd3868c2LL, 0x8f30bc247LL, 0xd51e057a2LL, 0xb839f5136LL, 0x2355cdbddLL, 0xa591a0056LL, 0xfd6828aceLL, 0xca8e761c4LL, 0x435572029LL, 0x7e1631057LL, 0xaa332e971LL, 0xb1b6c88d9LL, 0xaafa138e9LL, 0x92b791b98LL, 0xfd0c856c7LL, 0xe4984a598LL, 0xe121133b1LL, 0xda32a4fe3LL, 0x40dea854dLL, 0xab970e838LL, 0x1d0c29b13LL, 0x454e7da8aLL, 0x039f5068eLL, 0x7c98058d1LL, 0xb37e1af5cLL, 0x6769485abLL, 0x58f740275LL, 0x3fedd95acLL, 0x1f9291d59LL, 0x7b757d552LL, 0x95daf4d2dLL, 0x4930f67e2LL, 0xea3a465beLL, 0xd949a53f7LL, 0x2da8f6c88LL, 0x6117d512cLL, 0x736486e05LL, 0x7e5f57f54LL, 0x689b6f094LL, 0x53093f5b2LL, 0xc107236a6LL, 0x74c097917LL, 0xe28cc262dLL, 0xbaade0e72LL, 0xcd2c4bf29LL, 0x8f260f0f2LL, 0xb79a1c447LL, 0xd2c678b9aLL, 0x5e0bdfab6LL, 0x864e33a2dLL, 0x40f61606cLL, 0x98fe57ec2LL, 0xe5da0f7ebLL, 0x783f9deb3LL, 0x072df5394LL, 0x2f704930bLL, 0x4565eb5a9LL, 0x0f7874274LL, 0x6ea0dd876LL, 0x9e3512377LL, 0xab52d8f50LL, 0x2f14a5f04LL, 0xb6e2d5c39LL, 0x6e1381091LL, 0xe73def6b2LL, 0x93a582d99LL, 0x2d8eab3c9LL, 0x0644f57a8LL, 0xa408c777bLL, 0x0a8311907LL, 0x41cc9974eLL, 0x6a40a6aa3LL, 0xcd43b9a48LL, 0x12cd907e7LL, 0x54ae76fc1LL, 0xb532f10d5LL, 0x6888f2b8aLL, 0xa349b1bb8LL, 0xb2b458244LL, 0x75118dbc9LL, 0xe686a8ea4LL, 0x3e5d3191cLL, 0x6702f804fLL, 0xb3deaf978LL, 0x931ff5969LL, 0xb06778791LL, 0xe81472d94LL, 0x3178f34d6LL, 0x70a987a41LL, 0x5393bdff7LL, 0x8add45e3eLL, 0xf5323996dLL, 0x19fd566fdLL, 0xf1ecbbb64LL, 0x13a413ec9LL, 0xae2268093LL, 0x567d989f9LL, 0xcbff16a55LL, 0x246acb067LL, 0x8b48879afLL, 0x929a68539LL, 0x4d7403f56LL, 0x6aed4015cLL, 0xb82c6a241LL, 0xb10c42a95LL, 0xb4b53305aLL, 0x4ac3b1ce7LL, 0x2722ec68bLL, 0x20c9a9e57LL, 0x493db71acLL, 0xeb3fa52deLL, 0xe8f2c582bLL, 0x32258cb8fLL, 0x89fc15607LL, 0x96213de8aLL, 0x297f6a8a8LL, 0x6bf57cc1cLL, 0xb559fd35eLL, 0xc0868788bLL, 0x09eb07fcdLL, 0xc0e9fa047LL, 0x076c6f13cLL, 0xbf008cd50LL, 0xad1f1cbe8LL, 0x60d890e59LL, 0x98e8fdc18LL, 0xf21b971a2LL, 0x05f612d6bLL, 0x65503574bLL, 0x2e9bd949eLL, 0xed1e65480LL, 0x1cb299f81LL, 0xc164da322LL, 0xfc2599350LL, 0x88c710d7eLL, 0xae27596a8LL, 0xa0adef6c8LL, 0x8b1bbfbe6LL, 0x9a22f3ab6LL, 0xa6481c339LL, 0x4ea34cc9fLL, 0xcb76ab885LL, 0xb0521e4e7LL, 0x3b65cc025LL, 0xbf8b0b795LL, 0xe9297021eLL, 0x70c5e6b75LL, 0x1cca07aa0LL, 0x0e89b8b48LL, 0xee2e712f5LL, 0x0ba7ad4fbLL, 0x21cf08b77LL, 0xeec39ce8fLL, 0x6194c8c7cLL, 0x63544bf4aLL, 0x045d9bd26LL, 0xfd6f2d958LL, 0x5d2cc2af4LL, 0x8cf2b09d3LL, 0xcf68c2d47LL, 0xf218b9402LL, 0x7d5e2031eLL, 0xa2293d0aeLL, 0x819c3c47dLL, 0xaa73bbf8eLL, 0x14c8afabdLL, 0x673e94057LL, 0x6ef3e739dLL, 0x4eca58f28LL, 0x536be7843LL, 0x06c1e92f8LL, 0x44f9df50dLL, 0x4ff4b065cLL, 0x075714e92LL, 0xee4dae1c9LL, 0xb4b7468cfLL, 0xc0dc6f152LL, 0xac42dd9c6LL, 0x799ae449aLL, 0x08ecae137LL, 0x69a2e1629LL, 0xfd32c7425LL, 0xb431b949fLL, 0xfa82751b6LL, 0xe553b7e90LL, 0xd529fbc41LL, 0x457c8eb9dLL, 0xac2892107LL, 0xb034f4e88LL, 0xb7ea481ceLL, 0xd90c74999LL, 0x903d1fdf1LL, 0x73590f785LL, 0xf1250c6c1LL, 0x03a377778LL, 0x751892a53LL, 0x83bc54167LL, 0x8c388c425LL, 0xbf75b14ebLL, 0xc3821426cLL, 0xb5d6f0eaeLL, 0x249bb9f1aLL, 0xe5ac7d4f7LL, 0xdebe0f129LL, 0x5b5fb4931LL, 0x07f9013f6LL, 0x54d4b8d1fLL, 0xeab94d983LL, 0x28f143b98LL, 0x6b6755f0cLL, 0xca0262d29LL, 0x85712a2e0LL, 0x89af4643fLL, 0xa7ef675bdLL, 0xb7285486bLL, 0xc6f66d6b3LL, 0x2dd429ffbLL, 0x9f49452d6LL, 0x396426ce4LL, 0xdeb361fd4LL, 0x4219e7735LL, 0x0debf36ceLL, 0xe4b8d07b6LL, 0xb8402fa95LL, 0x73e0b042aLL, 0xc8d52d855LL, 0x16145793aLL, 0xd15165b13LL, 0x1a842ce77LL, 0x5329c57d0LL, 0x27a1f3a50LL, 0x0abde33e4LL, 0x82309daecLL, 0x396148f44LL, 0xda6a98d20LL, 0x8eeaf1f09LL, 0x6394d9567LL, 0xacc7a08cbLL, 0xd59f203dcLL, 0x9d64c95f4LL, 0x5f9045b9bLL, 0x3f0344f6aLL, 0x1035f53e1LL, 0xd9e50b8f0LL, 0x74635fabaLL, 0x7248393e5LL, 0x25cff4278LL, 0x590d1933eLL, 0x6407ea48dLL, 0xa306c561aLL, 0x1b4064c9aLL, 0x715f397f9LL, 0x6a3f1204dLL, 0x840f5dc8eLL, 0x6350a3c7fLL, 0x12fdb516fLL, 0x9ed847c25LL, 0x38f329633LL, 0x5a470f1dcLL, 0x4ccda51fcLL, 0x666c37a5fLL, 0xc2e44edf2LL, 0xf6e858e30LL, 0x6517f6302LL, 0x53f59bd5dLL, 0xba6fe5ee9LL, 0x25bd77d6eLL, 0xb4dd8862dLL, 0x439a26730LL, 0x85acc62e8LL, 0x56adbd381LL, 0xb3ecb968cLL, 0xb0a73b883LL, 0x7b1d36d0aLL, 0xa3f4b681bLL, 0x02c17ca16LL, 0x98a61167aLL, 0x57ef825d4LL, 0x79436817dLL, 0xd119f0bf5LL, 0x4669b5873LL, 0x0fb5595c6LL, 0x3fad00883LL, 0x3c6782a7aLL, 0xdd70d2856LL, 0x18634ac62LL, 0x235e1bdb1LL, 0x831bb0f4dLL, 0x8edd67014LL, 0x0657f0ad1LL, 0xa41bc2aa4LL, 0xdeaac86f4LL, 0xdbc8bd0a7LL, 0x75b1e56d7LL, 0x51adad8bfLL, 0x5a5b9ef5bLL, 0x6efcffa9cLL, 0xbbd8b73c5LL, 0xf2f085e2eLL, 0x1edbca36aLL, 0x494113d6bLL, 0x1635a8083LL, 0x1ad73699eLL, 0x038d52fa3LL, 0x8b29c98faLL, 0xc5b8cf54aLL, 0x67babd67cLL, 0x76c1f154cLL, 0x7b31c6a89LL, 0x2c6ca1e69LL, 0x25e97560cLL, 0x7a7a8027bLL, 0xae1b17afdLL, 0x5ef280721LL, 0x969f7ad24LL, 0x4e01df55aLL, 0xb47c296e6LL, 0x8d642cea3LL, 0xee4c19773LL, 0x1a375dcafLL, 0x46b7cdd85LL, 0x4e3b67cedLL, 0xa8669f5cdLL, 0x1eaf025a1LL, 0xc9f40d909LL, 0x896f37c41LL, 0xe46154499LL, 0xeb894affaLL, 0x01b0a6676LL, 0xaa3d903baLL, 0x286cffab2LL, 0xd944686d6LL, 0xeb911a3afLL, 0x4029f43d3LL, 0xc111850efLL, 0x5ea39dce4LL, 0x4eab9ae73LL, 0xd083fab30LL, 0xdfc4b7193LL, 0xab6509d4eLL, 0x1d3d977e5LL, 0xc9a52aeccLL, 0x957736e65LL, 0x95daa9621LL, 0xea0841ad4LL, 0xa859146d8LL, 0x0b8de0a5bLL, 0xf252c09b4LL, 0x8172d1273LL, 0xb88a9fcdcLL, 0x6993c1cdeLL, 0xcc96d4c83LL, 0xd1ff48516LL, 0xb1408e507LL, 0x097a8973bLL, 0x7496621e2LL, 0x03b672aa1LL, 0xdc3b03aa2LL, 0xae3498e91LL, 0xcf250c27eLL, 0x6160b091dLL, 0x96c0cb46dLL, 0xced13822cLL, 0x26d97a082LL, 0x8fda2c454LL, 0x9e85bcf1dLL, 0x689845be8LL, 0x06f143755LL, 0x9204f1293LL, 0x1d4a581afLL, 0xbee8d3b25LL, 0x8146094aaLL, 0x349c0af5fLL, 0xb1a8f22d8LL, 0x774ba5a66LL, 0x6ce6004b1LL, 0xf450bda2aLL, 0x930d2dd53LL, 0x68e16cae5LL, 0x8b91631a0LL, 0xb3d3b7117LL, 0x666aa3009LL, 0x5fadee019LL, 0x9de5e422eLL, 0x58bf7fc4bLL, 0xd52f6c075LL, 0x13fc8de24LL, 0xd6eeef343LL, 0x364911d23LL, 0x2d713665eLL, 0xa9e8f1e3dLL, 0x84c231ca6LL, 0xd9e8684afLL, 0xf72feb2d7LL, 0x5ea2d37b9LL, 0xfc66a578cLL, 0x812110a96LL, 0x936dc276fLL, 0x84fbba439LL, 0x17f67469bLL, 0xb7481055eLL, 0x96ecc8d0bLL, 0x8d7fc1aacLL, 0x0ec2f5bcfLL, 0xc24ab0a62LL, 0xd1839dd10LL, 0x8fd470914LL, 0x6cfa90230LL, 0x205091ce5LL, 0xdaf874324LL, 0xf15958133LL, 0x1158fc57fLL, 0xf0cbfb94eLL, 0xca17718c7LL, 0x18b2ac4beLL, 0xa92ecd88fLL, 0x228af528eLL, 0x774db92dbLL, 0x21fd98aa9LL, 0x8e7582062LL, 0xbb59648f4LL, 0xfd9dbd88aLL, 0x389035c96LL, 0x1114c6c97LL, 0xf49428de7LL, 0x669e6fc5cLL, 0x1dd6ea469LL, 0x882bddf98LL, 0xfed7b3728LL, 0x25309a3a8LL, 0xb04c1729bLL, 0xeb3e8f6a7LL, 0x7468cfeeeLL, 0x1a1b7d99aLL, 0x3eb4e134cLL, 0x43566fc67LL, 0xb1e97f8f5LL, 0xdb562afa0LL, 0xedd496057LL, 0x5ccb184a1LL, 0xe3da32613LL, 0x25ecd21cbLL, 0xdedd008f1LL, 0x45ec76617LL, 0x73fab05ddLL, 0xb2645fbd0LL, 0x78cdf82d6LL, 0x66e4b8db9LL, 0xc83017e45LL, 0x86b2a3e27LL, 0xc6dbd66e8LL, 0x00b395b2aLL, 0x1c11ab65bLL, 0x888994c14LL, 0x111ea98c1LL, 0xae86d848dLL, 0x54c6e271eLL, 0xb13ef9ab1LL, 0x2ed93d60fLL, 0xa4efe7205LL, 0xe70286dbdLL, 0xd2cc67dedLL, 0x2b0663021LL, 0x95f851a9fLL, 0xf67ccbbb3LL, 0x267c42225LL, 0x2be4b5ab8LL, 0x8a7fc28d5LL, 0x0e418f889LL, 0x1a34fed2eLL, 0x65590273eLL, 0x574a6cbc4LL, 0x3e0f4cb1dLL, 0xb7cee6cd8LL, 0x5627e4845LL, 0xadfe6d2bdLL, 0x568b57001LL, 0xd4ec7fad7LL, 0xad3f576faLL, 0x497d2eb92LL, 0x0fe6c7298LL, 0x16161faa3LL, 0x16799225fLL, 0xf30a85fe1LL, 0x52f9d455bLL, 0x130a2a42dLL, 0x598c9f522LL, 0x78c55e9f6LL, 0x7646c5b65LL, 0x7a53288e6LL, 0xb2c707e61LL, 0x134b81f75LL, 0x6d76b9855LL, 0x78a343d82LL, 0x2e4df469fLL, 0xf89236748LL, 0xf57e71d1dLL, 0x10dc8784eLL, 0x70fd8f1a6LL, 0x937bcc483LL, 0xf4c72b50fLL, 0x10ba6cbdaLL, 0x077f1ed59LL, 0x66d941739LL, 0x04ceccaeaLL, 0x015795903LL, 0xe53a6a20fLL, 0x3999bbaa0LL, 0x1281bf25dLL, 0x7337f274fLL, 0xcb40345a5LL, 0x68d24d19aLL, 0xd2fd56ca0LL, 0x0d30b94e9LL, 0xbb57cfe9eLL, 0xd74b11569LL, 0xe2db0e252LL, 0x5fe026216LL, 0xb47130e85LL, 0xfe076a9a5LL, 0x9415e9632LL, 0x3367854f5LL, 0x50af0831dLL, 0x49f25332dLL, 0x7739a837bLL, 0x099f36a43LL, 0xc8537be03LL, 0x3e06b323dLL, 0xf9a733bd2LL, 0x312a441acLL, 0x335508febLL, 0x7db227a83LL, 0x3ebb1bcabLL, 0x7a1106873LL, 0x5486d3f20LL, 0xa1628b849LL, 0xb64543ed2LL, 0xe293fabcaLL, 0xd1d512de1LL, 0x47884a21bLL, 0x4a406b83fLL, 0x7aa35466dLL, 0x6542ddf69LL, 0x348c210e9LL, 0x2aed60aacLL, 0xe9d35f24aLL, 0x2bbc14dd9LL, 0xf2891fc9bLL, 0x72a9cba3fLL, 0x409ecd462LL, 0x712ba02b9LL, 0x1b149ab0fLL, 0xaf6b65883LL, 0xf68306512LL, 0x20e84ff13LL, 0x88beaabb1LL, 0x7456d2803LL, 0xa04216d3fLL, 0x83f33226dLL, 0x394a0eb38LL, 0x65670c452LL, 0xa5903ed13LL, 0x473689a3eLL, 0x142b1dd56LL, 0x53bf24a7dLL, 0x88585a655LL, 0xe62c824faLL, 0x1bc6257ddLL, 0x266d228daLL, 0x9d18f806aLL, 0x26a6ab06dLL, 0xb90c39735LL, 0xd844f8c09LL, 0x4ebf14fbbLL, 0x5c720772eLL, 0xe033d46e2LL, 0x2d72fe7c7LL, 0x11878c4b1LL, 0xa075e3992LL, 0xf4dd045d8LL, 0x9caad8759LL, 0xf5b387cbaLL, 0x20dfb6633LL, 0xe7511e0eeLL, 0xc3e9e1225LL, 0xf6d332299LL, 0x43aee9bc2LL, 0x4882318bbLL, 0x958fa25c2LL, 0x6c9dd71f2LL, 0xda71d12bdLL, 0xead515c9fLL, 0x5622a7b24LL, 0xb1a220b61LL, 0x5e3b6d626LL, 0x95533c08fLL, 0xaf093c411LL, 0x364a0f961LL, 0x6dc550b86LL, 0x43cb48cf6LL, 0x01331bd0eLL, 0x55926d59fLL, 0xddfd98223LL, 0x509d0ac32LL, 0xdd3e82660LL, 0x72f15dee6LL, 0x114a5ba53LL, 0x70d637811LL, 0xbf7172408LL, 0xca9dfc935LL, 0x2b082b18aLL, 0x9e227de74LL, 0x084d8797aLL, 0xd91cc5635LL, 0xd9b9c0584LL, 0xe6a5cdd7fLL, 0x606567f3aLL, 0x30121d876LL, 0xed40680fbLL, 0x9e498a0fdLL, 0xf6e6f7aedLL, 0x7bd31c1d5LL, 0x7fadc5b78LL, 0x430399853LL, 0xd6937f64fLL, 0xe9755cec2LL, 0x336f0919eLL, 0x62d953c76LL, 0x6eccc311bLL, 0xa7725ba74LL, 0x6b2ba1f0bLL, 0xd9c680f4eLL, 0x0b4bf20fbLL, 0x7d247fb92LL, 0xe2dfb415bLL, 0xaa41eabb7LL, 0xdde282439LL, 0x19cd98b9bLL, 0xc169b393eLL, 0x7057af26bLL, 0x621760313LL, 0x091e4f51cLL, 0x712e3294dLL, 0xba60f9cb1LL, 0xcac43e693LL, 0x49ba92d03LL, 0x3ec790f69LL, 0x6b4fd03f4LL, 0x1753f131fLL, 0x140e73516LL, 0xb235b7ca5LL, 0x1d27a6723LL, 0x3102222ecLL, 0xc0b75e745LL, 0x3a1b554f9LL, 0xc313dc962LL, 0xa323d6c80LL, 0xccca0aabeLL, 0xa61580a37LL, 0xea4b16079LL, 0x6b2ad79e0LL, 0xb766b661aLL, 0xe22f727d7LL, 0xe4304d5edLL, 0x24376523aLL, 0xf853f00b3LL, 0x6592be5e4LL, 0x553748fb7LL, 0x6852aefbdLL, 0x73e2abca6LL, 0x5fb45c08bLL, 0x635d4c650LL, 0x0e0d2be1eLL, 0x60ee51f29LL, 0x38ddb7390LL, 0xa300f1ae1LL, 0x22efe44a7LL, 0x628bba583LL, 0x4b17ecb5fLL, 0x55c6b9011LL, 0xe1a14bac7LL, 0xe427b3d0dLL, 0x46100dce8LL, 0x04cc2245dLL, 0x84c2e41d8LL, 0x0919dcd26LL, 0x8d1d01822LL, 0x2e26515feLL, 0xf7ab7dae4LL, 0x7768b70ccLL, 0xe94913f18LL, 0x0f3e883dcLL, 0x3b8d3f0d4LL, 0xd67ea41c4LL, 0x99d477e9fLL, 0x3fb10f974LL, 0x02a370e93LL, 0x17768adb2LL, 0xd6329f527LL, 0x3bbc1a712LL, 0x1b052fab8LL, 0x1a4619ef5LL, 0x00b725dfcLL, 0x78f0c547cLL, 0xadbbb4432LL, 0xe1cf5cbdaLL, 0xd70644e69LL, 0xab4cb9d0bLL, 0x5fcd12ef4LL, 0xe396af25dLL, 0x2d9ff9ca3LL, 0x4ef3df84cLL, 0xc7ec94a8fLL, 0x6b18da2f5LL, 0xa63d0badfLL, 0xf91662835LL, 0x3410a9ff6LL, 0x7ed13b24aLL, 0xf95f89732LL, 0xce74b2901LL, 0x695679287LL, 0x4843eb226LL, 0x5a66b2ed6LL, 0xa86cc1f33LL, 0x16d5e7b98LL, 0x748025a14LL, 0x3397dd590LL, 0x7bafeb92aLL, 0xb45584283LL, 0xd54dc6a25LL, 0xaf2e68538LL, 0x220f32a8fLL, 0x44f8b18ddLL, 0x51814c91cLL, 0x8bb4af165LL, 0xbcd6adb56LL, 0x82e4a2e55LL, 0xcd4990ca2LL, 0xef32a23e5LL, 0x688ec9de4LL, 0x2aba4638bLL, 0x16f7382e1LL, 0xbdfe274eaLL, 0x2164acc4bLL, 0xaad044fdaLL, 0x64559f29aLL, 0x5a9c9339eLL, 0x6a0739a2aLL, 0x8f993b732LL, 0xa90de3f6cLL, 0xace88d90fLL, 0x36a7f9cf0LL, 0xdbef65c2bLL, 0x69bb34d8dLL, 0x6f095cd61LL, 0x3bdbd6405LL, 0x34600b852LL, 0x0d1e24fe6LL, 0xcc994f31eLL, 0xccfcc1adaLL, 0x106b721a4LL, 0xad3e751d6LL, 0xab0903242LL, 0xb60ba3746LL, 0xc5b7a191aLL, 0xc7da973a4LL, 0xf45b0747aLL, 0xba92e67a2LL, 0xb93ea5045LL, 0x011d2ac4cLL, 0x564361455LL, 0x420d42485LL, 0x47e0f7889LL, 0x110a80968LL, 0x7a3cec118LL, 0xd058e2ed7LL, 0x1dc9c639aLL, 0x42de0a027LL, 0xe5530907fLL, 0xaffabd8e4LL, 0xb08fe947eLL, 0x5ba33b408LL, 0xd5f80115dLL, 0xe6298c761LL, 0x893b86708LL, 0x85b1d3017LL, 0xf2ac6bc60LL, 0xfa9378384LL, 0x3f538c40bLL, 0x21b0661dcLL, 0xe762b80d4LL, 0x16f6ecbd5LL, 0x1264fca24LL, 0xf39f4e476LL, 0x51cc3b982LL, 0x757c9f748LL, 0x311d200ddLL, 0x2563393cbLL, 0x1b05631cbLL, 0x9a547cea2LL, 0x35fd287a0LL, 0x25460fd6cLL, 0x8d4e23de8LL, 0xd56e01537LL, 0x4714d5bf0LL, 0xce7f93169LL, 0xf16142c02LL, 0xf88939763LL, 0x42b49ee1dLL, 0x8495855f7LL, 0xade7e53e3LL, 0xea99e0abdLL, 0xd5a4abf2aLL, 0xe8868979dLL, 0xde8c25d59LL, 0x24796f2b4LL, 0x588d17a5cLL, 0x23446aa2bLL, 0xd7930a836LL, 0xa1ad628b6LL, 0x8502da9ddLL, 0x1abd85618LL, 0x044a252ffLL, 0xd33f9442dLL, 0x0711e508dLL, 0xb8548f822LL, 0x9298a3af1LL, 0x397d78086LL, 0x26d522fa6LL, 0x744606469LL, 0xab2c1baf4LL, 0x24568a115LL, 0x233bd114bLL, 0x68f7612c8LL, 0x391ef6edfLL, 0x03c6ab744LL, 0xe248dbb72LL, 0x7511dc9f6LL, 0x53309a668LL, 0x5438d7128LL, 0x57e1c76edLL, 0x61c9aec27LL, 0x4b1219026LL, 0x6d5e9cf25LL, 0x2242688caLL, 0xaa10985ffLL, 0x8454ac8ceLL, 0x23a648791LL, 0xb8d396be7LL, 0x482d2fc39LL, 0x62fde8f85LL, 0x84bd1069fLL, 0x1aa1a5303LL, 0x434f3adebLL, 0xc99f3f39aLL, 0x744f1eb68LL, 0x9cd2ca627LL, 0x472428c4eLL, 0xdbacacda0LL, 0x310c6bd3cLL, 0xa84d6d066LL, 0x6ba340d41LL, 0x7aaa74c11LL, 0xe21d5d0f3LL, 0x8e84f07daLL, 0x3b260c654LL, 0xdb704686dLL, 0xf197a1ee9LL, 0xcb4e599d3LL, 0x2c67ff681LL, 0xedaa7c03cLL, 0xb859ffc56LL, 0x320eeccbcLL, 0x8014fbd19LL, 0x32e57039eLL, 0x5e196e0ccLL, 0xa02c0dc84LL, 0x29f06b67aLL, 0xe48da0b64LL, 0x47f4262c5LL, 0x3abc13e2dLL, 0x57afc2c03LL, 0x9437d42b4LL, 0x57f8e9b00LL, 0xd0644255eLL, 0xb9f0e2245LL, 0xf68892060LL, 0x11b4ee7b3LL, 0x26f34a243LL, 0xd92e92d2eLL, 0xc3c36f4d5LL, 0x057a6bc86LL, 0xc1e1d1593LL, 0xf36f11af5LL, 0x926cd9966LL, 0x66fc75705LL, 0x6cc85b754LL, 0x373e56bdbLL, 0x9a7322f5eLL, 0x948cf1650LL, 0x11f57bdd0LL, 0x662314283LL, 0x8e2901cc7LL, 0x9a5dc8cb4LL, 0x3b6718a90LL, 0xfa4d1722eLL, 0x19d6cc9b4LL, 0x0fa2e07ddLL, 0xe0483446fLL, 0xcee973997LL, 0xcab326bedLL, 0x8653a7582LL, 0x07d8331edLL, 0x916db55a5LL, 0xac0cb5513LL, 0x445d66afeLL, 0x265e9d4c8LL, 0x543b1e0b0LL, 0xfc6c649edLL, 0x02a38c5adLL, 0xae31be812LL, 0x46f85eac3LL, 0x38b531dcbLL, 0x046cf24a5LL, 0xf77637b55LL, 0xf07d1c632LL, 0xe05b2f798LL, 0x10d5860e5LL, 0xe42b2bfe6LL, 0xbfc381a12LL, 0xad645382fLL, 0x762a6a152LL, 0x58336fed1LL, 0x5c88f9b4fLL, 0x64b15ddbbLL, 0x9cd938699LL, 0xf3c6c1525LL, 0x2362c53dbLL, 0xd40874c21LL, 0x2fc1763f1LL, 0xfbf6f4b46LL, 0xe9fb3911fLL, 0x692346b6dLL, 0x52e96efe7LL, 0xbe0d16e43LL, 0xcd144ad13LL, 0xd497e4c7bLL, 0x280b58b80LL, 0x443ff1d93LL, 0x2f528c5b5LL, 0x3ef6bb3d4LL, 0xb806de136LL, 0x9600a3394LL, 0x0bdae6a57LL, 0x7ac6bbd4cLL, 0x17364c5c2LL, 0xec5344b46LL, 0x0bf745b8bLL, 0x72718fd17LL, 0xe2fdab406LL, 0x7cc4b8dc2LL, 0x4f5379d4bLL, 0xde49a05e1LL, 0x5e9c05763LL, 0xed58a3866LL, 0x80535dac8LL, 0x6affa7d8eLL, 0x4e7f09edeLL, 0xee0a2e534LL, 0x7519486a6LL, 0xf9b9680f1LL, 0xabbb28449LL, 0x5a2396946LL, 0x7fd4d5522LL, 0x542319779LL, 0x3caf4bd55LL, 0xfd740a695LL, 0x68c96b8cfLL, 0xab1593c1aLL, 0x42b1dc797LL, 0x4acaa2299LL, 0x2e27e9775LL, 0x4f157edc2LL, 0xb068ad203LL, 0x437ad4f84LL, 0xb8e9d6ad6LL, 0xa6439526bLL, 0xbfe227aceLL, 0xd32f46eb2LL, 0x404bfa76fLL, 0xb7beb4e77LL, 0x1874e8369LL, 0x879d23b91LL, 0xd9bf340d9LL, 0x714131397LL, 0x16a00adf1LL, 0xb9b9d414dLL, 0x2631bd706LL, 0xa2a978ee5LL, 0xe8fa34bfcLL, 0xaf9d55a95LL, 0x70a649cbdLL, 0x754fa798dLL, 0x59cf771e8LL, 0x6814b7755LL, 0x36efdafc4LL, 0x305a3225dLL, 0xadf19801bLL, 0xa38bf2a66LL, 0x44de6973fLL, 0x62f4a0894LL, 0xcf4a6f1d9LL, 0x0b358593bLL, 0x18583db29LL, 0x2891984e2LL, 0x7702e90b0LL, 0x91a1e901eLL, 0x64ff5e2d4LL, 0xbf9da6adaLL, 0x02e26d17bLL, 0xf89c04a9aLL, 0x4b43a2412LL, 0x31de98342LL, 0x28c287395LL, 0xc3362e40aLL, 0xe5bc3aa9cLL, 0x9e48f6a06LL, 0x733435bacLL, 0xa43be8cdeLL, 0x5c653248cLL, 0xe2931bdc7LL, 0x02c4795f1LL, 0x62bb96f20LL, 0xb2f43a171LL, 0x19d1f6ab9LL, 0x433ea2164LL, 0x137da589aLL, 0x28fd58e72LL, 0x202d4cb62LL, 0xf03d74c5aLL, 0xf6112a05eLL, 0x1527ce8beLL, 0xd27556017LL, 0xf1bdb3c55LL, 0xc081699a5LL, 0xd8368ef11LL, 0xa1ed747d5LL, 0xd833b1171LL, 0x2e161f79dLL, 0xf2d7a26f4LL, 0xffb902d9aLL, 0x6c62a5731LL, 0x914cff395LL, 0x849a7a32dLL, 0x787d20e5fLL, 0x873080cddLL, 0x902f6862bLL, 0xa2ef2b22aLL, 0xbc6ba2e19LL, 0x27fa8c7e6LL, 0x3b7964fa8LL, 0x99d80b892LL, 0xa9d7ddab8LL, 0x0e6dabf62LL, 0x19b1a3faeLL, 0xdabcaba4bLL, 0x465086a2dLL, 0xaf9290947LL, 0x3f16139c2LL, 0xf5eb1ba13LL, 0x5be4c9d84LL, 0xb491d5edeLL, 0xecb1e1407LL, 0xe2bf4cd78LL, 0xe9ba7bb10LL, 0xbe6954783LL, 0xd3a7b0213LL, 0x50de815b5LL, 0xddc150b2bLL, 0xf83e35e25LL, 0x06bcfeb25LL, 0xd446be9d7LL, 0xb8f07825bLL, 0x0938b9c16LL, 0x5dc6e6ae5LL, 0x3d8adc166LL, 0x6deae71f4LL, 0x748581570LL, 0xf2d14fd9cLL, 0xde56fb4e4LL, 0x23852ee7cLL, 0x7663771e7LL, 0xac04e987fLL, 0x10c77faf2LL, 0x84f4bc3f1LL, 0xb54fd5e6aLL, 0x5b315ef54LL, 0x60cb8bbc5LL, 0xbd8cc9e55LL, 0xab5f55050LL, 0xdddd64553LL, 0x6cd38ade9LL, 0xf0f5ec7b9LL, 0x96da53643LL, 0x3bb39fc6dLL, 0x2b2884ad7LL, 0xaa26a84fcLL, 0x8a2bf56c5LL, 0xb68a4ab27LL, 0x4962ea115LL, 0xd4f9472e3LL, 0x975e4c01dLL, 0xa82cd2570LL, 0xb6433749fLL, 0x7234ae0e6LL, 0x14cbc7db2LL, 0x22d77fb8cLL, 0xac098f788LL, 0x7762cecbbLL, 0x2e715f49fLL, 0xa529f55f9LL, 0x935054846LL, 0x999c0c185LL, 0xf6a94f0b2LL, 0x6bdec8471LL, 0x290d12cf6LL, 0x73b35868bLL, 0x5e9c08e84LL, 0x35d427addLL, 0x4decbf805LL, 0xb1218bb88LL, 0xcf15a8069LL, 0x44d59fe6dLL, 0x48cf866e4LL, 0xd85fca129LL, 0xa81c1e1cfLL, 0x25be310e2LL, 0xe57ab2f62LL, 0x22c1da1d6LL, 0x6a2a71117LL, 0xd60d2eb36LL, 0x3f3f9a2e6LL, 0x6d1c1aeceLL, 0x9b831a4fbLL, 0xd64ba88deLL, 0x968db7b8eLL, 0x073910151LL, 0x3e5d9f584LL, 0x8fdae57c8LL, 0xb0cb58bb5LL, 0x8bc3d58f2LL, 0x01188bb85LL, 0x226c7172eLL, 0xed7f67b04LL, 0x63c7caad8LL, 0xf6768009dLL, 0x06e193e34LL, 0x2021a54f0LL, 0xd4437d532LL, 0x82ce066a3LL, 0x260bfdee8LL, 0x26b0c81ecLL, 0x081503c67LL, 0x18be917a6LL, 0x3aac944feLL, 0x5a91ed08bLL, 0xd93c3ca5eLL, 0x4ba9f608fLL, 0xf9d10ca44LL, 0x314c4dc69LL, 0x8acfdd4a5LL, 0x69e73946dLL, 0xa103f94ebLL, 0xe708b0565LL, 0x4ebcf058fLL, 0x3385e6ce7LL, 0xdbbc1ec39LL, 0xd13c2ddc5LL, 0x3df56eec6LL, 0x50fe589c2LL, 0x5e7f91d57LL, 0x411dc3670LL, 0xa1785375bLL, 0x8d710fdc9LL, 0xe767b052bLL, 0x950939ab0LL, 0x7b7767c17LL, 0x5d24ca58fLL, 0x409e70b9fLL, 0x02d1bc4fbLL, 0xddec53eacLL, 0xa05158be6LL, 0xb64eca239LL, 0x88794e4dbLL, 0x815926d2fLL, 0xa109e873eLL, 0x87b47cdd8LL, 0xc48d8473bLL, 0x4a2e11891LL, 0x355819bd2LL, 0x23f9590faLL, 0x95a7fcb68LL, 0x325115b71LL, 0x42d0b9687LL, 0xc63bd4849LL, 0xe6d873be4LL, 0xf48b66357LL, 0xb29fd2a28LL, 0x5cafd9507LL, 0x09b467b3dLL, 0xe2dae50a2LL, 0x0ba7b7a5eLL, 0x4b2172ec6LL, 0x49795d717LL, 0x6c9495943LL, 0xd257992c1LL, 0x96d7c46d0LL, 0x0266ae09dLL, 0x241b28662LL, 0xc1ee98d9fLL, 0x50132d568LL, 0x07d90455aLL, 0xc2e459355LL, 0x42bdf1a71LL, 0xc662953c6LL, 0x4cfdd40e7LL, 0xd5fe2a905LL, 0x5339ed2bcLL, 0xc9603561cLL, 0x47c64f707LL, 0x8110b2364LL, 0x1b3b324dcLL, 0xc665f1f85LL, 0x74299617eLL, 0x4216c87ecLL, 0xc675906efLL, 0x2dcb4f572LL, 0x6fd55584aLL, 0xa0c4d0932LL, 0x19ab0966bLL, 0x883e192f9LL, 0x5fa021f7bLL, 0x266d2ce3dLL, 0xe90bba30aLL, 0x782bcabc9LL, 0x6d40981e4LL, 0x5b98b080fLL, 0xc21fbb365LL, 0xc574d78d8LL, 0xb2b236f39LL, 0x80cb66e45LL, 0x669c9a05dLL, 0xbd6808275LL, 0x21f8e510aLL, 0x437ba62f1LL, 0xc5224cbd0LL, 0xcb441a4e6LL, 0x59aa067f7LL, 0xa9d51eb53LL, 0xdc294402dLL, 0x33abf8a53LL, 0x2bc9dd944LL, 0xc3f8742bbLL, 0xc72b75bbaLL, 0x17cf5a97cLL, 0x08f42434aLL, 0xec593abdbLL, 0x120a797b7LL, 0x6cdc8ec10LL, 0xd0a686b2dLL, 0x81a4fb9daLL, 0x44facf6b5LL, 0xbec238c25LL, 0x6c12019cdLL, 0x1aac292a8LL, 0xe88bd0a21LL, 0x959a41c81LL, 0x432c2ca9cLL, 0x6c0b7b962LL, 0x385867bd6LL, 0x8068a6bbbLL, 0x09a777181LL, 0xc27d59fe8LL, 0xa04074853LL, 0xa50631657LL, 0xba7b37adaLL, 0xd9872f1e5LL, 0x949f449aaLL, 0xd2aa72df6LL, 0xbcc9608d7LL, 0x93d9a8d7cLL, 0x5e5495818LL, 0xbc9bce5e3LL, 0xc3875ec11LL, 0x97ea32be7LL, 0x41c5a2548LL, 0xe64df88c0LL, 0x2e21d1372LL, 0x0630736adLL, 0x226cdbc75LL, 0x609746f95LL, 0xe7cd6d390LL, 0x3ee9d185aLL, 0xd8cd3e34aLL, 0xab785e407LL, 0x93addebf1LL, 0x854b75025LL, 0x26f1bfd50LL, 0x598394aa7LL, 0xdfdb6840bLL, 0x6ea7a4c78LL, 0x6b4faa965LL, 0xa7aaf424dLL, 0xc694d0ce4LL, 0xfdab0acf7LL, 0x582738889LL, 0xb30ea7f8cLL, 0xc1af8b900LL, 0xe26889dcfLL, 0x5ffd11dedLL, 0xf6b05ad7eLL, 0x35cb9503fLL, 0x8b86f73e2LL, 0x8d467a6b0LL, 0x91f7a7735LL, 0x214693632LL, 0x106d5ff8aLL, 0xb9cdef610LL, 0x3dfafe135LL, 0x0fa5b0ae7LL, 0x26cd7986eLL, 0xc6fd681c8LL, 0x6b4188c58LL, 0xa3ba597e8LL, 0x85f518945LL, 0xf365a0254LL, 0x2048b85bbLL, 0x0f3dcbb35LL, 0xd3e5031cdLL, 0x54de45ec8LL, 0x2af3dc7a2LL, 0xe08932e15LL, 0x7734ac9f1LL, 0x322d852e2LL, 0xcdbdf8b96LL, 0x806696a67LL, 0x8cd4e61e7LL, 0xe057f8856LL, 0x4d9e9613cLL, 0xb82f25c73LL, 0x93c8c49e9LL, 0x7aace1816LL, 0x646f619c5LL, 0x6aae5893aLL, 0x84d48be42LL, 0xe4a98eafdLL, 0xd7f42bcf5LL, 0xa8017604dLL, 0xcd715d0e1LL, 0x11abe3d38LL, 0xacd8e4e30LL, 0x623790ab0LL, 0x39f25ed99LL, 0x8aef091c2LL, 0x7a2574284LL, 0x108cb8578LL, 0xd7a2ea337LL, 0xf13e9edfaLL, 0x5a80a8d14LL, 0x748f6e6fdLL, 0x1e4ba118aLL, 0x546ce5112LL, 0x1703a322aLL, 0x894c63e47LL, 0x3f403b661LL, 0xed4d06757LL, 0x5b9902d5dLL, 0xdf86cd5afLL, 0x1ba88e704LL, 0x4f148ee08LL, 0x1eb0dbaafLL, 0xb377d99a9LL, 0xf480a804bLL, 0x477b4f4eaLL, 0x0a9c8c047LL, 0x7e4c0a8cbLL, 0x4a89583d5LL, 0x6fc78608bLL, 0x7b59965e9LL, 0x7dcaa4585LL, 0x4002e14f6LL, 0x14c436673LL, 0x98ed9cecdLL, 0x092ad5b7fLL, 0x19088d131LL, 0xdec2ae3deLL, 0x3ac6ea320LL, 0x4352c0d48LL, 0x47ad3bfdbLL, 0x469a523c6LL, 0xf529ccb4cLL, 0xf663c29eaLL, 0x9a225604aLL, 0xb71dd41d5LL, 0xee5201d72LL, 0xd06cb9ad0LL, 0x91b427aa0LL, 0x7ef2d044bLL, 0x61bfdd3a2LL, 0x33a4184e7LL, 0xc875c3536LL, 0x5c5e6e999LL, 0x8fb237053LL, 0x2143558aeLL, 0xe77b34bd6LL, 0xe5b1047b3LL, 0x729ad8bb3LL, 0x9d2efdbf2LL, 0x6a5d1a69dLL, 0xeab0c8b69LL, 0x24ced1108LL, 0x9488901ffLL, 0xbf910f4aeLL, 0xf89e416adLL, 0x9eb3973eaLL, 0x9fac35740LL, 0x1fc51212fLL, 0x2a106be25LL, 0xcd5bee55fLL, 0x4b78e174dLL, 0x231194dc2LL, 0xf094cec09LL, 0xbc94ebcb5LL, 0x9980b8913LL, 0xe6cca33c2LL, 0x4bcdb33ddLL, 0x1a6f4e4b9LL, 0x097ead2f2LL, 0x6f4980025LL, 0x7b9d83ee6LL, 0x61ab1d631LL, 0x522dfaa9bLL, 0xd58e68b08LL, 0x6dd894088LL, 0xa43851db8LL, 0xdb3da4317LL, 0xf1d8108b9LL, 0x0257b43cfLL, 0xa36dc4b75LL, 0x586b118aeLL, 0x4d292d0d7LL, 0xd5dd7ec58LL, 0x2961da611LL, 0x8e1eb4d44LL, 0xbdaf4157aLL, 0x783587d64LL, 0xb89073a03LL, 0xf2a98a98dLL, 0x8e6e953bfLL, 0x82728c63aLL, 0x08cf515b3LL, 0x192019a8bLL, 0x199feb37bLL, 0x4747d4de5LL, 0xbad74c26aLL, 0xa3eb1f4ebLL, 0x3a3fe72d5LL, 0xa24fca706LL, 0xac295c820LL, 0xdf8707b04LL, 0x240d933f8LL, 0x40d3afedaLL, 0x08967b352LL, 0xdb5908f2eLL, 0x0eb77e73dLL, 0x55e3af14bLL, 0x7e2af46d7LL, 0x2ad8069f0LL, 0x1c94d9cf8LL, 0xad3fd6707LL, 0xd527d05c1LL, 0x0e9661707LL, 0x5434c8225LL, 0x8b17ffb10LL, 0x6f4bca6ceLL, 0xe3d99b9e9LL, 0xb3f192e96LL, 0x4fe4ae9dbLL, 0x3e986a40dLL, 0xabebc86bdLL, 0xacec35dc8LL, 0xbff455399LL, 0x34ea8a485LL, 0x5c603d944LL, 0xf9228c5d5LL, 0x630b74068LL, 0xcc087e16fLL, 0xdcded3a77LL, 0x18e36b56dLL, 0x745c5e53fLL, 0xe54b21ebfLL, 0x8d71bb6a7LL, 0x804669bd9LL, 0x78a8843b2LL, 0xa841aa4c8LL, 0x7c1721cb9LL, 0x9a83406d5LL, 0xa896c7864LL, 0x04bb0aba5LL, 0x3c11533b6LL, 0x328444d58LL, 0x39bbda023LL, 0x8b36424c7LL, 0xf6120c770LL, 0x185bb28cfLL, 0x28319aaccLL, 0x44125fc8dLL, 0x9bd0b06bbLL, 0x77e3e63c2LL, 0x92cc0d22dLL, 0x6471ce5caLL, 0xa5cd27974LL, 0x821216a59LL, 0xb44bf0674LL, 0x91c306242LL, 0x59ddcc7f6LL, 0x8a49cdd23LL, 0x4126a5129LL, 0x5dae47e63LL, 0x5ae91a769LL, 0x1167709c8LL, 0xf72905476LL, 0x7f714af5bLL, 0xe1156f64eLL, 0xf043af7a7LL, 0xda86cb771LL, 0xefe9556eaLL, 0xe93191d0fLL, 0xc57680df4LL, 0xd6731845aLL, 0x40a0357d5LL, 0x369846e9cLL, 0x55c37b47bLL, 0xc1a35b0faLL, 0x53ac7be90LL, 0x85eb470c0LL, 0x3181486daLL, 0xf0e227153LL, 0x324271b12LL, 0x87850744fLL, 0x2e2d754b1LL, 0xe6eb202ceLL, 0xb0704c7b4LL, 0xa67db8125LL, 0x711fb0e4aLL, 0x37c2d1ce3LL, 0x6aea9caffLL, 0x443334cd8LL, 0x69e4738b4LL, 0x0d0afd5daLL, 0x9ab6c533dLL, 0xcca49a2bbLL, 0xc4d4fb6b6LL, 0x0e4b2ddd7LL, 0xda2509125LL, 0xe9c86da19LL, 0xc7274b59dLL, 0x2f4bbe74dLL, 0x96ac2a725LL, 0x1f30d673dLL, 0x425fd41bdLL, 0xa575e248bLL, 0xa41df8a63LL, 0x850967a78LL, 0x580a6f3fcLL, 0x3e7fa23edLL, 0x77d9a37b4LL, 0xaac2f4828LL, 0xd5afdeeceLL, 0x463bfa5bdLL, 0x8d8c594b4LL, 0xd5c3a4722LL, 0xc26bd81e9LL, 0xa25f27cc7LL, 0xb0a959849LL, 0xed63242d8LL, 0xf2a5d4c2cLL, 0xe55b4628aLL, 0x0b2dd55afLL, 0xc8738d953LL, 0x8a7cef286LL, 0xcc7c48409LL, 0x043816c4bLL, 0x85a74860cLL, 0x48fc51dbcLL, 0xee8237a9fLL, 0xdb101fca7LL, 0xa22651a66LL, 0xe15eb5386LL, 0xd43085b18LL, 0x651931da4LL, 0x9326a183fLL, 0x6f8dab598LL, 0x0e51eac76LL, 0x6e2623406LL, 0x9f935c569LL, 0x99d04350dLL, 0x2bfb7ef17LL, 0x9408845a8LL, 0x36e8c5171LL, 0x8af933fc5LL, 0x36ecec25bLL, 0xc90ec3e5aLL, 0x84942ea05LL, 0xfb5493f14LL, 0xd81204353LL, 0xd5060ecddLL, 0xddfda6095LL, 0x5cafc4e1dLL, 0x7754ff6eaLL, 0xc257672a5LL, 0x27299bc82LL, 0x4ca1520cbLL, 0xe1de3ec8bLL, 0xfb5ab687aLL, 0x88f4cc5feLL, 0x5c83fac92LL, 0x18bdce2abLL, 0x87fa99852LL, 0xeafdac7f7LL, 0xa095e0c0aLL, 0x1de9110e0LL, 0xabee689d5LL, 0x917d79b7aLL, 0xdb13b369aLL, 0x56aa3e642LL, 0xaaaf35e16LL, 0x9c0896962LL, 0x32ed98cd1LL, 0x40befddedLL, 0x733f9f984LL, 0xab7ee7d81LL, 0x38e744727LL, 0x7501364c7LL, 0xfd3421d02LL, 0x8d492864cLL, 0xb25d6c2d9LL, 0xb88708fa4LL, 0x66ca7ea8dLL, 0x53b061dd1LL, 0xbfe629317LL, 0xe48ccc4b2LL, 0x529e75dfaLL, 0x02ce3697aLL, 0xbe76866c4LL, 0xce03c67e3LL, 0x3c79ace12LL, 0xabb5ade8eLL, 0x5510819d4LL, 0x1853d91a5LL, 0xd99655b60LL, 0x47729ddffLL, 0x3c8c5ca2fLL, 0x1b819dd83LL, 0xdacb0ecddLL, 0xbf430f183LL, 0x829af66d3LL, 0xd1e2ca983LL, 0x779105c39LL, 0xb192af0a4LL, 0xd5098a6d7LL, 0x15cb456f1LL, 0x8481b933dLL, 0x409d19fadLL, 0x1f5c2f72dLL, 0x53673e5f5LL, 0x65e6a72eaLL, 0x0527da518LL, 0xeaddf7946LL, 0xb2617eaebLL, 0x691892fb2LL, 0xdcd129f35LL, 0xccb42e6b0LL, 0xdbc331935LL, 0x8867aa36eLL, 0xd7cd26a9cLL, 0x67a19fdc9LL, 0x20952b0aeLL, 0x6026d2e54LL, 0x98963fbd9LL, 0xa8b64c911LL, 0x97fd9f487LL, 0x7d3c848a5LL, 0x147a4c27bLL, 0x18c9cecadLL, 0x046eb72c9LL, 0xc88d38085LL, 0xb25dc1f48LL, 0x61790445dLL, 0xc125e5258LL, 0xfbeb9589bLL, 0xaecade15fLL, 0xda4f86a33LL, 0x6cf66cc43LL, 0xef65417e4LL, 0xdc50e0668LL, 0x8810cbcabLL, 0x4aafd7f97LL, 0x6c5c831a7LL, 0xb83ed224fLL, 0x64f1a00a8LL, 0x8db3c590fLL, 0x68ce5d2c0LL, 0xfd51258d2LL, 0xe2c5eb1b8LL, 0x59df15d2eLL, 0x700ebf57eLL, 0x3be89a8ccLL, 0x47fb46c45LL, 0xa923c0b32LL, 0xd88bac689LL, 0x27683edc8LL, 0x2eba1f952LL, 0x7102542c6LL, 0xd049fa79cLL, 0xcc0622afdLL, 0x693120c6bLL, 0x396461615LL, 0x2f1f4227eLL, 0x26b4bbf9fLL, 0xe8f7a6065LL, 0xecdf103d2LL, 0x708691ac7LL, 0x51941b750LL, 0xd33cd58a4LL, 0x95f2d0890LL, 0x3440d5b9cLL, 0x00afdca84LL, 0x625eae2ccLL, 0x2c36e42d9LL, 0x3f5f89ac8LL, 0x5649578c6LL, 0xbba60ace8LL, 0xe344ddca5LL, 0x128c435deLL, 0x02c52f620LL, 0x4db779c52LL, 0xaac4bcb7fLL, 0xf13e98394LL, 0x696f9e134LL, 0x179733908LL, 0x2c9580b9aLL, 0xa6f7072b9LL, 0x3358704e9LL, 0x7182449f1LL, 0x86eeb1594LL, 0x82b9adb34LL, 0x0ee258afcLL, 0xe122b5e67LL, 0x42ed1c2b8LL, 0x8633f61ceLL, 0xe8e335121LL, 0x525874e76LL, 0x96901dd2dLL, 0xda22fc8e0LL, 0xa72387097LL, 0x4f5d9a9c7LL, 0x88a87c443LL, 0x33b056d4dLL, 0x62d064cf1LL, 0x5e7c243bdLL, 0x18213a370LL, 0xf7bd5923dLL, 0x815aaa9aaLL, 0x84aa0b3ddLL, 0x1ee0819f4LL, 0x847cf7f08LL, 0xf120ded5fLL, 0x9a76c1290LL, 0x082b32d7bLL, 0xc8b59e9fdLL, 0x2d4dff53bLL, 0x293d29fc4LL, 0x08701ab95LL, 0xfd7fab48fLL, 0xf4cbfe2b3LL, 0xcbca4906dLL, 0x99cc8a279LL, 0x34251b504LL, 0x89092fc9aLL, 0x9e1c0ccabLL, 0xf697de846LL, 0x055fce443LL, 0x6b0cad5ecLL, 0x87923cab1LL, 0x8b4e2839fLL, 0x0a44b0122LL, 0xfb3909a5cLL, 0xa16fafee2LL, 0x1778046b8LL, 0xba9808373LL, 0x064852931LL, 0xb79d2dec4LL, 0x50cb67a1bLL, 0xbbfb05d16LL, 0xf0139fad3LL, 0xccbb36e49LL, 0xa2cd25458LL, 0x49343b972LL, 0x112efab27LL, 0x53d9d772cLL, 0x40bda71fbLL, 0xf8e62647eLL, 0xdee732912LL, 0x53500b219LL, 0x7e47a2a14LL, 0xe11cd7fb9LL, 0xfcaca6ec8LL, 0x6c2da7d57LL, 0xd93abceaaLL, 0x01bb3f4bdLL, 0x67dcf76f5LL, 0xb5b63e989LL, 0x5bae6b067LL, 0x9b6eee44bLL, 0xb7f96ef25LL, 0xf4d97b712LL, 0x930dff68aLL, 0xa40a96cf0LL, 0x0d5683834LL, 0x647e13c72LL, 0x2a5c63408LL, 0x395fef00dLL, 0xb4f5afa8aLL, 0x017bfe90bLL, 0xc9042b9b9LL, 0x2c9ad5aa2LL, 0x047a51093LL, 0xea341678cLL, 0x546838991LL, 0x09858cac9LL, 0x6061e2795LL, 0xce7d6ed07LL, 0x91b7626cdLL, 0x5805e4fe9LL, 0x119b5c832LL, 0x954f9e8f1LL, 0x6d1580b55LL, 0xf43091166LL, 0x0d5a7e04dLL, 0xe191d35a4LL, 0xd8ca15468LL, 0x0a708b652LL, 0xcc15b029eLL, 0xff5aa4719LL, 0x8e74f9498LL, 0xf9d92e911LL, 0x477e5d846LL, 0x042650770LL, 0x1e5730dcdLL, 0xa0e8eab0dLL, 0xddce6772eLL, 0x75a090773LL, 0xa19aa8eeeLL, 0xdcfe9d7caLL, 0xa0676c7ceLL, 0x17197c8bdLL, 0xa87af55afLL, 0xe2d1706aaLL, 0xa43dd708eLL, 0x26503e2fdLL, 0x123d37bdfLL, 0x68c7362b6LL, 0xea30ac137LL, 0xb01b70b8bLL, 0xb8d1315dcLL, 0x96824e75cLL, 0x98d141a84LL, 0x74aa24acdLL, 0xb4776887bLL, 0x8fdb27129LL, 0x70d84811dLL, 0xcb8b9f4c1LL, 0xa3a424426LL, 0xdd2531a76LL, 0xb29c389abLL, 0xbd1835e48LL, 0x47f3a453dLL, 0x8dfe4a80aLL, 0xa4b0a36eaLL, 0x4e886ad80LL, 0xe92d00ca8LL, 0xd7c6efc3aLL, 0xa5941aea9LL, 0x161a7aa58LL, 0x9ed69b98eLL, 0x64eda938cLL, 0x5588a6847LL, 0xd92d4a19cLL, 0xc9bd66defLL, 0x95b20c81bLL, 0x3e9ec0a50LL, 0xd957e66f7LL, 0x2251d77faLL, 0xa018cb128LL, 0x50bdb0443LL, 0xebebfa885LL, 0x05eb8892aLL, 0x294e078f1LL, 0x4c54e34b1LL, 0xc2ac1a6c4LL, 0x73e62b579LL, 0x95d24e16fLL, 0xd5edf5b3bLL, 0x9085ee304LL, 0xaa71ceb4eLL, 0xa51325eecLL, 0x2c0bd017dLL, 0xbf7101a25LL, 0x78d932686LL, 0x1bd9f946dLL, 0x4e18c469dLL, 0x61b4f9bd1LL, 0x9cc6aeeb1LL, 0xf457ec40aLL, 0x3a54d40cfLL, 0xf37fd42d2LL, 0xdca9017fdLL, 0x2018fb211LL, 0x0583139a2LL, 0xed794feb9LL, 0x2671f2339LL, 0x0a9dedb42LL, 0x00a59d973LL, 0xeeb0e6dd6LL, 0x27c5e838aLL, 0x15b92ceb6LL, 0xfaa9e1dc9LL, 0x691c2012dLL, 0xa700422f0LL, 0x42ce65421LL, 0xf36c2d8b9LL, 0xdd3255d33LL, 0xf16927235LL, 0xcea8c7ee5LL, 0x8b405217aLL, 0xa937cb21aLL, 0x1972d865eLL, 0x3dd15253aLL, 0x8e12c277eLL, 0x05f4e5ae1LL, 0x5cc68e658LL, 0x43dbcda4bLL, 0xa65e44bd5LL, 0xc2b21acbcLL, 0xd683fcfa5LL, 0x3f6a63ab8LL, 0x07da47408LL, 0xe8b12669eLL, 0x32a0a4644LL, 0xa9825a29cLL, 0x49b66fce0LL, 0xfa9a992ceLL, 0xd2b5fbfd3LL, 0x14a5b69ecLL, 0x74a030edaLL, 0x8e50a8e2fLL, 0x0a95167daLL, 0x80202bd54LL, 0xe8318bbe2LL, 0xa3ab33a01LL, 0xde35c6e5bLL, 0xa1226c83aLL, 0xb6c43aa86LL, 0xedc5e4d1aLL, 0xd5944aa7dLL, 0x478a80992LL, 0x47811cb87LL, 0xb95d85ffcLL, 0xe2360374bLL, 0xa9a9ec186LL, 0x74de32ca5LL, 0x04a66a427LL, 0xd4a65cf9dLL, 0xd8d75502dLL, 0x1c3b585a2LL, 0x795d13255LL, 0xd451f5854LL, 0xc70f36267LL, 0xc29959adeLL, 0x0c5da4711LL, 0x32123feacLL, 0xb7fdbc068LL, 0xae25f20b7LL, 0x25e3e6f31LL, 0x20232ce56LL, 0x74f543f01LL, 0x185cda063LL, 0x97d611476LL, 0x13f15f323LL, 0x173612c01LL, 0x3d616758dLL, 0x4737d01fbLL, 0x0d5d33019LL, 0x259e9f133LL, 0x5eae970d9LL, 0x4ac96e3bbLL, 0x2242e4b5cLL, 0x118f28ce7LL, 0x2f7cbf15dLL, 0x8a7d644e8LL, 0x80ef8b95fLL, 0x1be972336LL, 0xf42d2a60eLL, 0x42a580066LL, 0x9a911767cLL, 0x3a4b9612fLL, 0xdb0f1bbcdLL, 0x3ac93c1b1LL, 0xcfec5c2d1LL, 0x452d01604LL, 0x4f3f3998dLL, 0x8aa4775b3LL, 0x32fd7c6abLL, 0xe3e43832dLL, 0xe774f08a8LL, 0x25b8dcf5cLL, 0xc0f10d6e9LL, 0x23874a0c7LL, 0xc7e7c9c8bLL, 0x6470db895LL, 0x6a458e8d7LL, 0x886e29f3dLL, 0x4b6531365LL, 0x8eccc4bacLL, 0xeed4c9f8fLL, 0x7d0685f48LL, 0xf6e8b9b96LL, 0x524d9bc08LL, 0x8ba35deecLL, 0x127a38c15LL, 0x07d5ce1acLL, 0x8496b0888LL, 0x9589e40e3LL, 0x9f123483fLL, 0x0a7f03598LL, 0x57342da57LL, 0xdbaa17d6dLL, 0x4c068d8f3LL, 0xf76ad5b2fLL, 0xb398e63bcLL, 0x102afd902LL, 0x4877b8bfbLL, 0x790f6c2baLL, 0xc7655b992LL, 0x4e0d5b07dLL, 0xb02afe708LL, 0xa8875d3a1LL, 0x750681812LL, 0x77a9c79abLL, 0x02ad0c854LL, 0xc5cc6924fLL, 0x25e9c88dcLL, 0x877356004LL, 0xc67444a06LL, 0x12d72f8c9LL, 0xffa5a50eeLL, 0x931d865b3LL, 0xd5b7c6e10LL, 0x3d0aa7ef3LL, 0x92ce0cd5eLL, 0xa0f27b9a1LL, 0xd74b04855LL, 0xac4596a59LL, 0xa2b5aa65bLL, 0x88d45ef6dLL, 0x7294cb8a7LL, 0xeec1cb733LL, 0x964498855LL, 0x2279b8adbLL, 0x922c72d48LL, 0xf994172bdLL, 0x3e047e3dcLL, 0x317053d24LL, 0x42c8f55b7LL, 0xfa9d550dcLL, 0x91a2dd669LL, 0xa81f56081LL, 0x3fd3a6c56LL, 0xdf850d00aLL, 0xbe3e03545LL, 0x407ec6fd3LL, 0xbd016301aLL, 0x916a7194fLL, 0x11a49ea87LL, 0xc5d123c1eLL, 0x9a807b6b0LL, 0x0593ba877LL, 0x018d46d49LL, 0xa7a3e5dd0LL, 0x9686fbc5fLL, 0x22769d2b3LL, 0xb1395dd1cLL, 0x77e0a5c9fLL, 0x890a050ceLL, 0x49f50576cLL, 0xb43273783LL, 0xf909f5329LL, 0x9bb92f046LL, 0x4712ca12dLL, 0xe74059b06LL, 0x119424c3bLL, 0x08b0d1ef6LL, 0x636ed63efLL, 0x75fb4b1baLL, 0x64f15c372LL, 0x77d9f6a84LL, 0x007ae9bd7LL, 0xe00da4c99LL, 0x7a5814217LL, 0x117097acdLL, 0x5549740d1LL, 0x9f3aba1e0LL, 0x2ceeb811eLL, 0xed5f8aa13LL, 0x667486d91LL, 0x31472697eLL, 0x71fa40e6cLL, 0x29d3f8dccLL, 0x74e5ff0f1LL, 0xb18e969c0LL, 0x331847353LL, 0x95471db9eLL, 0x9e3816ef2LL, 0xc7bdacb19LL, 0xd33a176c1LL, 0xc15810741LL, 0x230e1a526LL, 0x3e7d17b0bLL, 0xfaf6ae216LL, 0x428e9f5b4LL, 0xbeb6ade2bLL, 0xde276103cLL, 0x6928c5d83LL, 0x685ca40eaLL, 0xc0d875c85LL, 0x8096f3381LL, 0xed453bc21LL, 0xfbda5c809LL, 0xa6542bb1dLL, 0x2e1a744a4LL, 0x9e5a72efdLL, 0xedfee81f2LL, 0xb48280aabLL, 0x7586536ccLL, 0xd46c9ac5bLL, 0x9f228c8e7LL, 0x2137d18f6LL, 0x513747f68LL, 0x88ae16997LL, 0x6e30cdf98LL, 0xa67eea5d4LL, 0x27bf40957LL, 0xf11c7e690LL, 0x25449ebbeLL, 0x0db5bb3b0LL, 0x7d784749aLL, 0x51a7cd63cLL, 0xe23ca5876LL, 0x4cdd52936LL, 0x326eaaa63LL, 0xf8c9d5d50LL, 0x0ed014396LL, 0xcf8a25b81LL, 0x2dbdcc80bLL, 0xc4aa9df2fLL, 0x6f6c2f6dcLL, 0x59e8ef553LL, 0x0f6c484dbLL, 0x9ec72a877LL, 0x2cc4e64caLL, 0x58899d59cLL, 0x0bfe119faLL, 0x8361ac7b7LL, 0xb3615653cLL, 0x541ea167bLL, 0xfdb2fd954LL, 0xcf4fd9d05LL, 0x48f5aab41LL, 0x22d74d9b3LL, 0x9b3270e8fLL, 0x2fff128d0LL, 0xca0170b88LL, 0x6ca810fc5LL, 0x90b6aa826LL, 0xb1b09d138LL, 0x02b238b76LL, 0x98f2254d5LL, 0xbbb2d3652LL, 0x3427c35ceLL, 0xf559d72f4LL, 0xf982e7fd6LL, 0xf102bc341LL, 0xf13b625b0LL, 0xc6f04c475LL, 0x7523d80e8LL, 0x44a0e7ebbLL, 0xafd06e1bdLL, 0x68808e0e5LL, 0x90621d64eLL, 0xe05e5a36cLL, 0x83131c4b6LL, 0x97eb76817LL, 0xd9cc44ff8LL, 0x8b36f9654LL, 0x0245dde9bLL, 0x520e35f6dLL, 0xd4cceb189LL, 0xc2b894013LL, 0xaeb641c96LL, 0x7af4d8aeaLL, 0xad2618e25LL, 0xabe43bbd9LL, 0xef52ec2a3LL, 0xb73c6029fLL, 0x6a3807c97LL, 0xddfb4bd6fLL, 0xf3d8e96d6LL, 0x2aa6fa42aLL, 0x43a2f1918LL, 0x3a90c3595LL, 0x2cd912f20LL, 0x9adc1bd3cLL, 0x7b9ac2f88LL, 0xc5833c0a4LL, 0xbd9b65455LL, 0xcb1c9e7eaLL, 0x63a0524fdLL, 0xf4c384b5aLL, 0xe8e524253LL, 0xc22b11d9fLL, 0x68422fc45LL, 0xa68c0b54cLL, 0x26f6c3e32LL, 0x6d33234ddLL, 0x3249aeef8LL, 0xa44768ab6LL, 0x799b27147LL, 0xc2d133071LL, 0x97db182d3LL, 0x86eb74d4aLL, 0xe937414e1LL, 0xa7a7bb500LL, 0xbb9745c67LL, 0x598c8590cLL, 0x0daa06872LL, 0x1630a0579LL, 0xed748567fLL, 0x985b0f840LL, 0x65fbf1b05LL, 0x2ac24e078LL, 0x8e44e7201LL, 0x73dea54fbLL, 0xec53fc29dLL, 0x17eb2107bLL, 0x0623990cdLL, 0xe8d2b249aLL, 0x3e53f5292LL, 0xb31e481faLL, 0xbd28c91c7LL, 0xc95853493LL, 0x4cf19e5a3LL, 0x13dd03e15LL, 0xe420f8b60LL, 0x0aba216f1LL, 0xfc949ce77LL, 0x12cc4c8b0LL, 0xda1af11cbLL, 0x95e51047dLL, 0x239712b3fLL, 0xdd8c9ff85LL, 0x3b7c11327LL, 0x03706c9cdLL, 0x4a5428a10LL, 0xf9ff29dacLL, 0x6880bb16eLL, 0x39ce35cc3LL, 0x348a6f738LL, 0x481ebd8beLL, 0x312f67693LL, 0x066d19548LL, 0x252c908b8LL, 0x93dd489c4LL, 0x2e916877eLL, 0x4f03ba3ebLL, 0x8332640a5LL, 0xb5418976cLL, 0x43e45adcfLL, 0x5ad03c442LL, 0xc5c651faaLL, 0xe9e22aff4LL, 0xe3e84f5acLL, 0xfa27e6393LL, 0x5c2975ff6LL, 0xeb28706c1LL, 0xbc16eb250LL, 0xb230ff592LL, 0xe01b41ea3LL, 0x550beeb62LL, 0x3acc6d9d9LL, 0xfee57e361LL, 0x0e47f1f33LL, 0xa13c0e131LL, 0x8c28e5a14LL, 0x7d201d0eeLL, 0x72377cd9dLL, 0xc2d4903e8LL, 0xd865c90ceLL, 0x5159115feLL, 0x70d91786dLL, 0xa6859de80LL, 0xfb7b97d08LL, 0x5844a534dLL, 0x902dba777LL, 0x36d28050eLL, 0x01bc8a4f9LL, 0x6c810239dLL, 0x1b8bdbc1dLL, 0xed019746dLL, 0xb2e22db8bLL, 0xce20242c4LL, 0x277835081LL, 0x55c421fe5LL, 0x1c3d0d749LL, 0x26bd653e3LL, 0xd7ed2ff69LL, 0x10c6fc33bLL, 0x9ea03e399LL, 0x5b22ed1a3LL, 0x99ced9f23LL, 0x6ac377418LL, 0x9129d7dd6LL, 0x43ace647bLL, 0x7c4b49f58LL, 0x900ae344aLL, 0x2b8959613LL, 0x47fc08ec9LL, 0x21d67379eLL, 0xd89df08faLL, 0x37d955225LL, 0xcc10979b9LL, 0x6621b1eb7LL, 0x0de2fc4a2LL, 0x745cfaf22LL, 0x8d46a9619LL, 0x61b1ceee4LL, 0x675b9a2bfLL, 0x40058aafaLL, 0x05d25b9c4LL, 0xefcb86566LL, 0xc13e788eeLL, 0xab9e7280bLL, 0xd9306e8d4LL, 0xa266d71b8LL, 0x45995737dLL, 0xea8f02adbLL, 0xb313918c4LL, 0x970f9be58LL, 0xaaf674ce6LL, 0xc7d00b910LL, 0xfa502e688LL, 0x3702f79aeLL, 0xd893b843dLL, 0x0f1e45ddbLL, 0x93c9617a8LL, 0xe14247733LL, 0x43f4cb24cLL, 0xf1b75980eLL, 0xcea5553fbLL, 0x4f4d89e4bLL, 0xd8bfdb958LL, 0xe231a41daLL, 0x60116696aLL, 0x12cdb2096LL, 0x6d0202075LL, 0xb75ae1a2aLL, 0x888d79ea8LL, 0xc7d80e5c6LL, 0x4f21633fdLL, 0x9375ea207LL, 0x84017c66bLL, 0x7ec585495LL, 0x5be0f7c5cLL, 0x14b106f8aLL, 0xba2feec25LL, 0xf12261575LL, 0xd87fc9c27LL, 0x7ffea3372LL, 0xfe6f6a5b2LL, 0x494a627dfLL, 0x73c4f89f4LL, 0xb54156ddbLL, 0x14d501f4eLL, 0xbe50c2ad8LL, 0x76cfa8441LL, 0x2a6832d8fLL, 0x82ed6f577LL, 0x234ee72bdLL, 0x3fe939c14LL, 0xd9aa5f1bfLL, 0x436b7049eLL, 0xe939e6266LL, 0xcab65d44cLL, 0xc6b90201dLL, 0x9dd60ae8cLL, 0x975c2a441LL, 0x3eaa603e5LL, 0x67e5526b2LL, 0xdc57f5beaLL, 0xbca1f74baLL, 0xc3472a0b3LL, 0x74fc1b243LL, 0x4011a3cadLL, 0x688f30527LL, 0x7cb1bd3b6LL, 0xc6172373dLL, 0x1e58a5d3bLL, 0x161e143f2LL, 0x69fecc526LL, 0xedc625028LL, 0x62386857cLL, 0xc65817d6eLL, 0x1c2907aceLL, 0x5f845976aLL, 0xc71a8dc11LL, 0x56431fdb7LL, 0x096c5d1c4LL, 0x7fa1f8582LL, 0x71e278b58LL, 0x50719d063LL, 0x9f166f178LL, 0x503ada677LL, 0x044953c8bLL, 0x3477a905cLL, 0xe18a293adLL, 0x63eb0f954LL, 0x6504fe3f3LL, 0x207477edcLL, 0x45395da2eLL, 0x0d5d3fe02LL, 0xa8b671eabLL, 0x53b890c75LL, 0x21318478dLL, 0x84c15d0ffLL, 0x1163ede85LL, 0xa3d4784cfLL, 0x0e8d78cf3LL, 0x029b52b98LL, 0x31261cfa9LL, 0x8ce517f2fLL, 0xebc01b551LL, 0x55e719687LL, 0x14a334c3eLL, 0x7a18edcf6LL, 0xd2adc5527LL, 0xb9355ca29LL, 0x899223cf7LL, 0xcb97089c8LL, 0xc024cf877LL, 0xb705e424bLL, 0xa8e0142c5LL, 0xd60a133e3LL, 0xfbb712edcLL, 0x90ac40d24LL, 0x80b3aa7bcLL, 0xa0fa4a297LL, 0xa660659ebLL, 0x6270b5e01LL, 0x7875e8d73LL, 0x99b1c9fe5LL, 0x26147c55fLL, 0x494d6032aLL, 0x805c954b3LL, 0x4bc4dc338LL, 0x8d28838b6LL, 0x65f2934e9LL, 0xd2e0ba595LL, 0x2f63fd89cLL, 0x1d468e027LL, 0xae5c50ea9LL, 0x2754729f5LL, 0xb4821c6f9LL, 0x1e2deee48LL, 0x6fd833c89LL, 0xaf7e537b5LL, 0x0f96cbd90LL, 0xf82ef480bLL, 0x906fa3f87LL, 0x621287584LL, 0xf05c6358eLL, 0x5698e95c5LL, 0x105509992LL, 0x0762e2a0eLL, 0x6ffb64722LL, 0x64d6dccbdLL, 0x1b7f83d6bLL, 0x5518fcb18LL, 0x025ee2c96LL, 0x8e78ed132LL, 0xad93d4196LL, 0x8c8750aa2LL, 0x9e3c7b381LL, 0xe5148c631LL, 0x4b38f2617LL, 0x82b98bc77LL, 0x92dcc557cLL, 0x28dc57fd1LL, 0xf912e0c62LL, 0xaac55e599LL, 0x081984b5cLL, 0x93826e871LL, 0xce7abded7LL, 0x414db1e2dLL, 0xef68bda6bLL, 0x47da612d0LL, 0x07b99ba1eLL, 0x8b7872c39LL, 0x3390d271bLL, 0x32f6bc3aeLL, 0x77325c35dLL, 0xff2993b97LL, 0xd811caa67LL, 0xf6610ec51LL, 0xf61101d05LL, 0x9bdf143c8LL, 0x712a1da5fLL, 0x69edb2899LL, 0x918f4b5fdLL, 0x9b70b01edLL, 0xd1d7c1bd4LL, 0x7455d932bLL, 0x53590c39aLL, 0x4021360a9LL, 0x1dc922b31LL, 0xe893c6a98LL, 0x7e274d451LL, 0xb3197bda3LL, 0xe0d039a4cLL, 0xb5bb78bf2LL, 0x3f453821eLL, 0x1ceea1703LL, 0x7e245e4faLL, 0x43dfb0af8LL, 0x56a72aab3LL, 0x4cdc702f9LL, 0x7196ac417LL, 0x185c13ae6LL, 0x72a069055LL, 0xe3b657a99LL, 0xe54143be9LL, 0xb96b8d06aLL, 0xb917bc739LL, 0x075675126LL, 0x667eae736LL, 0xa24735968LL, 0x80cb15f06LL, 0x9084726feLL, 0x9115fce0fLL, 0x4fd117067LL, 0x08ed4302bLL, 0x09d57f52eLL, 0x83b947bd2LL, 0x1b379cbc5LL, 0x6e4f6d6c3LL, 0xf3a45c9a2LL, 0xa6edec8baLL, 0xbfe48b96dLL, 0xa316bb1f0LL, 0x4b02d5449LL, 0xb7c8d6f14LL, 0xf8aab8168LL, 0xbbd63e715LL, 0xff2e5262dLL, 0xe7ce4a45dLL, 0xe69ea1730LL, 0x83a416eb4LL, 0xd131294b9LL, 0x0cff9c21dLL, 0x51f972f8cLL, 0x8aeb661f1LL, 0x79db4388cLL, 0x38d94d7a3LL, 0x8035d996dLL, 0xcaf8d77c6LL, 0x6ccc390e7LL, 0x7510bb2deLL, 0x47570789cLL, 0xbb0f0b128LL, 0xe344866dcLL, 0xf8e8538c5LL, 0xa89fe5639LL, 0x30bf26d3aLL, 0x9903d49fcLL, 0x475e6c8f8LL, 0x7e3d98813LL, 0x5e6f1698dLL, 0xa1304eb9bLL, 0x8662da415LL, 0x5b84b9c39LL, 0x8759f87dbLL, 0x9bc957f53LL, 0xcfafd522dLL, 0xf8fdaa82aLL, 0x3975f695cLL, 0xd9eabbf19LL, 0x054c9e489LL, 0xcebed9ba9LL, 0x3f29c0269LL, 0x280639ce6LL, 0x19a9698ecLL, 0x7e9965582LL, 0x23efea1b4LL, 0x5674b2e35LL, 0x28cfadfadLL, 0xd224628aeLL, 0x1a5857d9fLL, 0x1f2fb59cbLL, 0x45eeca839LL, 0xf57b0db20LL, 0x2cf4d5a09LL, 0x5898bb1b1LL, 0xdce4dd9faLL, 0xb2fed21f2LL, 0x29fcd26a6LL, 0xff1ed7959LL, 0x4582dd7c6LL, 0x94d96dcbcLL, 0x08fcaf998LL, 0xc2150dc02LL, 0xcb725e6feLL, 0x497bf9d00LL, 0x9a07aa199LL, 0x231cc756aLL, 0x8029024e4LL, 0x706c3b697LL, 0x21dab6df9LL, 0x9822ce6c1LL, 0xd9339f82bLL, 0x08588a50cLL, 0xfc592f12bLL, 0x5d0de92e1LL, 0x84cf29460LL, 0x17dd9463eLL, 0xd552a6e3fLL, 0x9921d4023LL, 0x511997b06LL, 0xa7db8d63bLL, 0x7628f7e02LL, 0xb5a70da67LL, 0xb78b26044LL, 0xcc757da90LL, 0xa372fe31fLL, 0xa6d0e7885LL, 0xfdd5c9958LL, 0x22d093093LL, 0xac02463ccLL, 0xb8dc89befLL, 0x0c6ac81d2LL, 0x2453311fdLL, 0x2a4b37046LL, 0x1022902e5LL, 0x09b58f09fLL, 0xdca206ca4LL, 0x8c5443cfeLL, 0xd1bcfda67LL, 0x06156bcbbLL, 0x71826e127LL, 0xd8b00b0eaLL, 0x16d325e74LL, 0xf67b3e901LL, 0xe7d585092LL, 0xc1aa82c54LL, 0x020121d7aLL, 0x836756756LL, 0xed93b03e8LL, 0x25e1b4a2bLL, 0x94279c34fLL, 0xa6130a7fbLL, 0x0cf097151LL, 0x192bb41b7LL, 0x82650ff19LL, 0x222fe2d89LL, 0xf38d6658fLL, 0x0da70ff01LL, 0xd531e4109LL, 0x8fc408d99LL, 0x864f65e98LL, 0x0f552caeaLL, 0xf07941d6eLL, 0x69520b2ccLL, 0x4e19bbdfbLL, 0x201ad85b4LL, 0xa24f492e0LL, 0xf577561a2LL, 0x3ac417bfdLL, 0x2c262d743LL, 0x1a57ae77dLL, 0x9cd8b411cLL, 0xd39c85983LL, 0x11b2afd51LL, 0x249ce5cfbLL, 0xd33548b8eLL, 0x5dd56d7c9LL, 0xa56ee9248LL, 0x425d68e8aLL, 0x10f87e0e9LL, 0xc6b7f55b9LL, 0x359462f78LL, 0xf68d21c1eLL, 0x7611706cdLL, 0x1252d7e37LL, 0xa8bfdb38cLL, 0xe86b4fbd2LL, 0x35a2f972fLL, 0xe3d6ec1c8LL, 0x1b4bc2a9cLL, 0x1daaaa19dLL, 0x2883e99faLL, 0x0deb8b211LL, 0xa3779f2c6LL, 0xe9e6d10a7LL, 0x4ea5c2770LL, 0xec50b1009LL, 0x914f44e74LL, 0x42c94c52eLL, 0xddaeee892LL, 0x77cbea957LL, 0xa1c5404a3LL, 0xbb7add120LL, 0x3a28116d5LL, 0x862e8ec66LL, 0x4ecbec8b1LL, 0x86bbbeb7aLL, 0x09efcbc44LL, 0xa213bc2a8LL, 0xf18dee3d2LL, 0xcfabe895bLL, 0x62d502afdLL, 0xdee6d82c7LL, 0x72e120e31LL, 0xf40d09991LL, 0xdf823444bLL, 0x5265afe71LL, 0x30a0e4dfeLL, 0x5c5c0b7f4LL, 0x4996907d6LL, 0x600c53191LL, 0x01ac06c9aLL, 0x4a53707b6LL, 0xd16ea6a44LL, 0x60be6e211LL, 0x5edf96076LL, 0x6b2e67779LL, 0x93d96e2eeLL, 0x90846d495LL, 0x61eaedc80LL, 0x1e267b96cLL, 0x17530b57dLL, 0x64fa1dd35LL, 0xbbb4c69f5LL, 0xb6323afb6LL, 0xf8d4d766bLL, 0x25e356458LL, 0x53934ceedLL, 0x864225814LL, 0x1c36a7a0fLL, 0x778490a0cLL, 0x5cc616142LL, 0x636654005LL, 0x8f554e490LL, 0x2984db337LL, 0x02ac89bedLL, 0x09a93c224LL, 0xa4a194fe7LL, 0x9a75ab8d8LL, 0x26245c20aLL, 0xa64fd081dLL, 0xfa8cde28aLL, 0x975c3c712LL, 0x8b1ad0475LL, 0xcfa7b1de2LL, 0xf2bdad2edLL, 0x071b918baLL, 0x1eaea2ff4LL, 0x04838c15bLL, 0x599911ed2LL, 0x41ddcd721LL, 0xdb10e3fb5LL, 0x8bbadce41LL, 0xfb483af95LL, 0x919fc8b26LL, 0xe2414e967LL, 0x33632f41aLL, 0x8ef4feed2LL, 0x7288b6a3aLL, 0x8f0db5f29LL, 0x35429b088LL, 0x34b45572bLL, 0x5a4889ae3LL, 0x22ac2efa9LL, 0x49b0e4db7LL, 0x8fd129de6LL, 0x0aaddf60dLL, 0x164d7e181LL, 0xb4c6f0d55LL, 0x8a1d30ab1LL, 0xd69bcbc97LL, 0x84b118674LL, 0x83223f98fLL, 0xa7e49656eLL, 0xb2eaa12b5LL, 0xbbd1fc497LL, 0x8737c574eLL, 0x9c29ec54fLL, 0x48941910cLL, 0x7b5140011LL, 0x14d62b95aLL, 0x004a8bee9LL, 0x17d22cde5LL, 0x779768374LL, 0x41d9d481eLL, 0xdfc61088aLL, 0x8ae83d895LL, 0xefd5a6e97LL, 0x0e9142829LL, 0x684e1983dLL, 0xbd5acb77fLL, 0x2eb9f5998LL, 0x084ad2738LL, 0xca47a6db4LL, 0xb730d9221LL, 0xe1d02dae3LL, 0x1897bbeedLL, 0xae6460eceLL, 0x2ae10dcc2LL, 0x20b562229LL, 0xab5a5963eLL, 0x23f786b94LL, 0x7a1e0be5bLL, 0x0d416a4bdLL, 0xfda34600bLL, 0xea42e3913LL, 0x191c6acd2LL, 0x2cd0c0257LL, 0x3fa745285LL, 0x486a34308LL, 0xd5bc0fc16LL, 0x9c29c1970LL, 0xc748e4213LL, 0x7d894d16dLL, 0x462f9a307LL, 0x93e77fc9bLL, 0x2586ca766LL, 0x7f4843d74LL, 0xc59dfb603LL, 0xcc9f8a977LL, 0xfcd5dbdaaLL, 0xabd042620LL, 0x01e98ed52LL, 0x3723c0ed4LL, 0xbba7a3d47LL, 0x87191e9e5LL, 0x96f863d99LL, 0x52ff61749LL, 0xed5634dceLL, 0x29e089ed4LL, 0xc7840939cLL, 0x710fd2bd7LL, 0xa7bcde8eeLL, 0x6992f5b0eLL, 0x8e43638daLL, 0xa8f8bb730LL, 0xb3ac0121aLL, 0x9a39430a5LL, 0xb400f5406LL, 0xf1fed9cf4LL, 0xecb8eed3dLL, 0xf982961b8LL, 0xd2ca7f580LL, 0xc0ff82a1aLL, 0x472f86bfaLL, 0x6c7819a1aLL, 0xc5fb45b51LL, 0x1e8265bbcLL, 0x992ea46fbLL, 0xeae69326cLL, 0x5cf05b4b0LL, 0x4dba41e1bLL, 0x678b0c87bLL, 0x541dfe46dLL, 0x5c6805370LL, 0xa46d1f411LL, 0x20ca43c25LL, 0xb825728d0LL, 0x60e64b7c3LL, 0x158a87789LL, 0xec8327da8LL, 0x312f31d11LL, 0x44e751abdLL, 0xb86749801LL, 0xf1e21217dLL, 0x1df98766aLL, 0x3b82a6a92LL, 0x813e55a4aLL, 0x425a3e347LL, 0x23d2e0991LL, 0x125b5b7a8LL, 0x299f2386dLL, 0xae87ad90eLL, 0x88d6c80c2LL, 0x505d3e3acLL, 0x8c08d9bf5LL, 0xbc74ec4c7LL, 0xb4b961f43LL, 0xeffd6e25bLL, 0xa34783dd4LL, 0x4b77e81edLL, 0xdae14fb89LL, 0x652a40492LL, 0xb89ac1d1fLL, 0xc326835a3LL, 0x62377a03eLL, 0x073a0bde3LL, 0x63ab6d119LL, 0xa4516da9bLL, 0x7310b4716LL, 0x0eaa5bfd1LL, 0xb2e0c1b7aLL, 0x6b2d91842LL, 0xa71ee6024LL, 0xe96938770LL, 0x94bf95cc1LL, 0x13752cd22LL, 0x0212fadb3LL, 0xa6d81af51LL, 0xdf3594b4eLL, 0x74d16cfeaLL, 0x1c7666313LL, 0xd4d2825d3LL, 0x32ef55c77LL, 0xb41709c57LL, 0x65496705fLL, 0x46c363baaLL, 0xe5d09d8b4LL, 0x4a092d2adLL, 0x1c85dc273LL, 0xb7fd50ec1LL, 0x85898eb2fLL, 0x64e4c723dLL, 0x53d9f5ed1LL, 0x3a557ef3bLL, 0xbb1bf6342LL, 0x5a5ef851bLL, 0x47ad6c0efLL, 0x8fb3984b8LL, 0xde8ad29aaLL, 0xce019b097LL, 0xa2e99fbdaLL, 0x254954e30LL, 0x380afe9d3LL, 0x6ed87890fLL, 0x75c288fadLL, 0xfed4797b1LL, 0x1b19ca486LL, 0x2bddbba6bLL, 0xbda8398a9LL, 0x4b96c12d9LL, 0x74448a4d4LL, 0x84a06a048LL, 0x7be5eb6f5LL, 0xbf1429ebcLL, 0x317cbe9b3LL, 0xda1bfba51LL, 0xf19033d98LL, 0x6692ec390LL, 0x85de73668LL, 0x502e6e128LL, 0x6bf51b707LL, 0x858deeef7LL, 0x231f255c8LL, 0xdc5285120LL, 0x1f7499d35LL, 0x51e562dfbLL, 0xfd31d9e13LL, 0xf56f7abe5LL, 0xbfe2014b4LL, 0xae9e0b0baLL, 0x04ab6818fLL, 0xfc5222cf4LL, 0xe479029b2LL, 0xedac0cbc2LL, 0x3dd056ec8LL, 0x2b3ab1bf3LL, 0x96090fde2LL, 0x7414985b2LL, 0x07c5f1053LL, 0xc912a2b86LL, 0x2238893caLL, 0x0a047b8d4LL, 0x956a79db3LL, 0xc987e2e17LL, 0xa3c23d85aLL, 0x97129d3d4LL, 0x8602ebdf8LL, 0x0c1e5c94aLL, 0x728fa9af1LL, 0x6783b3466LL, 0xd03a47b3fLL, 0x577e66a97LL, 0x064ea8d8fLL, 0xf810556b3LL, 0x550d70ce2LL, 0xeaf8c9627LL, 0xe1c975366LL, 0xb0c0fb42aLL, 0x60b80b9c6LL, 0xe5c4caf80LL, 0x54c8d166bLL, 0xc8f6e29f8LL, 0xb0c391d73LL, 0xc5684f473LL, 0x0812877caLL, 0xd98c3dc0fLL, 0xa530878c2LL, 0x83b26929fLL, 0x473ec8d20LL, 0xfb27d5064LL, 0x790c88e09LL, 0x4fad3f4a3LL, 0xde15f1749LL, 0xc2ba6e2acLL, 0xe52a2cfb9LL, 0xc5797941cLL, 0x383593138LL, 0x7a025b10aLL, 0x92d15a83dLL, 0x3084af2a3LL, 0x6cfe2a38bLL, 0x66973a8f4LL, 0x265dd307bLL, 0x884284f79LL, 0xf04052eb4LL, 0x94b8aaadeLL, 0x0bd36a298LL, 0xe22dd0993LL, 0x30a8178f2LL, 0xef40b3badLL, 0x5fd9521deLL, 0xe323cbc56LL, 0x2dbd84334LL, 0xe7f732978LL, 0xc940d0d9eLL, 0x017e1db0dLL, 0xb30f2898dLL, 0x3a70d4c13LL, 0x6877cc2c7LL, 0xb29c26091LL, 0x86d1b6ba0LL, 0x34790d00aLL, 0xcea3d1c3aLL, 0x609555677LL, 0x47ae0cd43LL, 0x6bc9151d8LL, 0xcd08c96bfLL, 0x2dcf87f90LL, 0x5d38abc7aLL, 0xb53944398LL, 0x5aa36c2c2LL, 0x6dc1a9602LL, 0x3f78a849eLL, 0x9a75bd745LL, 0xcccae769fLL, 0x7bb8bd72bLL, 0x37c8f5b48LL, 0x63307f50eLL, 0xa16e04992LL, 0x461e6f322LL, 0x05c9b0b16LL, 0x125571aceLL, 0xd671fa72fLL, 0x1da08d7dfLL, 0xf75a3b250LL, 0xc3e35da05LL, 0x9debf4cf5LL, 0x568893e81LL, 0x9f45f5a8fLL, 0xd28f36e5fLL, 0x39ed2c299LL, 0x7ec5b28bfLL, 0x726971dbdLL, 0x62a7e6608LL, 0x2465cc620LL, 0x54b82a639LL, 0x16f964b59LL, 0x18afd656aLL, 0xfa45a2401LL, 0xd63e74a3eLL, 0x85c8b9164LL, 0x01c5a5bfbLL, 0x7b406d02cLL, 0x1a6885edaLL, 0xea0f61089LL, 0x3356e4e19LL, 0x954d1c237LL, 0x73894ee02LL, 0x594d206b1LL, 0xa34df0652LL, 0x6627dfb9cLL, 0xe0a8d336fLL, 0xf76b0bf3eLL, 0xf4bcb8c6cLL, 0x9cfee3b2aLL, 0x727af778eLL, 0x2af80ac19LL, 0xa99353076LL, 0xbc1b3a11fLL, 0x055fa22abLL, 0x5bc6b849eLL, 0xa7b5b3826LL, 0x1464c65f1LL, 0xb797e45feLL, 0x06aa7641cLL, 0xd6402bea1LL, 0x69042dc7aLL, 0xc27a84b0fLL, 0xc3e40bc3eLL, 0x5188a9088LL, 0x9ca221e9fLL, 0x2eeb0cf45LL, 0x387326c49LL, 0xc192f116fLL, 0x5b4afe3f7LL, 0x1e2b7028bLL, 0xf315bf477LL, 0x1dfa88408LL, 0x82ede7a9fLL, 0x1daec4b02LL, 0x90b55c4fbLL, 0x1dfa5f446LL, 0x35d8744beLL, 0x9ed16f6c2LL, 0xbe5a98781LL, 0x057cf82b0LL, 0x62c4d6074LL, 0x0e524f153LL, 0x5ad102332LL, 0x5ce554139LL, 0x09b4882e4LL, 0x2f854512bLL, 0x42376f39fLL, 0xbd5b7cd06LL, 0x5f34b8fa2LL, 0xc8dd8867aLL, 0x26ff91debLL, 0x24fc05cf4LL, 0x61e928d08LL, 0xe5ed22480LL, 0x7edcca236LL, 0xbf19c5c5aLL, 0xd876f6600LL, 0x18cc2a06eLL, 0x98f9a0f2dLL, 0x0e46da359LL, 0xf10ca127dLL, 0x4ea58d6c8LL, 0xc5d584facLL, 0x4168b3771LL, 0xf7742c3abLL, 0x3d9e01ab6LL, 0x749603675LL, 0x93d380e27LL, 0x935fbdf99LL, 0x8b004f544LL, 0xf21336ccbLL, 0x55be0a2d7LL, 0x31a272812LL, 0x9356f2104LL, 0x64cd11059LL, 0xb306fab7bLL, 0xc4b4bcecbLL, 0xa44ee754cLL, 0x9c5b95d4aLL, 0xafe82cb14LL, 0xd2802c70aLL, 0x9b067ca08LL, 0xa0d33eadfLL, 0xf82d374e8LL, 0x97608d4c1LL, 0x3c49f7262LL, 0x1f8b8b4b2LL, 0xd49171ea7LL, 0x9abe1e7f5LL, 0x127c694ccLL, 0xad02f7180LL, 0x48dcd2cc8LL, 0x781de93caLL, 0x2593187eeLL, 0xdc2e3515fLL, 0x97535dc18LL, 0x3db5f0785LL, 0xcb73b619dLL, 0x76c0cae0fLL, 0x195f0526dLL, 0x4b0893b28LL, 0xca2151e27LL, 0x0e01526baLL, 0x844d01096LL, 0x96ed3edd6LL, 0xfe1d07fadLL, 0x625d110ecLL, 0xd6e07dc43LL, 0xa0d531bbfLL, 0xed6e29fd3LL, 0x9aa5ff7e9LL, 0x2bf330b23LL, 0xaa905fb48LL, 0x575c3dd88LL, 0xc12a8ea3eLL, 0xa60ce7ea7LL, 0x36e3d46beLL, 0x84a1acf90LL, 0x99315f5efLL, 0x23c5c149eLL, 0xe819f9d2aLL, 0x0327b7f85LL, 0x60878c9fcLL, 0xbbe1b425fLL, 0x5efb57d1aLL, 0xae81e06a3LL, 0xc683416e0LL, 0xa138a207bLL, 0x531a8805dLL, 0x3207cd72bLL, 0x1ffad4babLL, 0xa05d08c41LL, 0xc4ffc6089LL, 0xf6ebda0bcLL, 0xf9c6b0a7bLL, 0x76ac7063fLL, 0xbd7b1a5b1LL, 0xf6e306a3fLL, 0x93f23d425LL, 0xc9834e422LL, 0x73088434aLL, 0xb618f4fe0LL, 0x7affb1ed0LL, 0x8de2b690dLL, 0xe7bbca4f9LL, 0x0e92d2cd5LL, 0xb572af3ddLL, 0xa32f50358LL, 0x97d0aca69LL, 0x4e02a1956LL, 0x489a38814LL, 0x16cde41bfLL, 0xf844bb35bLL, 0xb3cb5e287LL, 0x798ca35d5LL, 0xad78e6952LL, 0x74e3accf5LL, 0x2c2509397LL, 0x9ed3b6e0bLL, 0x382fd5392LL, 0x46b1753ccLL, 0x3f1a79503LL, 0xd511be53cLL, 0x2c0ad700bLL, 0x0c83dab06LL, 0x854c15d1cLL, 0xa27cbb3d7LL, 0xf178f1810LL, 0xaa69504f2LL, 0x619f3c356LL, 0x2a31f3e7cLL, 0xef900e61dLL, 0xdfe8751a9LL, 0x651358e73LL, 0xc825327afLL, 0xbc151fafaLL, 0x876051de6LL, 0x06729eb40LL, 0x75876a783LL, 0x8c76d5474LL, 0x39bd3dd20LL, 0xbbc32223bLL, 0xf8ae91f88LL, 0xe3bbca3d6LL, 0xa0a3ee436LL, 0xf993c8887LL, 0xde8781781LL, 0xc3d26acc2LL, 0xce1748eb0LL, 0x09c49b6abLL, 0xe7ea808f1LL, 0x34d62cf99LL, 0xf0dcc0da8LL, 0x49ca61d07LL, 0x1b930880dLL, 0xdca578c15LL, 0xda7a6a1ceLL, 0x9c3294a82LL, 0xf0874721eLL, 0xa5dc060edLL, 0x23a8939a5LL, 0x6f6c16f02LL, 0x42edd94dcLL, 0x5a74f1432LL, 0xe019cb4ddLL, 0xc2c5b533eLL, 0xecc894724LL, 0xa5cff9f11LL, 0x04a5864abLL, 0xbf6b96bc9LL, 0xd7b3ded99LL, 0x7a9377d6fLL, 0x8a7f9cef2LL, 0xacf09eaadLL, 0x785f73185LL, 0x8904b414bLL, 0xbd5fa7379LL, 0x43ddabc60LL, 0x17f40d8fbLL, 0xcf6903101LL, 0x4cf434f98LL, 0xa97973ef7LL, 0xb3c4ac601LL, 0xc0aad8a1bLL, 0x2150bb64dLL, 0xed72e5ac5LL, 0x131cef825LL, 0x8659cecc4LL, 0xf30a8f072LL, 0x457c90fdaLL, 0xc9df54666LL, 0x68743306fLL, 0x62021b53dLL, 0xf9d7ea28aLL, 0x844e5bcd0LL, 0xfb1a1a1dcLL, 0x1394fd8d2LL, 0xf744f941eLL, 0xc07e65afbLL, 0x84a9e17d6LL, 0x9cd3f1176LL, 0x332e89b30LL, 0xbb9140f2dLL, 0x941e8e334LL, 0x9439f6e24LL, 0x0b89c5739LL, 0x077975e23LL, 0x4317e76cbLL, 0x53f2c5011LL, 0x7826317f7LL, 0xfad95bff5LL, 0x053de8de6LL, 0x856bcf7edLL, 0x77772201bLL, 0x1b15d0bb4LL, 0x42d5df9e2LL, 0xd9c5aea63LL, 0xb4980534aLL, 0x89c348f43LL, 0x3d7300d9bLL, 0x49440d5e1LL, 0xce4694114LL, 0x9e6c19bd7LL, 0xfa1294b28LL, 0xe2b6e790dLL, 0xe4891435cLL, 0x706bb090aLL, 0xc2f74169cLL, 0x9d669a64dLL, 0x4118daf4bLL, 0x5b118d504LL, 0x3f57b597fLL, 0x916e47de1LL, 0xe3b88c4feLL, 0x2af4fd773LL, 0x74664c8dfLL, 0xd66194717LL, 0x90a0baf41LL, 0xcf1f647faLL, 0xea211f2d8LL, 0x75a1f220aLL, 0x9cb63c40dLL, 0x812805a51LL, 0xd5d503690LL, 0x532bc4026LL, 0x021a8d59fLL, 0x5150b9dd3LL, 0x0a2ee41baLL, 0x846fade51LL, 0xc684ba60eLL, 0x3a8f6efd7LL, 0x03e17b682LL, 0x3927db859LL, 0x87b0b1480LL, 0x52536e422LL, 0x946ed17ecLL, 0xa214c62c5LL, 0x4342229d4LL, 0x7cd868ed0LL, 0xf19ba2ab2LL, 0xf8c1c41ccLL, 0x0cc7d69fbLL, 0x3c3911e61LL, 0x18fc07576LL, 0x0846ea988LL, 0x9f86addc8LL, 0xcacea817eLL, 0x975a047cbLL, 0xcc00a9d6cLL, 0x35b095cfdLL, 0x1032b2bd9LL, 0x839311750LL, 0xf61bbb4b8LL, 0xbeddf32a6LL, 0x5db253de6LL, 0xad1c79f1aLL, 0x0943a5b28LL, 0x8574f9b27LL, 0x6cb284742LL, 0x66a469a9aLL, 0x9c9e646b7LL, 0x15e3f3da7LL, 0xa7335e7c1LL, 0x81d5b8c23LL, 0xd3ce4adb1LL, 0xe6c2c0436LL, 0x0c422445aLL, 0x69efe9391LL, 0x6e494fb4bLL, 0x52938a7d8LL, 0x1538c045aLL, 0x86ffd6a31LL, 0x3a7a1eda4LL, 0x6a06881dbLL, 0x0ee1f5cfeLL, 0x7dbd99f14LL, 0x2222f7d4fLL, 0xa092a9546LL, 0x4f6ea12ecLL, 0x786076e0dLL, 0x2025a3e06LL, 0x3f3c67e47LL, 0xeeb2ddb02LL, 0xb1997582fLL, 0x84774c908LL, 0xa94d9c44eLL, 0x687970da3LL, 0x4ac4aaaceLL, 0xaa5b6afcdLL, 0x797cefd6eLL, 0x239edc6f7LL, 0xf6b54c197LL, 0x71141e85aLL, 0x22be2c02bLL, 0xb8343a746LL, 0xbb01b3487LL, 0x2fc5f873dLL, 0x80dbb9d8fLL, 0x749b8643dLL, 0x61163a4efLL, 0x899b02afcLL, 0x6d1359b14LL, 0x0e87bed7eLL, 0x34e646200LL, 0xdb37c86d3LL, 0x4a0cf7e11LL, 0xe33dd1e6eLL, 0x3b788a101LL, 0x06a936d91LL, 0x1ddc58739LL, 0xc92051779LL, 0x838ce51aeLL, 0xa7a3ef32dLL, 0x9f7eb103aLL, 0x36b9802d8LL, 0x65da1e81cLL, 0xea1cf562fLL, 0xa8c63c0ceLL, 0xf6104a32aLL, 0x4c4cf0893LL, 0xd47da0be3LL, 0x80483d254LL, 0xdc6de57b1LL, 0x7fac92c49LL, 0xda1e5f16eLL, 0x4026949bfLL, 0x8e17a9d20LL, 0x12f3419d6LL, 0x4e9089cdfLL, 0xd8f5fc61eLL, 0xdc8f7325bLL, 0x61247ef19LL, 0x0c1d908c9LL, 0x69bd43b4cLL, 0x3f7f5c231LL, 0x91c41e9bdLL, 0xbf844084dLL, 0x05827c577LL, 0xc07d61e65LL, 0x7615c75b6LL, 0x62600d7bbLL, 0x82f62a3faLL, 0x95f7fe291LL, 0x2ea50c1e5LL, 0xc90ae812cLL, 0xc85c79bf6LL, 0x0614cafdaLL, 0x692bc66d5LL, 0x43cae4bbeLL, 0xcc4dd9e12LL, 0x02b1f9181LL, 0xd19a5261cLL, 0xd8e402b96LL, 0x08f730fc6LL, 0x159e9ba29LL, 0xb9413ddabLL, 0xbd296f2feLL, 0x53ae2f38dLL, 0x42e6e15c5LL, 0x5efba4ecfLL, 0x764371efeLL, 0x8b665b2e1LL, 0x542432a00LL, 0xddd7e3c19LL, 0x195083f58LL, 0x7ed57c706LL, 0xb50bbbe8eLL, 0x203d18735LL, 0x2a4c69b07LL, 0xd72e90958LL, 0xcd3c5031bLL, 0x313e2ee85LL, 0xa12fd86c6LL, 0x9009383eaLL, 0xe550f2a4fLL, 0x49ae8c013LL, 0xeba56010aLL, 0xa7210786cLL, 0xfda4a611fLL, 0xcfe525fb0LL, 0xefcaf6ef6LL, 0x23c31fd13LL, 0x3847c5dcdLL, 0x166fd6693LL, 0x06f8ec087LL, 0xa531e0fb4LL, 0x76f9bc318LL, 0xe56c0cb7fLL, 0x51e2d02baLL, 0xb3e8d6022LL, 0x9a20ad5faLL, 0x73af547b7LL, 0x5e6134b2eLL, 0x8582b0e60LL, 0xc8ea6019dLL, 0x281e3763eLL, 0x647d83e97LL, 0x293634305LL, 0x6743c18c0LL, 0x2a01782f9LL, 0xbf49e9eb6LL, 0x05856f3b9LL, 0x634812ac0LL, 0x035cbb63eLL, 0x032540c2aLL, 0xaf358f5e0LL, 0x7826f829fLL, 0xb8c89448bLL, 0xebbe0239dLL, 0x905e5aa89LL, 0x310ddb064LL, 0xb762f118bLL, 0xc9b1a8e31LL, 0xff0661391LL, 0x8a66f999fLL, 0xb3593b7ceLL, 0x9f603a864LL, 0xa23a960efLL, 0xaae9837acLL, 0xc755afc59LL, 0x9c76f21c9LL, 0x39cd67a7fLL, 0x29f1dfa26LL, 0x01a253c9cLL, 0x1d3a11177LL, 0x7f60cd2ddLL, 0x5871af5ddLL, 0xa7d3d617cLL, 0xddbb2b303LL, 0x6c69147baLL, 0x843954b47LL, 0x46cc229a9LL, 0x4eac19c20LL, 0xfb7b51ca4LL, 0x9b7015d5bLL, 0xf2051a917LL, 0xe701d5d53LL, 0x38fd5da8cLL, 0xd3f2acb84LL, 0xba7cb1ebeLL, 0x1f016ea57LL, 0xd63c1250fLL, 0x646f446adLL, 0xc43b287cdLL, 0xdf676c468LL, 0x30f777359LL, 0xb8f10af87LL, 0x88c425335LL, 0x6c9f778aeLL, 0x50da4eebaLL, 0x0690f680aLL, 0xac101a310LL, 0xe603ce909LL, 0xef2918ebeLL, 0x3bb23758aLL, 0x409e4b934LL, 0x74f744bfcLL, 0x64e96d541LL, 0x271bc72c6LL, 0xba27f9edeLL, 0x782a145a1LL, 0x61f275072LL, 0x8c15dac40LL, 0x3431268feLL, 0x9853d7243LL, 0x98ba31d02LL, 0x6d1b5e69cLL, 0xe67d3e6a8LL, 0x3afb9b2d3LL, 0x8aef7d109LL, 0x0e0d77de8LL, 0xdcbaa0356LL, 0x6bc718df0LL, 0x77d6f39faLL, 0x1207d50beLL, 0x68758144aLL, 0x5600bd7f1LL, 0x99722a481LL, 0xbf673f8d1LL, 0x4bf1cce7eLL, 0xd3ee2609bLL, 0x31e9e4c73LL, 0x32a28985bLL, 0x9314656c1LL, 0xcf02f5fdcLL, 0x2748d2cbeLL, 0xeeaa65fcaLL, 0xc9294684aLL, 0x706fb6725LL, 0x32f6c276bLL, 0xe030afe87LL, 0x912b88011LL, 0x2ab7ee1feLL, 0xb04877110LL, 0x474b92b6dLL, 0xaccab7218LL, 0x04c6bb58aLL, 0x0d6777d7cLL, 0x6f6008813LL, 0x1d8da74efLL, 0xd9d905134LL, 0x4ff1a2f68LL, 0x2861d5528LL, 0x4ed66d24dLL, 0x7ee0b92f3LL, 0x0f2979e98LL, 0x36291e9bbLL, 0x0635f0e0bLL, 0x6cf4ea059LL, 0xa20b467b1LL, 0x610cc1d4dLL, 0x2da5cb5fdLL, 0x07553968cLL, 0x844a86803LL, 0xb1d48ccc2LL, 0xbba3fe918LL, 0xae0d869f3LL, 0x7c6a7de9cLL, 0x91f8382f4LL, 0x3014094caLL, 0x8c3a9a5d5LL, 0x69c3f2722LL, 0x8bb38f982LL, 0xb8efcdcdaLL, 0xda51b2ff0LL, 0xcf8e8a44aLL, 0x8158ddc00LL, 0x643ecd9c0LL, 0x5e10383a9LL, 0x24a886e16LL, 0x4ba9db990LL, 0xc8b4e335fLL, 0x8b1c2c0b8LL, 0xfba632127LL, 0x77bb6e51eLL, 0x63142235cLL, 0x7906aa8d6LL, 0x0295334c5LL, 0x30e855681LL, 0x7cc4fc438LL, 0x1cba770b2LL, 0xeb67c65e9LL, 0xbf495ac78LL, 0x250ef8472LL, 0xe918df704LL, 0x9ae42835cLL, 0xba887baddLL, 0x0a9e3a8cdLL, 0x6b4efbb80LL, 0x47e50b225LL, 0x492ddb0bbLL, 0x58bc2506dLL, 0x52aedbb0eLL, 0x51eb43eb4LL, 0xea2d9d116LL, 0x278274efcLL, 0xc013733d6LL, 0xd371949d4LL, 0x8d49ee822LL, 0x30745b332LL, 0xafc18e441LL, 0x0cd9fb4d7LL, 0x582ae0f65LL, 0xcacbd79cbLL, 0x1fe675d20LL, 0xee7117735LL, 0xa7280378fLL, 0xbaf1b5a32LL, 0x0b5d5e9b3LL, 0x44196d9ddLL, 0xd8b6fdafcLL, 0x82450b356LL, 0x8e0a46aa9LL, 0x250b0460eLL, 0x44582149aLL, 0x18a09bc26LL, 0xb26286d64LL, 0xc9e4860ceLL, 0xadfc81dadLL, 0x16506214aLL, 0x038769b15LL, 0xc01b50928LL, 0x151ac514fLL, 0x5492593daLL, 0x5946d5c27LL, 0xab9aed4dfLL, 0xc91cd1da7LL, 0xc3495b589LL, 0x2e962ae83LL, 0xeab920345LL, 0x155b3b617LL, 0x985edf48fLL, 0xe758d6ba3LL, 0x99660780eLL, 0x7b8a41acfLL, 0x6c4f8c59fLL, 0x970c219faLL, 0x5fa614ff0LL, 0xa7b3adda7LL, 0x809edc138LL, 0xb4ee65663LL, 0x6b487e8baLL, 0xa530f957eLL, 0x8a533c79bLL, 0x00d94ff5cLL, 0x9b709ef88LL, 0xd7c38826eLL, 0x0e7bcd688LL, 0x23668c8a0LL, 0x87244e859LL, 0xb1006de44LL, 0x15b37df10LL, 0x801e94f9dLL, 0xbce729ec0LL, 0x17ddf288bLL, 0x4bafecdbdLL, 0x8536977b1LL, 0x6f5ddbf9aLL, 0x23d5da865LL, 0x68e3a57c7LL, 0xfbb7bc650LL, 0xa412fceb2LL, 0x0cb978a6fLL, 0x30b6a7328LL, 0x167252f06LL, 0xe0df087e5LL, 0x022465eb6LL, 0xb580a4f2cLL, 0x0198d1533LL, 0x6e118afbeLL, 0xa3a697b20LL, 0x481fe6809LL, 0xc1cae26f0LL, 0xad006f34bLL, 0xdef609d40LL, 0x18a918f11LL, 0x68a4bf8dfLL, 0x4f8f44a9aLL, 0x4311a21a3LL, 0x53d72ea12LL, 0x51e4bd4d4LL, 0xf2cda5994LL, 0xd575a8336LL, 0x41cf85c53LL, 0x30eda148aLL, 0x907fa024dLL, 0x6e3cf917bLL, 0x470d9c67dLL, 0x73dc15f58LL, 0x708ec5e31LL, 0x83e1f42e8LL, 0xb315ee8bbLL, 0xd6b511041LL, 0xea2405d10LL, 0x8f51802b0LL, 0xb2ab1513bLL, 0xf31e33ae6LL, 0x55a48d565LL, 0x57cd5d243LL, 0xe705530c5LL, 0xf0908bfd5LL, 0x37a4d3e77LL }; + +static const TagCodes tagCodes36h9 = TagCodes(36, 9, t36h9, sizeof(t36h9)/sizeof(t36h9[0])); + +} diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagDetection.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagDetection.h new file mode 100644 index 0000000..9016f3a --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagDetection.h @@ -0,0 +1,99 @@ +#ifndef TAGDETECTION_H +#define TAGDETECTION_H + +#include + +#include "opencv2/opencv.hpp" + +#include +#include + +namespace AprilTags { + +struct TagDetection { + + //! Constructor + TagDetection(); + + //! Constructor for manually creating tags in a world map + TagDetection(int id); + + //! Is the detection good enough? + bool good; + + //! Observed code + long long obsCode; + + //! Matched code + long long code; + + //! What was the ID of the detected tag? + int id; + + //! The hamming distance between the detected code and the true code + int hammingDistance; + + //! How many 90 degree rotations were required to align the code (internal use only) + int rotation; + + /////////////// Fields below are filled in by TagDetector /////////////// + //! Position (in fractional pixel coordinates) of the detection. + /* The points travel counter-clockwise around the target, always + * starting from the same corner of the tag. + */ + std::pair p[4]; + + //! Center of tag in pixel coordinates. + std::pair cxy; + + //! Measured in pixels, how long was the observed perimeter. + /*! Observed perimeter excludes the inferred perimeter which is used to connect incomplete quads. */ + float observedPerimeter; + + //! A 3x3 homography that computes pixel coordinates from tag-relative coordinates. + /* Both the input and output coordinates are 2D homogeneous vectors, with y = Hx. + * 'y' are pixel coordinates, 'x' are tag-relative coordinates. Tag coordinates span + * from (-1,-1) to (1,1). The orientation of the homography reflects the orientation + * of the target. + */ + Eigen::Matrix3d homography; + + //! Orientation in the xy plane + float getXYOrientation() const; + + //! The homography is relative to image center, whose coordinates are below. + std::pair hxy; + + //! Interpolate point given (x,y) is in tag coordinate space from (-1,-1) to (1,1). + std::pair interpolate(float x, float y) const; + + //! Used to eliminate redundant tags + bool overlapsTooMuch(const TagDetection &other) const; + + //! Relative pose of tag with respect to the camera + /* Returns the relative location and orientation of the tag using a + 4x4 homogeneous transformation matrix (see Hartley&Zisserman, + Multi-View Geometry, 2003). Requires knowledge of physical tag + size (side length of black square in meters) as well as camera + calibration (focal length and principal point); Result is in + camera frame (z forward, x right, y down) + */ + Eigen::Matrix4d getRelativeTransform(double tag_size, double fx, double fy, + double px, double py) const; + + //! Recover rotation matrix and translation vector of April tag relative to camera. + // Result is in object frame (x forward, y left, z up) + void getRelativeTranslationRotation(double tag_size, double fx, double fy, double px, double py, + Eigen::Vector3d& trans, Eigen::Matrix3d& rot) const; + + //! Draw the detection within the supplied image, including boarders and tag ID. + void draw(cv::Mat& image) const; + + //! Compare function to sort detections by std::sort + static bool sortByIdCompare (const TagDetection &a, const TagDetection &b) { return (a.id + +#include "opencv2/opencv.hpp" + +#include "apriltags//TagDetection.h" +#include "apriltags//TagFamily.h" +#include "apriltags//FloatImage.h" + +namespace AprilTags { + +class TagDetector { +public: + + const TagFamily thisTagFamily; + + //! Constructor + // note: TagFamily is instantiated here from TagCodes + TagDetector(const TagCodes& tagCodes, const size_t blackBorder=2) : thisTagFamily(tagCodes, blackBorder) {} + + std::vector extractTags(const cv::Mat& image); + +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagFamily.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagFamily.h new file mode 100644 index 0000000..e54e90e --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagFamily.h @@ -0,0 +1,106 @@ +#ifndef TAGFAMILY_H +#define TAGFAMILY_H + +#include +#include +#include +#include +#include + +#include "apriltags//TagDetection.h" +using namespace std; + +namespace AprilTags { + +class TagCodes { +public: + int bits; + int minHammingDistance; + std::vector codes; +public: + TagCodes(int bits, int minHammingDistance, + const unsigned long long* codesA, int num) + : bits(bits), minHammingDistance(minHammingDistance), + codes(codesA, codesA+num) // created vector for all entries of codesA + {} +}; + +//! Generic class for all tag encoding families +class TagFamily { +public: + //! The codes array is not copied internally and so must not be modified externally. + TagFamily(const TagCodes& tagCodes, const size_t blackBorder); + + void setErrorRecoveryBits(int b); + + void setErrorRecoveryFraction(float v); + + /* if the bits in w were arranged in a d*d grid and that grid was + * rotated, what would the new bits in w be? + * The bits are organized like this (for d = 3): + * + * 8 7 6 2 5 8 0 1 2 + * 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice) + * 2 1 0 0 3 6 6 7 8 + */ + static unsigned long long rotate90(unsigned long long w, int d); + + //! Computes the hamming distance between two unsigned long longs. + static int hammingDistance(unsigned long long a, unsigned long long b); + + //! How many bits are set in the unsigned long long? + static unsigned char popCountReal(unsigned long long w); + + static int popCount(unsigned long long w); + + //! Given an observed tag with code 'rCode', try to recover the id. + /* The corresponding fields of TagDetection will be filled in. */ + void decode(TagDetection& det, unsigned long long rCode) const; + + //! Prints the hamming distances of the tag codes. + void printHammingDistances() const; + + //! Numer of pixels wide of the inner black border. + int blackBorder; + + //! Number of bits in the tag. Must be n^2. + int bits; + + //! Dimension of tag. e.g. for 16 bits, dimension=4. Must be sqrt(bits). + int dimension; + + //! Minimum hamming distance between any two codes. + /* Accounting for rotational ambiguity? The code can recover + * (minHammingDistance-1)/2 bit errors. + */ + int minimumHammingDistance; + + /* The error recovery value determines our position on the ROC + * curve. We will report codes that are within errorRecoveryBits + * of a valid code. Small values mean greater rejection of bogus + * tags (but false negatives). Large values mean aggressive + * reporting of bad tags (but with a corresponding increase in + * false positives). + */ + int errorRecoveryBits; + + //! The array of the codes. The id for a code is its index. + std::vector codes; + + static const int popCountTableShift = 12; + static const unsigned int popCountTableSize = 1 << popCountTableShift; + static unsigned char popCountTable[popCountTableSize]; + + //! Initializes the static popCountTable + static class TableInitializer { + public: + TableInitializer() { + for (unsigned int i = 0; i < TagFamily::popCountTableSize; i++) + TagFamily::popCountTable[i] = TagFamily::popCountReal(i); + } + } initializer; +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/UnionFindSimple.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/UnionFindSimple.h new file mode 100644 index 0000000..841f576 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/UnionFindSimple.h @@ -0,0 +1,41 @@ +#ifndef UNIONFINDSIMPLE_H +#define UNIONFINDSIMPLE_H + +#include + +namespace AprilTags { + +//! Implementation of disjoint set data structure using the union-find algorithm +class UnionFindSimple { + //! Identifies parent ids and sizes. + struct Data { + int id; + int size; + }; + +public: + explicit UnionFindSimple(int maxId) : data(maxId) { + init(); + }; + + int getSetSize(int thisId) { return data[getRepresentative(thisId)].size; } + + int getRepresentative(int thisId); + + //! Returns the id of the merged node. + /* @param aId + * @param bId + */ + int connectNodes(int aId, int bId); + + void printDataVector() const; + +private: + void init(); + + std::vector data; +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/XYWeight.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/XYWeight.h new file mode 100644 index 0000000..064b73d --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/XYWeight.h @@ -0,0 +1,19 @@ +#ifndef XYWeight_H_ +#define XYWeight_H_ + +namespace AprilTags { + +//! Represents a triple holding an x value, y value, and weight value. +struct XYWeight { + float x; + float y; + float weight; + + XYWeight(float xval, float yval, float weightval) : + x(xval), y(yval), weight(weightval) {} + +}; + +} // namespace + +#endif diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/pch.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/pch.h new file mode 100644 index 0000000..1e4f58f --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/pch.h @@ -0,0 +1,17 @@ +#include "apriltags/Edge.h" +#include "apriltags/FloatImage.h" +#include "apriltags/GLine2D.h" +//#include "apriltags/GLineSegment2D.h" +#include "apriltags/Gaussian.h" +//#include "apriltags/GrayModel.h" +//#include "apriltags/Gridder.h" +#include "apriltags/Homography33.h" +#include "apriltags/MathUtil.h" +//#include "apriltags/Quad.h" +//#include "apriltags/Segment.h" +//#include "apriltags/TagDetection.h" +//#include "apriltags/TagDetector.h" +//#include "apriltags/TagFamily.h" +#include "apriltags/UnionFindSimple.h" +#include "apriltags/XYWeight.h" + diff --git a/thirdparty/apriltag/ethz_apriltag2/package.xml b/thirdparty/apriltag/ethz_apriltag2/package.xml new file mode 100644 index 0000000..4fc8b1c --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/package.xml @@ -0,0 +1,13 @@ + + + ethz_apriltag2 + ethz_apriltag2 + 0.0.1 + Paul Furgale + Paul Furgale + New BSD + catkin + cmake_modules + opencv + eigen2 + diff --git a/thirdparty/apriltag/ethz_apriltag2/src/Edge.cc b/thirdparty/apriltag/ethz_apriltag2/src/Edge.cc new file mode 100644 index 0000000..d2d21d0 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/Edge.cc @@ -0,0 +1,119 @@ +#include "apriltags/Edge.h" +#include "apriltags/FloatImage.h" +#include "apriltags/MathUtil.h" +#include "apriltags/UnionFindSimple.h" + +namespace AprilTags { + +float const Edge::minMag = 0.004f; +float const Edge::maxEdgeCost = 30.f * float(M_PI) / 180.f; +int const Edge::WEIGHT_SCALE = 100; +float const Edge::thetaThresh = 100; +float const Edge::magThresh = 1200; + +int Edge::edgeCost(float theta0, float theta1, float mag1) { + if (mag1 < minMag) // mag0 was checked by the main routine so no need to recheck here + return -1; + + const float thetaErr = std::abs(MathUtil::mod2pi(theta1 - theta0)); + if (thetaErr > maxEdgeCost) + return -1; + + const float normErr = thetaErr / maxEdgeCost; + return (int) (normErr*WEIGHT_SCALE); +} + +void Edge::calcEdges(float theta0, int x, int y, + const FloatImage& theta, const FloatImage& mag, + std::vector &edges, size_t &nEdges) { + int width = theta.getWidth(); + int thisPixel = y*width+x; + + // horizontal edge + int cost1 = edgeCost(theta0, theta.get(x+1,y), mag.get(x+1,y)); + if (cost1 >= 0) { + edges[nEdges].cost = cost1; + edges[nEdges].pixelIdxA = thisPixel; + edges[nEdges].pixelIdxB = y*width+x+1; + ++nEdges; + } + + // vertical edge + int cost2 = edgeCost(theta0, theta.get(x, y+1), mag.get(x,y+1)); + if (cost2 >= 0) { + edges[nEdges].cost = cost2; + edges[nEdges].pixelIdxA = thisPixel; + edges[nEdges].pixelIdxB = (y+1)*width+x; + ++nEdges; + } + + // downward diagonal edge + int cost3 = edgeCost(theta0, theta.get(x+1, y+1), mag.get(x+1,y+1)); + if (cost3 >= 0) { + edges[nEdges].cost = cost3; + edges[nEdges].pixelIdxA = thisPixel; + edges[nEdges].pixelIdxB = (y+1)*width+x+1; + ++nEdges; + } + + // updward diagonal edge + int cost4 = (x == 0) ? -1 : edgeCost(theta0, theta.get(x-1, y+1), mag.get(x-1,y+1)); + if (cost4 >= 0) { + edges[nEdges].cost = cost4; + edges[nEdges].pixelIdxA = thisPixel; + edges[nEdges].pixelIdxB = (y+1)*width+x-1; + ++nEdges; + } +} + +void Edge::mergeEdges(std::vector &edges, UnionFindSimple &uf, + float tmin[], float tmax[], float mmin[], float mmax[]) { + for (size_t i = 0; i < edges.size(); i++) { + int ida = edges[i].pixelIdxA; + int idb = edges[i].pixelIdxB; + + ida = uf.getRepresentative(ida); + idb = uf.getRepresentative(idb); + + if (ida == idb) + continue; + + int sza = uf.getSetSize(ida); + int szb = uf.getSetSize(idb); + + float tmina = tmin[ida], tmaxa = tmax[ida]; + float tminb = tmin[idb], tmaxb = tmax[idb]; + + float costa = (tmaxa-tmina); + float costb = (tmaxb-tminb); + + // bshift will be a multiple of 2pi that aligns the spans of 'b' with 'a' + // so that we can properly take the union of them. + float bshift = MathUtil::mod2pi((tmina+tmaxa)/2, (tminb+tmaxb)/2) - (tminb+tmaxb)/2; + + float tminab = min(tmina, tminb + bshift); + float tmaxab = max(tmaxa, tmaxb + bshift); + + if (tmaxab-tminab > 2*(float)M_PI) // corner case that's probably not too useful to handle correctly, oh well. + tmaxab = tminab + 2*(float)M_PI; + + float mminab = min(mmin[ida], mmin[idb]); + float mmaxab = max(mmax[ida], mmax[idb]); + + // merge these two clusters? + float costab = (tmaxab - tminab); + if (costab <= (min(costa, costb) + Edge::thetaThresh/(sza+szb)) && + (mmaxab-mminab) <= min(mmax[ida]-mmin[ida], mmax[idb]-mmin[idb]) + Edge::magThresh/(sza+szb)) { + + int idab = uf.connectNodes(ida, idb); + + tmin[idab] = tminab; + tmax[idab] = tmaxab; + + mmin[idab] = mminab; + mmax[idab] = mmaxab; + } + } +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/FloatImage.cc b/thirdparty/apriltag/ethz_apriltag2/src/FloatImage.cc new file mode 100644 index 0000000..dba9019 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/FloatImage.cc @@ -0,0 +1,77 @@ +#include "apriltags/FloatImage.h" +#include "apriltags/Gaussian.h" +#include + +namespace AprilTags { + +FloatImage::FloatImage() : width(0), height(0), pixels() {} + +FloatImage::FloatImage(int widthArg, int heightArg) + : width(widthArg), height(heightArg), pixels(widthArg*heightArg) {} + +FloatImage::FloatImage(int widthArg, int heightArg, const std::vector& pArg) + : width(widthArg), height(heightArg), pixels(pArg) {} + +FloatImage& FloatImage::operator=(const FloatImage& other) { + width = other.width; + height = other.height; + if (pixels.size() != other.pixels.size()) + pixels.resize(other.pixels.size()); + pixels = other.pixels; + return *this; +} + +void FloatImage::decimateAvg() { + int nWidth = width/2; + int nHeight = height/2; + + for (int y = 0; y < nHeight; y++) + for (int x = 0; x < nWidth; x++) + pixels[y*nWidth+x] = pixels[(2*y)*width + (2*x)]; + + width = nWidth; + height = nHeight; + pixels.resize(nWidth*nHeight); +} + +void FloatImage::normalize() { + const float maxVal = *max_element(pixels.begin(),pixels.end()); + const float minVal = *min_element(pixels.begin(),pixels.end()); + const float range = maxVal - minVal; + const float rescale = 1 / range; + for ( unsigned int i = 0; i < pixels.size(); i++ ) + pixels[i] = (pixels[i]-minVal) * rescale; +} + +void FloatImage::filterFactoredCentered(const std::vector& fhoriz, const std::vector& fvert) { + // do horizontal + std::vector r(pixels); + + for (int y = 0; y < height; y++) { + Gaussian::convolveSymmetricCentered(pixels, y*width, width, fhoriz, r, y*width); + } + + // do vertical + std::vector tmp(height); // column before convolution + std::vector tmp2(height); // column after convolution + + for (int x = 0; x < width; x++) { + + // copy the column out for locality + for (int y = 0; y < height; y++) + tmp[y] = r[y*width + x]; + + Gaussian::convolveSymmetricCentered(tmp, 0, height, fvert, tmp2, 0); + + for (int y = 0; y < height; y++) + pixels[y*width + x] = tmp2[y]; + } +} + +void FloatImage::printMinMax() const { + std::cout << "Min: " << *min_element(pixels.begin(),pixels.end()) << ", Max: " << *max_element(pixels.begin(),pixels.end()) << std::endl; + //for (int i = 0; i < getNumFloatImagePixels(); i++) + // std::cout << "Index[" << i << "]: " << this->normalize().getFloatImagePixels()[i] << endl; +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/GLine2D.cc b/thirdparty/apriltag/ethz_apriltag2/src/GLine2D.cc new file mode 100644 index 0000000..4bb0926 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/GLine2D.cc @@ -0,0 +1,109 @@ +#include "apriltags/GLine2D.h" + +namespace AprilTags { + +GLine2D::GLine2D() + : dx(0), dy(0), p(0,0), didNormalizeSlope(false), didNormalizeP(false) {} + +GLine2D::GLine2D(float slope, float b) + : dx(1), dy(slope), p(0,b), didNormalizeSlope(false), didNormalizeP(false){} + +GLine2D::GLine2D(float dX, float dY, const std::pair& pt) + : dx(dX), dy(dY), p(pt), didNormalizeSlope(false), didNormalizeP(false) {} + +GLine2D::GLine2D(const std::pair& p1, const std::pair& p2) + : dx(p2.first - p1.first), dy(p2.second - p1.second), p(p1), didNormalizeSlope(false), didNormalizeP(false) {} + +float GLine2D::getLineCoordinate(const std::pair& pt) { + normalizeSlope(); + return pt.first*dx + pt.second*dy; +} + +std::pair GLine2D::getPointOfCoordinate(float coord) { + normalizeP(); + return std::pair(p.first + coord*dx, p.second + coord*dy); +} + +std::pair GLine2D::intersectionWith(const GLine2D& line) const { + float m00 = dx; + float m01 = -line.getDx(); + float m10 = dy; + float m11 = -line.getDy(); + + // determinant of 'm' + float det = m00*m11 - m01*m10; + + // parallel lines? if so, return (-1,0). + if (fabs(det) < 1e-10) + return std::pair(-1,0); + + // inverse of 'm' + float i00 = m11/det; + // float i11 = m00/det; + float i01 = -m01/det; + // float i10 = -m10/det; + + float b00 = line.getFirst() - p.first; + float b10 = line.getSecond() - p.second; + + float x00 = i00*b00 + i01*b10; + + return std::pair(dx*x00+p.first, dy*x00+p.second); +} + +GLine2D GLine2D::lsqFitXYW(const std::vector& xyweights) { + float Cxx=0, Cyy=0, Cxy=0, Ex=0, Ey=0, mXX=0, mYY=0, mXY=0, mX=0, mY=0; + float n=0; + + int idx = 0; + for (unsigned int i = 0; i < xyweights.size(); i++) { + float x = xyweights[i].x; + float y = xyweights[i].y; + float alpha = xyweights[i].weight; + + mY += y*alpha; + mX += x*alpha; + mYY += y*y*alpha; + mXX += x*x*alpha; + mXY += x*y*alpha; + n += alpha; + + idx++; + } + + Ex = mX/n; + Ey = mY/n; + Cxx = mXX/n - MathUtil::square(mX/n); + Cyy = mYY/n - MathUtil::square(mY/n); + Cxy = mXY/n - (mX/n)*(mY/n); + + // find dominant direction via SVD + float phi = 0.5f*std::atan2(-2*Cxy,(Cyy-Cxx)); + // float rho = Ex*cos(phi) + Ey*sin(phi); //why is this needed if he never uses it? + std::pair pts = std::pair(Ex,Ey); + + // compute line parameters + return GLine2D(-std::sin(phi), std::cos(phi), pts); +} + +void GLine2D::normalizeSlope() { + if ( !didNormalizeSlope ) { + float mag = std::sqrt(dx*dx+dy*dy); + dx /= mag; + dy /= mag; + didNormalizeSlope=true; + } +} + +void GLine2D::normalizeP() { + if ( !didNormalizeP ) { + normalizeSlope(); + // we already have a point (P) on the line, and we know the line vector U + // and its perpendicular vector V: so, P'=P.*V *V + float dotprod = -dy*p.first + dx*p.second; + p = std::pair(-dy*dotprod, dx*dotprod); + didNormalizeP = true; + } +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/GLineSegment2D.cc b/thirdparty/apriltag/ethz_apriltag2/src/GLineSegment2D.cc new file mode 100644 index 0000000..2177b48 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/GLineSegment2D.cc @@ -0,0 +1,26 @@ +#include "apriltags/GLineSegment2D.h" +#include + +namespace AprilTags { + +GLineSegment2D::GLineSegment2D(const std::pair& p0Arg, const std::pair& p1Arg) +: line(p0Arg,p1Arg), p0(p0Arg), p1(p1Arg), weight() {} + +GLineSegment2D GLineSegment2D::lsqFitXYW(const std::vector& xyweight) { + GLine2D gline = GLine2D::lsqFitXYW(xyweight); + float maxcoord = -std::numeric_limits::infinity(); + float mincoord = std::numeric_limits::infinity();; + + for (unsigned int i = 0; i < xyweight.size(); i++) { + std::pair p(xyweight[i].x, xyweight[i].y); + float coord = gline.getLineCoordinate(p); + maxcoord = std::max(maxcoord, coord); + mincoord = std::min(mincoord, coord); + } + + std::pair minValue = gline.getPointOfCoordinate(mincoord); + std::pair maxValue = gline.getPointOfCoordinate(maxcoord); + return GLineSegment2D(minValue,maxValue); +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/Gaussian.cc b/thirdparty/apriltag/ethz_apriltag2/src/Gaussian.cc new file mode 100644 index 0000000..52c0ae8 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/Gaussian.cc @@ -0,0 +1,71 @@ +#include "apriltags/Gaussian.h" +#include + +namespace AprilTags { + +bool Gaussian::warned = false; + +std::vector Gaussian::makeGaussianFilter(float sigma, int n) { + std::vector f(n,0.0f); + + if (sigma == 0) { + for (int i = 0; i < n; i++) + f[i] = 0; + f[n/2] = 1; + return f; + } + + double const inv_variance = 1./(2*sigma*sigma); + float sum = 0; + for (int i = 0; i < n; i++) { + int j = i - n/2; + f[i] = (float)std::exp(-j*j * inv_variance); + sum += f[i]; + } + + // normalize the gaussian + for (int i = 0; i < n; i++) + f[i] /= sum; + + return f; +} + +void Gaussian::convolveSymmetricCentered(const std::vector& a, unsigned int aoff, unsigned int alen, + const std::vector& f, std::vector& r, unsigned int roff) { + if ((f.size()&1)== 0 && !warned) { + std::cout<<"convolveSymmetricCentered Warning: filter is not odd length\n"; + warned = true; + } + + for (size_t i = f.size()/2; i < f.size(); i++) { + double acc = 0; + for (size_t j = 0; j < f.size(); j++) { + if ((aoff + i) < j || (aoff + i) >= (alen + j)) + acc += a[aoff] * f[j]; + else + acc += a[aoff + i - j] * f[j]; + } + r[roff + i - f.size()/2] = (float)acc; + } + + for (size_t i = f.size(); i < alen; i++) { + double acc = 0; + for (unsigned int j = 0; j < f.size(); j++) { + acc += a[aoff + i - j] * f[j]; + } + r[roff + i - f.size()/2] = (float)acc; + } + + for (size_t i = alen; i < alen + f.size()/2; i++) { + double acc = 0; + for (size_t j = 0; j < f.size(); j++) { + if ((aoff + i) >= (alen + j) || (aoff + i) < j) + acc += a[aoff + alen - 1] * f[j]; + else + acc += a[aoff + i - j] * f[j]; + } + r[roff + i - f.size()/2] = (float)acc; + } +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/GrayModel.cc b/thirdparty/apriltag/ethz_apriltag2/src/GrayModel.cc new file mode 100644 index 0000000..129d2ec --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/GrayModel.cc @@ -0,0 +1,81 @@ +#include + +#include +#include + +#include "apriltags/GrayModel.h" + +namespace AprilTags { + +GrayModel::GrayModel() : A(), v(), b(), nobs(0), dirty(false) { + A.setZero(); + v.setZero(); + b.setZero(); +} + +void GrayModel::addObservation(float x, float y, float gray) { + float xy = x*y; + + // update only upper-right elements. A'A is symmetric, + // we'll fill the other elements in later. + A(0,0) += x*x; + A(0,1) += x*y; + A(0,2) += x*xy; + A(0,3) += x; + A(1,1) += y*y; + A(1,2) += y*xy; + A(1,3) += y; + A(2,2) += xy*xy; + A(2,3) += xy; + A(3,3) += 1; + + b[0] += x*gray; + b[1] += y*gray; + b[2] += xy*gray; + b[3] += gray; + + nobs++; + dirty = true; +} + +float GrayModel::interpolate(float x, float y) { + if (dirty) compute(); + return v[0]*x + v[1]*y + v[2]*x*y + v[3]; +} + +void GrayModel::compute() { + // we really only need 4 linearly independent observations to fit our answer, but we'll be very + // sensitive to noise if we don't have an over-determined system. Thus, require at least 6 + // observations (or we'll use a constant model below). + + dirty = false; + if (nobs >= 6) { + // make symmetric + Eigen::Matrix4d Ainv; + for (int i = 0; i < 4; i++) + for (int j = i+1; j < 4; j++) + A(j,i) = A(i,j); + + // try { + // Ainv = A.inverse(); + bool invertible; + double det_unused; + A.computeInverseAndDetWithCheck(Ainv, det_unused, invertible); + if (invertible) { + v = Ainv * b; + return; + } + std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n"; + // } + // catch (std::underflow_error&) { + // std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n"; + // } + } + + // If we get here, either nobs < 6 or the matrix inverse generated + // an underflow, so use a constant model. + v.setZero(); // need the cast to avoid operator= ambiguity wrt. const-ness + v[3] = b[3] / nobs; +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/Homography33.cc b/thirdparty/apriltag/ethz_apriltag2/src/Homography33.cc new file mode 100644 index 0000000..87c906a --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/Homography33.cc @@ -0,0 +1,216 @@ +//-*-c++-*- + +#include + +#include + +#include + +#include "apriltags/Homography33.h" + +Homography33::Homography33(const std::pair &opticalCenter) : cxy(opticalCenter), fA(), H(), valid(false) { + fA.setZero(); + H.setZero(); +} + +Eigen::Matrix3d& Homography33::getH() { + compute(); + return H; +} + +#ifdef STABLE_H +void Homography33::setCorrespondences(const std::vector< std::pair > &sPts, + const std::vector< std::pair > &dPts) { + valid = false; + srcPts = sPts; + dstPts = dPts; +} +#else +void Homography33::addCorrespondence(float worldx, float worldy, float imagex, float imagey) { + valid = false; + imagex -= cxy.first; + imagey -= cxy.second; + + /* Here are the rows of matrix A. We will compute A'*A + * A[3*i+0][3] = -worldxyh[i][0]*imagexy[i][2]; + * A[3*i+0][4] = -worldxyh[i][1]*imagexy[i][2]; + * A[3*i+0][5] = -worldxyh[i][2]*imagexy[i][2]; + * A[3*i+0][6] = worldxyh[i][0]*imagexy[i][1]; + * A[3*i+0][7] = worldxyh[i][1]*imagexy[i][1]; + * A[3*i+0][8] = worldxyh[i][2]*imagexy[i][1]; + * + * A[3*i+1][0] = worldxyh[i][0]*imagexy[i][2]; + * A[3*i+1][1] = worldxyh[i][1]*imagexy[i][2]; + * A[3*i+1][2] = worldxyh[i][2]*imagexy[i][2]; + * A[3*i+1][6] = -worldxyh[i][0]*imagexy[i][0]; + * A[3*i+1][7] = -worldxyh[i][1]*imagexy[i][0]; + * A[3*i+1][8] = -worldxyh[i][2]*imagexy[i][0]; + * + * A[3*i+2][0] = -worldxyh[i][0]*imagexy[i][1]; + * A[3*i+2][1] = -worldxyh[i][1]*imagexy[i][1]; + * A[3*i+2][2] = -worldxyh[i][2]*imagexy[i][1]; + * A[3*i+2][3] = worldxyh[i][0]*imagexy[i][0]; + * A[3*i+2][4] = worldxyh[i][1]*imagexy[i][0]; + * A[3*i+2][5] = worldxyh[i][2]*imagexy[i][0]; + */ + + // only update upper-right. A'A is symmetric, we'll finish the lower left later. + float a03 = -worldx; + float a04 = -worldy; + float a05 = -1; + float a06 = worldx*imagey; + float a07 = worldy*imagey; + float a08 = imagey; + + fA(3, 3) += a03*a03; + fA(3, 4) += a03*a04; + fA(3, 5) += a03*a05; + fA(3, 6) += a03*a06; + fA(3, 7) += a03*a07; + fA(3, 8) += a03*a08; + + fA(4, 4) += a04*a04; + fA(4, 5) += a04*a05; + fA(4, 6) += a04*a06; + fA(4, 7) += a04*a07; + fA(4, 8) += a04*a08; + + fA(5, 5) += a05*a05; + fA(5, 6) += a05*a06; + fA(5, 7) += a05*a07; + fA(5, 8) += a05*a08; + + fA(6, 6) += a06*a06; + fA(6, 7) += a06*a07; + fA(6, 8) += a06*a08; + + fA(7, 7) += a07*a07; + fA(7, 8) += a07*a08; + + fA(8, 8) += a08*a08; + + float a10 = worldx; + float a11 = worldy; + float a12 = 1; + float a16 = -worldx*imagex; + float a17 = -worldy*imagex; + float a18 = -imagex; + + fA(0, 0) += a10*a10; + fA(0, 1) += a10*a11; + fA(0, 2) += a10*a12; + fA(0, 6) += a10*a16; + fA(0, 7) += a10*a17; + fA(0, 8) += a10*a18; + + fA(1, 1) += a11*a11; + fA(1, 2) += a11*a12; + fA(1, 6) += a11*a16; + fA(1, 7) += a11*a17; + fA(1, 8) += a11*a18; + + fA(2, 2) += a12*a12; + fA(2, 6) += a12*a16; + fA(2, 7) += a12*a17; + fA(2, 8) += a12*a18; + + fA(6, 6) += a16*a16; + fA(6, 7) += a16*a17; + fA(6, 8) += a16*a18; + + fA(7, 7) += a17*a17; + fA(7, 8) += a17*a18; + + fA(8, 8) += a18*a18; + + float a20 = -worldx*imagey; + float a21 = -worldy*imagey; + float a22 = -imagey; + float a23 = worldx*imagex; + float a24 = worldy*imagex; + float a25 = imagex; + + fA(0, 0) += a20*a20; + fA(0, 1) += a20*a21; + fA(0, 2) += a20*a22; + fA(0, 3) += a20*a23; + fA(0, 4) += a20*a24; + fA(0, 5) += a20*a25; + + fA(1, 1) += a21*a21; + fA(1, 2) += a21*a22; + fA(1, 3) += a21*a23; + fA(1, 4) += a21*a24; + fA(1, 5) += a21*a25; + + fA(2, 2) += a22*a22; + fA(2, 3) += a22*a23; + fA(2, 4) += a22*a24; + fA(2, 5) += a22*a25; + + fA(3, 3) += a23*a23; + fA(3, 4) += a23*a24; + fA(3, 5) += a23*a25; + + fA(4, 4) += a24*a24; + fA(4, 5) += a24*a25; + + fA(5, 5) += a25*a25; +} +#endif + +#ifdef STABLE_H +void Homography33::compute() { + if ( valid ) return; + + std::vector sPts; + std::vector dPts; + for (int i=0; i<4; i++) { + sPts.push_back(cv::Point2f(srcPts[i].first, srcPts[i].second)); + } + for (int i=0; i<4; i++) { + dPts.push_back(cv::Point2f(dstPts[i].first - cxy.first, dstPts[i].second - cxy.second)); + } + cv::Mat homography = cv::findHomography(sPts, dPts); + for (int c=0; c<3; c++) { + for (int r=0; r<3; r++) { + H(r,c) = homography.at(r,c); + } + } + + valid = true; +} +#else +void Homography33::compute() { + if ( valid ) return; + + // make symmetric + for (int i = 0; i < 9; i++) + for (int j = i+1; j < 9; j++) + fA(j,i) = fA(i,j); + + Eigen::JacobiSVD svd(fA, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::MatrixXd eigV = svd.matrixV(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + H(i,j) = eigV(i*3+j, eigV.cols()-1); + } + } + + valid = true; +} +#endif + +std::pair Homography33::project(float worldx, float worldy) { + compute(); + + std::pair ixy; + ixy.first = H(0,0)*worldx + H(0,1)*worldy + H(0,2); + ixy.second = H(1,0)*worldx + H(1,1)*worldy + H(1,2); + float z = H(2,0)*worldx + H(2,1)*worldy + H(2,2); + ixy.first = ixy.first/z + cxy.first; + ixy.second = ixy.second/z + cxy.second; + return ixy; +} + diff --git a/thirdparty/apriltag/ethz_apriltag2/src/MathUtil.cc b/thirdparty/apriltag/ethz_apriltag2/src/MathUtil.cc new file mode 100644 index 0000000..8f4adf3 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/MathUtil.cc @@ -0,0 +1,11 @@ +#include "apriltags/MathUtil.h" + +namespace AprilTags{ + +// Output operator for std::pair, useful for debugging +std::ostream& operator<<(std::ostream &os, const std::pair &pt) { + os << pt.first << "," << pt.second; + return os; +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/Quad.cc b/thirdparty/apriltag/ethz_apriltag2/src/Quad.cc new file mode 100644 index 0000000..cc8071d --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/Quad.cc @@ -0,0 +1,159 @@ +#include + +#include "apriltags/FloatImage.h" +#include "apriltags/MathUtil.h" +#include "apriltags/GLine2D.h" +#include "apriltags/Quad.h" +#include "apriltags/Segment.h" + +namespace AprilTags { + +const float Quad::maxQuadAspectRatio = 32; + +Quad::Quad(const std::vector< std::pair >& p, const std::pair& opticalCenter) + : quadPoints(p), segments(), observedPerimeter(), homography(opticalCenter) { +#ifdef STABLE_H + std::vector< std::pair > srcPts; + srcPts.push_back(std::make_pair(-1, -1)); + srcPts.push_back(std::make_pair( 1, -1)); + srcPts.push_back(std::make_pair( 1, 1)); + srcPts.push_back(std::make_pair(-1, 1)); + homography.setCorrespondences(srcPts, p); +#else + homography.addCorrespondence(-1, -1, quadPoints[0].first, quadPoints[0].second); + homography.addCorrespondence( 1, -1, quadPoints[1].first, quadPoints[1].second); + homography.addCorrespondence( 1, 1, quadPoints[2].first, quadPoints[2].second); + homography.addCorrespondence(-1, 1, quadPoints[3].first, quadPoints[3].second); +#endif + +#ifdef INTERPOLATE + p0 = Eigen::Vector2f(p[0].first, p[0].second); + p3 = Eigen::Vector2f(p[3].first, p[3].second); + p01 = (Eigen::Vector2f(p[1].first, p[1].second) - p0); + p32 = (Eigen::Vector2f(p[2].first, p[2].second) - p3); +#endif +} + +std::pair Quad::interpolate(float x, float y) { +#ifdef INTERPOLATE + Eigen::Vector2f r1 = p0 + p01 * (x+1.)/2.; + Eigen::Vector2f r2 = p3 + p32 * (x+1.)/2.; + Eigen::Vector2f r = r1 + (r2-r1) * (y+1)/2; + return std::pair(r(0), r(1)); +#else + return homography.project(x,y); +#endif +} + +std::pair Quad::interpolate01(float x, float y) { + return interpolate(2*x-1, 2*y-1); +} + +void Quad::search(const FloatImage& fImage, std::vector& path, + Segment& parent, int depth, std::vector& quads, + const std::pair& opticalCenter) { + // cout << "Searching segment " << parent.getId() << ", depth=" << depth << ", #children=" << parent.children.size() << endl; + // terminal depth occurs when we've found four segments. + if (depth == 4) { + // cout << "Entered terminal depth" << endl; // debug code + + // Is the first segment the same as the last segment (i.e., a loop?) + if (path[4] == path[0]) { + // the 4 corners of the quad as computed by the intersection of segments. + std::vector< std::pair > p(4); + float calculatedPerimeter = 0; + bool bad = false; + for (int i = 0; i < 4; i++) { + // compute intersections between all the lines. This will give us + // sub-pixel accuracy for the corners of the quad. + GLine2D linea(std::make_pair(path[i]->getX0(),path[i]->getY0()), + std::make_pair(path[i]->getX1(),path[i]->getY1())); + GLine2D lineb(std::make_pair(path[i+1]->getX0(),path[i+1]->getY0()), + std::make_pair(path[i+1]->getX1(),path[i+1]->getY1())); + + p[i] = linea.intersectionWith(lineb); + calculatedPerimeter += path[i]->getLength(); + + // no intersection? Occurs when the lines are almost parallel. + if (p[i].first == -1) + bad = true; + } + // cout << "bad = " << bad << endl; + // eliminate quads that don't form a simply connected loop, i.e., those + // that form an hour glass, or wind the wrong way. + if (!bad) { + float t0 = std::atan2(p[1].second-p[0].second, p[1].first-p[0].first); + float t1 = std::atan2(p[2].second-p[1].second, p[2].first-p[1].first); + float t2 = std::atan2(p[3].second-p[2].second, p[3].first-p[2].first); + float t3 = std::atan2(p[0].second-p[3].second, p[0].first-p[3].first); + + // double ttheta = fmod(t1-t0, 2*M_PI) + fmod(t2-t1, 2*M_PI) + + // fmod(t3-t2, 2*M_PI) + fmod(t0-t3, 2*M_PI); + float ttheta = MathUtil::mod2pi(t1-t0) + MathUtil::mod2pi(t2-t1) + + MathUtil::mod2pi(t3-t2) + MathUtil::mod2pi(t0-t3); + // cout << "ttheta=" << ttheta << endl; + // the magic value is -2*PI. It should be exact, + // but we allow for (lots of) numeric imprecision. + if (ttheta < -7 || ttheta > -5) + bad = true; + } + + if (!bad) { + float d0 = MathUtil::distance2D(p[0], p[1]); + float d1 = MathUtil::distance2D(p[1], p[2]); + float d2 = MathUtil::distance2D(p[2], p[3]); + float d3 = MathUtil::distance2D(p[3], p[0]); + float d4 = MathUtil::distance2D(p[0], p[2]); + float d5 = MathUtil::distance2D(p[1], p[3]); + + // check sizes + if (d0 < Quad::minimumEdgeLength || d1 < Quad::minimumEdgeLength || d2 < Quad::minimumEdgeLength || + d3 < Quad::minimumEdgeLength || d4 < Quad::minimumEdgeLength || d5 < Quad::minimumEdgeLength) { + bad = true; + // cout << "tagsize too small" << endl; + } + + // check aspect ratio + float dmax = max(max(d0,d1), max(d2,d3)); + float dmin = min(min(d0,d1), min(d2,d3)); + + if (dmax > dmin*Quad::maxQuadAspectRatio) { + bad = true; + // cout << "aspect ratio too extreme" << endl; + } + } + + if (!bad) { + Quad q(p, opticalCenter); + q.segments=path; + q.observedPerimeter = calculatedPerimeter; + quads.push_back(q); + } + } + return; + } + + // if (depth >= 1) // debug code + //cout << "depth: " << depth << endl; + + // Not terminal depth. Recurse on any children that obey the correct handedness. + for (unsigned int i = 0; i < parent.children.size(); i++) { + Segment &child = *parent.children[i]; + // cout << " Child " << child.getId() << ": "; + // (handedness was checked when we created the children) + + // we could rediscover each quad 4 times (starting from + // each corner). If we had an arbitrary ordering over + // points, we can eliminate the redundant detections by + // requiring that the first corner have the lowest + // value. We're arbitrarily going to use theta... + if ( child.getTheta() > path[0]->getTheta() ) { + // cout << "theta failed: " << child.getTheta() << " > " << path[0]->getTheta() << endl; + continue; + } + path[depth+1] = &child; + search(fImage, path, child, depth+1, quads, opticalCenter); + } +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/Segment.cc b/thirdparty/apriltag/ethz_apriltag2/src/Segment.cc new file mode 100644 index 0000000..817ed3e --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/Segment.cc @@ -0,0 +1,21 @@ +#include "apriltags/Segment.h" +#include + +namespace AprilTags { + +const float Segment::minimumLineLength = 4; + +Segment::Segment() + : children(), x0(0), y0(0), x1(0), y1(0), theta(0), length(0), segmentId(++idCounter) {} + +float Segment::segmentLength() { + return std::sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0)); +} + +void Segment::printSegment() { + std::cout <<"("<< x0 <<","<< y0 <<"), "<<"("<< x1 <<","<< y1 <<")" << std::endl; +} + +int Segment::idCounter = 0; + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/TagDetection.cc b/thirdparty/apriltag/ethz_apriltag2/src/TagDetection.cc new file mode 100644 index 0000000..422e493 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/TagDetection.cc @@ -0,0 +1,161 @@ + +#include "opencv2/opencv.hpp" + +#include "apriltags/TagDetection.h" +#include "apriltags/MathUtil.h" + +#ifdef PLATFORM_APERIOS +//missing/broken isnan +namespace std { + static bool isnan(float x) { + const int EXP = 0x7f800000; + const int FRAC = 0x007fffff; + const int y = *((int*)(&x)); + return ((y&EXP)==EXP && (y&FRAC)!=0); + } +} +#endif + +namespace AprilTags { + +TagDetection::TagDetection() + : good(false), obsCode(), code(), id(), hammingDistance(), rotation(), p(), + cxy(), observedPerimeter(), homography(), hxy() { + homography.setZero(); +} + +TagDetection::TagDetection(int _id) + : good(false), obsCode(), code(), id(_id), hammingDistance(), rotation(), p(), + cxy(), observedPerimeter(), homography(), hxy() { + homography.setZero(); +} + +float TagDetection::getXYOrientation() const { + // Because the order of segments in a quad is arbitrary, so is the + // homography's rotation, so we can't determine orientation directly + // from the homography. Instead, use the homography to find two + // bottom corners of a properly oriented tag in pixel coordinates, + // and then compute orientation from that. + std::pair p0 = interpolate(-1,-1); // lower left corner of tag + std::pair p1 = interpolate(1,-1); // lower right corner of tag + float orient = atan2(p1.second - p0.second, p1.first - p0.first); + return ! std::isnan(float(orient)) ? orient : 0.; +} + +std::pair TagDetection::interpolate(float x, float y) const { + float z = homography(2,0)*x + homography(2,1)*y + homography(2,2); + if ( z == 0 ) + return std::pair(0,0); // prevents returning a pair with a -NaN, for which gcc 4.4 flubs isnan + float newx = (homography(0,0)*x + homography(0,1)*y + homography(0,2))/z + hxy.first; + float newy = (homography(1,0)*x + homography(1,1)*y + homography(1,2))/z + hxy.second; + return std::pair(newx,newy); +} + +bool TagDetection::overlapsTooMuch(const TagDetection &other) const { + // Compute a sort of "radius" of the two targets. We'll do this by + // computing the average length of the edges of the quads (in + // pixels). + float radius = + ( MathUtil::distance2D(p[0], p[1]) + + MathUtil::distance2D(p[1], p[2]) + + MathUtil::distance2D(p[2], p[3]) + + MathUtil::distance2D(p[3], p[0]) + + MathUtil::distance2D(other.p[0], other.p[1]) + + MathUtil::distance2D(other.p[1], other.p[2]) + + MathUtil::distance2D(other.p[2], other.p[3]) + + MathUtil::distance2D(other.p[3], other.p[0]) ) / 16.0f; + + // distance (in pixels) between two tag centers + float dist = MathUtil::distance2D(cxy, other.cxy); + + // reject pairs where the distance between centroids is smaller than + // the "radius" of one of the tags. + return ( dist < radius ); +} + +Eigen::Matrix4d TagDetection::getRelativeTransform(double tag_size, double fx, double fy, double px, double py) const { + std::vector objPts; + std::vector imgPts; + double s = tag_size/2.; + objPts.push_back(cv::Point3f(-s,-s, 0)); + objPts.push_back(cv::Point3f( s,-s, 0)); + objPts.push_back(cv::Point3f( s, s, 0)); + objPts.push_back(cv::Point3f(-s, s, 0)); + + std::pair p1 = p[0]; + std::pair p2 = p[1]; + std::pair p3 = p[2]; + std::pair p4 = p[3]; + imgPts.push_back(cv::Point2f(p1.first, p1.second)); + imgPts.push_back(cv::Point2f(p2.first, p2.second)); + imgPts.push_back(cv::Point2f(p3.first, p3.second)); + imgPts.push_back(cv::Point2f(p4.first, p4.second)); + + cv::Mat rvec, tvec; + cv::Matx33f cameraMatrix( + fx, 0, px, + 0, fy, py, + 0, 0, 1); + cv::Vec4f distParam(0,0,0,0); // all 0? + cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec); + cv::Matx33d r; + cv::Rodrigues(rvec, r); + Eigen::Matrix3d wRo; + wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2); + + Eigen::Matrix4d T; + T.topLeftCorner(3,3) = wRo; + T.col(3).head(3) << tvec.at(0), tvec.at(1), tvec.at(2); + T.row(3) << 0,0,0,1; + + return T; +} + +void TagDetection::getRelativeTranslationRotation(double tag_size, double fx, double fy, double px, double py, + Eigen::Vector3d& trans, Eigen::Matrix3d& rot) const { + Eigen::Matrix4d T = + getRelativeTransform(tag_size, fx, fy, px, py); + + // converting from camera frame (z forward, x right, y down) to + // object frame (x forward, y left, z up) + Eigen::Matrix4d M; + M << + 0, 0, 1, 0, + -1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, 0, 1; + Eigen::Matrix4d MT = M*T; + // translation vector from camera to the April tag + trans = MT.col(3).head(3); + // orientation of April tag with respect to camera: the camera + // convention makes more sense here, because yaw,pitch,roll then + // naturally agree with the orientation of the object + rot = T.block(0,0,3,3); +} + +// draw one April tag detection on actual image +void TagDetection::draw(cv::Mat& image) const { + // use corner points detected by line intersection + std::pair p1 = p[0]; + std::pair p2 = p[1]; + std::pair p3 = p[2]; + std::pair p4 = p[3]; + + // plot outline + cv::line(image, cv::Point2f(p1.first, p1.second), cv::Point2f(p2.first, p2.second), cv::Scalar(255,0,0,0) ); + cv::line(image, cv::Point2f(p2.first, p2.second), cv::Point2f(p3.first, p3.second), cv::Scalar(0,255,0,0) ); + cv::line(image, cv::Point2f(p3.first, p3.second), cv::Point2f(p4.first, p4.second), cv::Scalar(0,0,255,0) ); + cv::line(image, cv::Point2f(p4.first, p4.second), cv::Point2f(p1.first, p1.second), cv::Scalar(255,0,255,0) ); + + // mark center + cv::circle(image, cv::Point2f(cxy.first, cxy.second), 8, cv::Scalar(0,0,255,0), 2); + + // print ID + std::ostringstream strSt; + strSt << "#" << id; + cv::putText(image, strSt.str(), + cv::Point2f(cxy.first + 10, cxy.second + 10), + cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,255)); +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/TagDetector.cc b/thirdparty/apriltag/ethz_apriltag2/src/TagDetector.cc new file mode 100644 index 0000000..7fdbd0a --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/TagDetector.cc @@ -0,0 +1,613 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include "apriltags/Edge.h" +#include "apriltags/FloatImage.h" +#include "apriltags/GLine2D.h" +#include "apriltags/GLineSegment2D.h" +#include "apriltags/Gaussian.h" +#include "apriltags/GrayModel.h" +#include "apriltags/Gridder.h" +#include "apriltags/Homography33.h" +#include "apriltags/MathUtil.h" +#include "apriltags/Quad.h" +#include "apriltags/Segment.h" +#include "apriltags/TagFamily.h" +#include "apriltags/UnionFindSimple.h" +#include "apriltags/XYWeight.h" + +#include "apriltags/TagDetector.h" + +//#define DEBUG_APRIL + +#ifdef DEBUG_APRIL +#include +#include +#endif + +using namespace std; + +namespace AprilTags { + +std::vector TagDetector::extractTags(const cv::Mat &image) { + // convert to internal AprilTags image (todo: slow, change internally to + // OpenCV) + int width = image.cols; + int height = image.rows; + AprilTags::FloatImage fimOrig(width, height); + int i = 0; + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + fimOrig.set(x, y, image.data[i] / 255.); + i++; + } + } + std::pair opticalCenter(width / 2, height / 2); + +#ifdef DEBUG_APRIL +#if 0 + { // debug - write + int height_ = fimOrig.getHeight(); + int width_ = fimOrig.getWidth(); + cv::Mat image(height_, width_, CV_8UC3); + { + for (int y=0; y(y, x) = v; + } + } + } + imwrite("out.bmp", image); + } +#endif +#if 0 + FloatImage fimOrig = fimOrig_; + { // debug - read + + cv::Mat image = cv::imread("test.bmp"); + int height_ = fimOrig.getHeight(); + int width_ = fimOrig.getWidth(); + { + for (int y=0; y(y,x); + float val = (float)v(0)/255.; + fimOrig.set(x,y,val); + } + } + } + } +#endif +#endif + + //================================================================ + // Step one: preprocess image (convert to grayscale) and low pass if necessary + + FloatImage fim = fimOrig; + + //! Gaussian smoothing kernel applied to image (0 == no filter). + /*! Used when sampling bits. Filtering is a good idea in cases + * where A) a cheap camera is introducing artifical sharpening, B) + * the bayer pattern is creating artifcats, C) the sensor is very + * noisy and/or has hot/cold pixels. However, filtering makes it + * harder to decode very small tags. Reasonable values are 0, or + * [0.8, 1.5]. + */ + float sigma = 0; + + //! Gaussian smoothing kernel applied to image (0 == no filter). + /*! Used when detecting the outline of the box. It is almost always + * useful to have some filtering, since the loss of small details + * won't hurt. Recommended value = 0.8. The case where sigma == + * segsigma has been optimized to avoid a redundant filter + * operation. + */ + float segSigma = 0.8f; + + if (sigma > 0) { + int filtsz = ((int)max(3.0f, 3 * sigma)) | 1; + std::vector filt = Gaussian::makeGaussianFilter(sigma, filtsz); + fim.filterFactoredCentered(filt, filt); + } + + //================================================================ + // Step two: Compute the local gradient. We store the direction and magnitude. + // This step is quite sensitve to noise, since a few bad theta estimates will + // break up segments, causing us to miss Quads. It is useful to do a Gaussian + // low pass on this step even if we don't want it for encoding. + + FloatImage fimSeg; + if (segSigma > 0) { + if (segSigma == sigma) { + fimSeg = fim; + } else { + // blur anew + int filtsz = ((int)max(3.0f, 3 * segSigma)) | 1; + std::vector filt = Gaussian::makeGaussianFilter(segSigma, filtsz); + fimSeg = fimOrig; + fimSeg.filterFactoredCentered(filt, filt); + } + } else { + fimSeg = fimOrig; + } + + FloatImage fimTheta(fimSeg.getWidth(), fimSeg.getHeight()); + FloatImage fimMag(fimSeg.getWidth(), fimSeg.getHeight()); + + for (int y = 1; y < fimSeg.getHeight() - 1; y++) { + for (int x = 1; x < fimSeg.getWidth() - 1; x++) { + float Ix = fimSeg.get(x + 1, y) - fimSeg.get(x - 1, y); + float Iy = fimSeg.get(x, y + 1) - fimSeg.get(x, y - 1); + + float mag = Ix * Ix + Iy * Iy; +#if 0 // kaess: fast version, but maybe less accurate? + float theta = MathUtil::fast_atan2(Iy, Ix); +#else + float theta = atan2(Iy, Ix); +#endif + + fimTheta.set(x, y, theta); + fimMag.set(x, y, mag); + } + } + +#ifdef DEBUG_APRIL + int height_ = fimSeg.getHeight(); + int width_ = fimSeg.getWidth(); + cv::Mat dbgImage(height_, width_, CV_8UC3); + { + for (int y = 0; y < height_; y++) { + for (int x = 0; x < width_; x++) { + cv::Vec3b v; + // float vf = fimMag.get(x,y); + float vf = fimOrig.get(x, y); + int val = (int)(vf * 255.); + if ((val & 0xffff00) != 0) { + printf("problem... %i\n", val); + } + for (int k = 0; k < 3; k++) { + v(k) = val; + } + dbgImage.at(y, x) = v; + } + } + } +#endif + + //================================================================ + // Step three. Extract edges by grouping pixels with similar + // thetas together. This is a greedy algorithm: we start with + // the most similar pixels. We use 4-connectivity. + UnionFindSimple uf(fimSeg.getWidth() * fimSeg.getHeight()); + + vector edges(width * height * 4); + size_t nEdges = 0; + + // Bounds on the thetas assigned to this group. Note that because + // theta is periodic, these are defined such that the average + // value is contained *within* the interval. + { // limit scope of storage + /* Previously all this was on the stack, but this is 1.2MB for 320x240 + * images + * That's already a problem for OS X (default 512KB thread stack size), + * could be a problem elsewhere for bigger images... so store on heap */ + vector storage( + width * height * + 4); // do all the memory in one big block, exception safe + float *tmin = &storage[width * height * 0]; + float *tmax = &storage[width * height * 1]; + float *mmin = &storage[width * height * 2]; + float *mmax = &storage[width * height * 3]; + + for (int y = 0; y + 1 < height; y++) { + for (int x = 0; x + 1 < width; x++) { + float mag0 = fimMag.get(x, y); + if (mag0 < Edge::minMag) continue; + mmax[y * width + x] = mag0; + mmin[y * width + x] = mag0; + + float theta0 = fimTheta.get(x, y); + tmin[y * width + x] = theta0; + tmax[y * width + x] = theta0; + + // Calculates then adds edges to 'vector edges' + Edge::calcEdges(theta0, x, y, fimTheta, fimMag, edges, nEdges); + + // XXX Would 8 connectivity help for rotated tags? + // Probably not much, so long as input filtering hasn't been disabled. + } + } + + edges.resize(nEdges); + std::stable_sort(edges.begin(), edges.end()); + Edge::mergeEdges(edges, uf, tmin, tmax, mmin, mmax); + } + + //================================================================ + // Step four: Loop over the pixels again, collecting statistics for each + // cluster. + // We will soon fit lines (segments) to these points. + + map > clusters; + for (int y = 0; y + 1 < fimSeg.getHeight(); y++) { + for (int x = 0; x + 1 < fimSeg.getWidth(); x++) { + if (uf.getSetSize(y * fimSeg.getWidth() + x) < + Segment::minimumSegmentSize) + continue; + + int rep = (int)uf.getRepresentative(y * fimSeg.getWidth() + x); + + map >::iterator it = clusters.find(rep); + if (it == clusters.end()) { + clusters[rep] = vector(); + it = clusters.find(rep); + } + vector &points = it->second; + points.push_back(XYWeight(x, y, fimMag.get(x, y))); + } + } + + //================================================================ + // Step five: Loop over the clusters, fitting lines (which we call Segments). + std::vector segments; // used in Step six + std::map >::const_iterator clustersItr; + for (clustersItr = clusters.begin(); clustersItr != clusters.end(); + clustersItr++) { + std::vector points = clustersItr->second; + GLineSegment2D gseg = GLineSegment2D::lsqFitXYW(points); + + // filter short lines + float length = MathUtil::distance2D(gseg.getP0(), gseg.getP1()); + if (length < Segment::minimumLineLength) continue; + + Segment seg; + float dy = gseg.getP1().second - gseg.getP0().second; + float dx = gseg.getP1().first - gseg.getP0().first; + + float tmpTheta = std::atan2(dy, dx); + + seg.setTheta(tmpTheta); + seg.setLength(length); + + // We add an extra semantic to segments: the vector + // p1->p2 will have dark on the left, white on the right. + // To do this, we'll look at every gradient and each one + // will vote for which way they think the gradient should + // go. This is way more retentive than necessary: we + // could probably sample just one point! + + float flip = 0, noflip = 0; + for (unsigned int i = 0; i < points.size(); i++) { + XYWeight xyw = points[i]; + + float theta = fimTheta.get((int)xyw.x, (int)xyw.y); + float mag = fimMag.get((int)xyw.x, (int)xyw.y); + + // err *should* be +M_PI/2 for the correct winding, but if we + // got the wrong winding, it'll be around -M_PI/2. + float err = MathUtil::mod2pi(theta - seg.getTheta()); + + if (err < 0) + noflip += mag; + else + flip += mag; + } + + if (flip > noflip) { + float temp = seg.getTheta() + (float)M_PI; + seg.setTheta(temp); + } + + float dot = dx * std::cos(seg.getTheta()) + dy * std::sin(seg.getTheta()); + if (dot > 0) { + seg.setX0(gseg.getP1().first); + seg.setY0(gseg.getP1().second); + seg.setX1(gseg.getP0().first); + seg.setY1(gseg.getP0().second); + } else { + seg.setX0(gseg.getP0().first); + seg.setY0(gseg.getP0().second); + seg.setX1(gseg.getP1().first); + seg.setY1(gseg.getP1().second); + } + + segments.push_back(seg); + } + +#ifdef DEBUG_APRIL +#if 0 + { + for (vector::iterator it = segments.begin(); it!=segments.end(); it++) { + long int r = random(); + cv::line(dbgImage, + cv::Point2f(it->getX0(), it->getY0()), + cv::Point2f(it->getX1(), it->getY1()), + cv::Scalar(r%0xff,(r%0xff00)>>8,(r%0xff0000)>>16,0) ); + } + } +#endif +#endif + + // Step six: For each segment, find segments that begin where this segment + // ends. + // (We will chain segments together next...) The gridder accelerates the + // search by + // building (essentially) a 2D hash table. + Gridder gridder(0, 0, width, height, 10); + + // add every segment to the hash table according to the position of the + // segment's + // first point. Remember that the first point has a specific meaning due to + // our + // left-hand rule above. + for (unsigned int i = 0; i < segments.size(); i++) { + gridder.add(segments[i].getX0(), segments[i].getY0(), &segments[i]); + } + + // Now, find child segments that begin where each parent segment ends. + for (unsigned i = 0; i < segments.size(); i++) { + Segment &parentseg = segments[i]; + + // compute length of the line segment + GLine2D parentLine( + std::pair(parentseg.getX0(), parentseg.getY0()), + std::pair(parentseg.getX1(), parentseg.getY1())); + + Gridder::iterator iter = gridder.find( + parentseg.getX1(), parentseg.getY1(), 0.5f * parentseg.getLength()); + while (iter.hasNext()) { + Segment &child = iter.next(); + if (MathUtil::mod2pi(child.getTheta() - parentseg.getTheta()) > 0) { + continue; + } + + // compute intersection of points + GLine2D childLine(std::pair(child.getX0(), child.getY0()), + std::pair(child.getX1(), child.getY1())); + + std::pair p = parentLine.intersectionWith(childLine); + if (p.first == -1) { + continue; + } + + float parentDist = MathUtil::distance2D( + p, std::pair(parentseg.getX1(), parentseg.getY1())); + float childDist = MathUtil::distance2D( + p, std::pair(child.getX0(), child.getY0())); + + if (max(parentDist, childDist) > parentseg.getLength()) { + // cout << "intersection too far" << endl; + continue; + } + + // everything's OK, this child is a reasonable successor. + parentseg.children.push_back(&child); + } + } + + //================================================================ + // Step seven: Search all connected segments to see if any form a loop of + // length 4. + // Add those to the quads list. + vector quads; + + vector tmp(5); + for (unsigned int i = 0; i < segments.size(); i++) { + tmp[0] = &segments[i]; + Quad::search(fimOrig, tmp, segments[i], 0, quads, opticalCenter); + } + +#ifdef DEBUG_APRIL + { + for (unsigned int qi = 0; qi < quads.size(); qi++) { + Quad &quad = quads[qi]; + std::pair p1 = quad.quadPoints[0]; + std::pair p2 = quad.quadPoints[1]; + std::pair p3 = quad.quadPoints[2]; + std::pair p4 = quad.quadPoints[3]; + cv::line(dbgImage, cv::Point2f(p1.first, p1.second), + cv::Point2f(p2.first, p2.second), cv::Scalar(0, 0, 255, 0)); + cv::line(dbgImage, cv::Point2f(p2.first, p2.second), + cv::Point2f(p3.first, p3.second), cv::Scalar(0, 0, 255, 0)); + cv::line(dbgImage, cv::Point2f(p3.first, p3.second), + cv::Point2f(p4.first, p4.second), cv::Scalar(0, 0, 255, 0)); + cv::line(dbgImage, cv::Point2f(p4.first, p4.second), + cv::Point2f(p1.first, p1.second), cv::Scalar(0, 0, 255, 0)); + + p1 = quad.interpolate(-1, -1); + p2 = quad.interpolate(-1, 1); + p3 = quad.interpolate(1, 1); + p4 = quad.interpolate(1, -1); + cv::circle(dbgImage, cv::Point2f(p1.first, p1.second), 3, + cv::Scalar(0, 255, 0, 0), 1); + cv::circle(dbgImage, cv::Point2f(p2.first, p2.second), 3, + cv::Scalar(0, 255, 0, 0), 1); + cv::circle(dbgImage, cv::Point2f(p3.first, p3.second), 3, + cv::Scalar(0, 255, 0, 0), 1); + cv::circle(dbgImage, cv::Point2f(p4.first, p4.second), 3, + cv::Scalar(0, 255, 0, 0), 1); + } + cv::imshow("debug_april", dbgImage); + } +#endif + + //================================================================ + // Step eight. Decode the quads. For each quad, we first estimate a + // threshold color to decide between 0 and 1. Then, we read off the + // bits and see if they make sense. + + std::vector detections; + + for (unsigned int qi = 0; qi < quads.size(); qi++) { + Quad &quad = quads[qi]; + + // Find a threshold + GrayModel blackModel, whiteModel; + const int dd = 2 * thisTagFamily.blackBorder + thisTagFamily.dimension; + + for (int iy = -1; iy <= dd; iy++) { + float y = (iy + 0.5f) / dd; + for (int ix = -1; ix <= dd; ix++) { + float x = (ix + 0.5f) / dd; + std::pair pxy = quad.interpolate01(x, y); + int irx = (int)(pxy.first + 0.5); + int iry = (int)(pxy.second + 0.5); + if (irx < 0 || irx >= width || iry < 0 || iry >= height) continue; + float v = fim.get(irx, iry); + if (iy == -1 || iy == dd || ix == -1 || ix == dd) + whiteModel.addObservation(x, y, v); + else if (iy == 0 || iy == (dd - 1) || ix == 0 || ix == (dd - 1)) + blackModel.addObservation(x, y, v); + } + } + + bool bad = false; + unsigned long long tagCode = 0; + for (int iy = thisTagFamily.dimension - 1; iy >= 0; iy--) { + float y = (thisTagFamily.blackBorder + iy + 0.5f) / dd; + for (int ix = 0; ix < thisTagFamily.dimension; ix++) { + float x = (thisTagFamily.blackBorder + ix + 0.5f) / dd; + std::pair pxy = quad.interpolate01(x, y); + int irx = (int)(pxy.first + 0.5); + int iry = (int)(pxy.second + 0.5); + if (irx < 0 || irx >= width || iry < 0 || iry >= height) { + // cout << "*** bad: irx=" << irx << " iry=" << iry << endl; + bad = true; + continue; + } + float threshold = + (blackModel.interpolate(x, y) + whiteModel.interpolate(x, y)) * + 0.5f; + float v = fim.get(irx, iry); + tagCode = tagCode << 1; + if (v > threshold) tagCode |= 1; +#ifdef DEBUG_APRIL + { + if (v > threshold) + cv::circle(dbgImage, cv::Point2f(irx, iry), 1, + cv::Scalar(0, 0, 255, 0), 2); + else + cv::circle(dbgImage, cv::Point2f(irx, iry), 1, + cv::Scalar(0, 255, 0, 0), 2); + } +#endif + } + } + + if (!bad) { + TagDetection thisTagDetection; + thisTagFamily.decode(thisTagDetection, tagCode); + + // compute the homography (and rotate it appropriately) + thisTagDetection.homography = quad.homography.getH(); + thisTagDetection.hxy = quad.homography.getCXY(); + + float c = std::cos(thisTagDetection.rotation * (float)M_PI / 2); + float s = std::sin(thisTagDetection.rotation * (float)M_PI / 2); + Eigen::Matrix3d R; + R.setZero(); + R(0, 0) = R(1, 1) = c; + R(0, 1) = -s; + R(1, 0) = s; + R(2, 2) = 1; + Eigen::Matrix3d tmp; + tmp = thisTagDetection.homography * R; + thisTagDetection.homography = tmp; + + // Rotate points in detection according to decoded + // orientation. Thus the order of the points in the + // detection object can be used to determine the + // orientation of the target. + std::pair bottomLeft = thisTagDetection.interpolate(-1, -1); + int bestRot = -1; + float bestDist = FLT_MAX; + for (int i = 0; i < 4; i++) { + float const dist = + AprilTags::MathUtil::distance2D(bottomLeft, quad.quadPoints[i]); + if (dist < bestDist) { + bestDist = dist; + bestRot = i; + } + } + + for (int i = 0; i < 4; i++) + thisTagDetection.p[i] = quad.quadPoints[(i + bestRot) % 4]; + + if (thisTagDetection.good) { + thisTagDetection.cxy = quad.interpolate01(0.5f, 0.5f); + thisTagDetection.observedPerimeter = quad.observedPerimeter; + detections.push_back(thisTagDetection); + } + } + } + +#ifdef DEBUG_APRIL + { cv::imshow("debug_april", dbgImage); } +#endif + + //================================================================ + // Step nine: Some quads may be detected more than once, due to + // partial occlusion and our aggressive attempts to recover from + // broken lines. When two quads (with the same id) overlap, we will + // keep the one with the lowest error, and if the error is the same, + // the one with the greatest observed perimeter. + + std::vector goodDetections; + + // NOTE: allow multiple non-overlapping detections of the same target. + + for (vector::const_iterator it = detections.begin(); + it != detections.end(); it++) { + const TagDetection &thisTagDetection = *it; + + bool newFeature = true; + + for (unsigned int odidx = 0; odidx < goodDetections.size(); odidx++) { + TagDetection &otherTagDetection = goodDetections[odidx]; + + if (thisTagDetection.id != otherTagDetection.id || + !thisTagDetection.overlapsTooMuch(otherTagDetection)) + continue; + + // There's a conflict. We must pick one to keep. + newFeature = false; + + // This detection is worse than the previous one... just don't use it. + if (thisTagDetection.hammingDistance > otherTagDetection.hammingDistance) + continue; + + // Otherwise, keep the new one if it either has strictly *lower* error, or + // greater perimeter. + if (thisTagDetection.hammingDistance < + otherTagDetection.hammingDistance || + thisTagDetection.observedPerimeter > + otherTagDetection.observedPerimeter) + goodDetections[odidx] = thisTagDetection; + } + + if (newFeature) goodDetections.push_back(thisTagDetection); + } + + // cout << "AprilTags: edges=" << nEdges << " clusters=" << clusters.size() << + // " segments=" << segments.size() + // << " quads=" << quads.size() << " detections=" << detections.size() << + // " unique tags=" << goodDetections.size() << endl; + + return goodDetections; +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/TagFamily.cc b/thirdparty/apriltag/ethz_apriltag2/src/TagFamily.cc new file mode 100644 index 0000000..11065cb --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/TagFamily.cc @@ -0,0 +1,138 @@ +#include + +#include "apriltags/TagFamily.h" + +/** + +// example of instantiation of tag family: + +#include "apriltags/TagFamily.h" +#include "apriltags/Tag36h11.h" +TagFamily *tag36h11 = new TagFamily(tagCodes36h11); + +// available tag families: + +#include "apriltags/Tag16h5.h" +#include "apriltags/Tag16h5_other.h" +#include "apriltags/Tag25h7.h" +#include "apriltags/Tag25h9.h" +#include "apriltags/Tag36h11.h" +#include "apriltags/Tag36h11_other.h" +#include "apriltags/Tag36h9.h" + +*/ + + +namespace AprilTags { + +TagFamily::TagFamily(const TagCodes& tagCodes, const size_t blackBorder) + : blackBorder(blackBorder), bits(tagCodes.bits), dimension((int)std::sqrt((float)bits)), + minimumHammingDistance(tagCodes.minHammingDistance), + errorRecoveryBits(1), codes() { + if ( bits != dimension*dimension ) + cerr << "Error: TagFamily constructor called with bits=" << bits << "; must be a square number!" << endl; + codes = tagCodes.codes; +} + +void TagFamily::setErrorRecoveryBits(int b) { + errorRecoveryBits = b; +} + +void TagFamily::setErrorRecoveryFraction(float v) { + errorRecoveryBits = (int) (((int) (minimumHammingDistance-1)/2)*v); +} + +unsigned long long TagFamily::rotate90(unsigned long long w, int d) { + unsigned long long wr = 0; + const unsigned long long oneLongLong = 1; + + for (int r = d-1; r>=0; r--) { + for (int c = 0; c>= popCountTableShift; + } + return count; +} + +void TagFamily::decode(TagDetection& det, unsigned long long rCode) const { + int bestId = -1; + int bestHamming = INT_MAX; + int bestRotation = 0; + unsigned long long bestCode = 0; + + unsigned long long rCodes[4]; + rCodes[0] = rCode; + rCodes[1] = rotate90(rCodes[0], dimension); + rCodes[2] = rotate90(rCodes[1], dimension); + rCodes[3] = rotate90(rCodes[2], dimension); + + for (unsigned int id = 0; id < codes.size(); id++) { + for (unsigned int rot = 0; rot < 4; rot++) { + int thisHamming = hammingDistance(rCodes[rot], codes[id]); + if (thisHamming < bestHamming) { + bestHamming = thisHamming; + bestRotation = rot; + bestId = id; + bestCode = codes[id]; + } + } + } + det.id = bestId; + det.hammingDistance = bestHamming; + det.rotation = bestRotation; + det.good = (det.hammingDistance <= errorRecoveryBits); + det.obsCode = rCode; + det.code = bestCode; +} + +void TagFamily::printHammingDistances() const { + vector hammings(dimension*dimension+1); + for (unsigned i = 0; i < codes.size(); i++) { + unsigned long long r0 = codes[i]; + unsigned long long r1 = rotate90(r0, dimension); + unsigned long long r2 = rotate90(r1, dimension); + unsigned long long r3 = rotate90(r2, dimension); + for (unsigned int j = i+1; j < codes.size(); j++) { + int d = min(min(hammingDistance(r0, codes[j]), + hammingDistance(r1, codes[j])), + min(hammingDistance(r2, codes[j]), + hammingDistance(r3, codes[j]))); + hammings[d]++; + } + } + + for (unsigned int i = 0; i < hammings.size(); i++) + printf("hammings: %u = %d\n", i, hammings[i]); +} + +unsigned char TagFamily::popCountTable[TagFamily::popCountTableSize]; + +TagFamily::TableInitializer TagFamily::initializer; + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/UnionFindSimple.cc b/thirdparty/apriltag/ethz_apriltag2/src/UnionFindSimple.cc new file mode 100644 index 0000000..cc24a0f --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/UnionFindSimple.cc @@ -0,0 +1,54 @@ +#include "apriltags/UnionFindSimple.h" +#include + +namespace AprilTags { + +int UnionFindSimple::getRepresentative(int thisId) { + // terminal case: a node is its own parent + if (data[thisId].id == thisId) + return thisId; + + // otherwise, recurse... + int root = getRepresentative(data[thisId].id); + + // short circuit the path + data[thisId].id = root; + + return root; +} + +void UnionFindSimple::printDataVector() const { + for (unsigned int i = 0; i < data.size(); i++) + std::cout << "data[" << i << "]: " << " id:" << data[i].id << " size:" << data[i].size << std::endl; +} + +int UnionFindSimple::connectNodes(int aId, int bId) { + int aRoot = getRepresentative(aId); + int bRoot = getRepresentative(bId); + + if (aRoot == bRoot) + return aRoot; + + int asz = data[aRoot].size; + int bsz = data[bRoot].size; + + if (asz > bsz) { + data[bRoot].id = aRoot; + data[aRoot].size += bsz; + return aRoot; + } else { + data[aRoot].id = bRoot; + data[bRoot].size += asz; + return bRoot; + } +} + +void UnionFindSimple::init() { + for (unsigned int i = 0; i < data.size(); i++) { + // everyone is their own cluster of size 1 + data[i].id = i; + data[i].size = 1; + } +} + +} // namespace diff --git a/thirdparty/apriltag/ethz_apriltag2/src/example/CMakeLists.txt b/thirdparty/apriltag/ethz_apriltag2/src/example/CMakeLists.txt new file mode 100644 index 0000000..a19899a --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/example/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required (VERSION 2.6) + +link_libraries(apriltags) + +add_executable(apriltags_demo apriltags_demo.cpp Serial.cpp) +pods_install_executables(apriltags_demo) + +add_executable(imu imu.cpp Serial.cpp) +pods_install_executables(imu) diff --git a/thirdparty/apriltag/ethz_apriltag2/src/example/Serial.cpp b/thirdparty/apriltag/ethz_apriltag2/src/example/Serial.cpp new file mode 100644 index 0000000..a278972 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/example/Serial.cpp @@ -0,0 +1,113 @@ +/** + * @file Serial.cpp + * @brief Simple serial interface, for example to talk to Arduino. + * @author: Michael Kaess + */ + +#include +#include +#include +#include +#include +#include + +#include "Serial.h" + +using namespace std; + + +// open a serial port connection +void Serial::open(const string& port, int rate) { + m_serialPort = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if (m_serialPort==-1) { + cout << "Unable to open serial port" << endl; + exit(1); + } + fcntl(m_serialPort, F_SETFL,0); // O_NONBLOCK might be needed for write... + + struct termios port_settings; // structure to store the port settings in + tcgetattr(m_serialPort, &port_settings); // get current settings + + speed_t b; + switch(rate) { + case(9600): + b = B9600; + break; + case(19200): + b = B19200; + break; + case(38400): + b = B38400; + break; + case(115200): + b = B115200; + break; + default: + cout << "Error: Unknown baud rate requested in Serial.open()" << endl; + exit(1); + } + + cfsetispeed(&port_settings, b); // set baud rates + cfsetospeed(&port_settings, b); + + port_settings.c_cflag &= ~PARENB; // set no parity, stop bits, 8 data bits + port_settings.c_cflag &= ~CSTOPB; + port_settings.c_cflag &= ~CSIZE; + port_settings.c_cflag |= CS8; + + tcsetattr(m_serialPort, TCSANOW, &port_settings); // apply the settings to the port +} + +// read a single character +int Serial::read() const { + unsigned char result; + if (::read(m_serialPort, &result, 1) == 1) { + return (int)result; + } else { + return -1; + } +} + +// read until special character up to a maximum number of bytes +string Serial::readBytesUntil(unsigned char until, int max_length) { + string result(max_length, ' '); + int n = 0; + int c; + do { + c = read(); + if (c<0) { // wait for more characters + usleep(100); + } else { + result[n] = (unsigned char)c; + n++; + } + } while ((c != (int)until) && (n < max_length)); + result.resize(n); + return result; +} + +// send a string +void Serial::print(string str) const { + int res = ::write(m_serialPort, str.c_str(), str.length()); +} + +// send an integer +void Serial::print(int num) const { + stringstream stream; + stream << num; + string str = stream.str(); + print(str); +} + +// send a double +void Serial::print(double num) const { + stringstream stream; + stream << num; + string str = stream.str(); + print(str); +} + +// send a float +void Serial::print(float num) const { + print((double)num); +} diff --git a/thirdparty/apriltag/ethz_apriltag2/src/example/Serial.h b/thirdparty/apriltag/ethz_apriltag2/src/example/Serial.h new file mode 100644 index 0000000..635ee87 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/example/Serial.h @@ -0,0 +1,41 @@ +/** + * @file Serial.h + * @brief Simple serial interface, for example to talk to Arduino. + * @author: Michael Kaess + */ + +#pragma once + +#include + + +class Serial { + + int m_serialPort; // file description for the serial port + +public: + + Serial() : m_serialPort(-1) {} + + // open a serial port connection + void open(const std::string& port, int rate = 115200); + + // read a single character + int read() const; + + // read until special character up to a maximum number of bytes + std::string readBytesUntil(unsigned char until, int length = 300); + + // send a string + void print(std::string str) const; + + // send an integer + void print(int num) const; + + // send a double + void print(double num) const; + + // send a float + void print(float num) const; + +}; diff --git a/thirdparty/apriltag/ethz_apriltag2/src/example/apriltags_demo.cpp b/thirdparty/apriltag/ethz_apriltag2/src/example/apriltags_demo.cpp new file mode 100644 index 0000000..919388a --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/example/apriltags_demo.cpp @@ -0,0 +1,453 @@ +/** + * @file april_tags.cpp + * @brief Example application for April tags library + * @author: Michael Kaess + * + * Opens the first available camera (typically a built in camera in a + * laptop) and continuously detects April tags in the incoming + * images. Detections are both visualized in the live image and shown + * in the text console. Optionally allows selecting of a specific + * camera in case multiple ones are present and specifying image + * resolution as long as supported by the camera. Also includes the + * option to send tag detections via a serial port, for example when + * running on a Raspberry Pi that is connected to an Arduino. + */ + +using namespace std; + +#include +#include +#include +#include + +const string usage = "\n" + "Usage:\n" + " apriltags_demo [OPTION...] [deviceID]\n" + "\n" + "Options:\n" + " -h -? Show help options\n" + " -a Arduino (send tag ids over serial port)\n" + " -d disable graphics\n" + " -C Tag family (default 36h11)\n" + " -F Focal length in pixels\n" + " -W Image width (default 640, availability depends on camera)\n" + " -H Image height (default 480, availability depends on camera)\n" + " -S Tag size (square black frame) in meters\n" + " -E Manually set camera exposure (default auto; range 0-10000)\n" + " -G Manually set camera gain (default auto; range 0-255)\n" + " -B Manually set the camera brightness (default 128; range 0-255)\n" + "\n"; + +const string intro = "\n" + "April tags test code\n" + "(C) 2012-2013 Massachusetts Institute of Technology\n" + "Michael Kaess\n" + "\n"; + + +#ifndef __APPLE__ +#define EXPOSURE_CONTROL // only works in Linux +#endif + +#ifdef EXPOSURE_CONTROL +#include +#include +#include +#include +#endif + +// OpenCV library for easy access to USB camera and drawing of images +// on screen +#include "opencv2/opencv.hpp" + +// April tags detector and various families that can be selected by command line option +#include "apriltags/TagDetector.h" +#include "apriltags/Tag16h5.h" +#include "apriltags/Tag25h7.h" +#include "apriltags/Tag25h9.h" +#include "apriltags/Tag36h9.h" +#include "apriltags/Tag36h11.h" + + +// Needed for getopt / command line options processing +#include +extern int optind; +extern char *optarg; + +// For Arduino: locally defined serial port access class +#include "Serial.h" + + +const char* window_name = "apriltags_demo"; + + +// utility function to provide current system time (used below in +// determining frame rate at which images are being processed) +double tic() { + struct timeval t; + gettimeofday(&t, NULL); + return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.); +} + + +#include + +#ifndef PI +const double PI = 3.14159265358979323846; +#endif +const double TWOPI = 2.0*PI; + +/** + * Normalize angle to be within the interval [-pi,pi]. + */ +inline double standardRad(double t) { + if (t >= 0.) { + t = fmod(t+PI, TWOPI) - PI; + } else { + t = fmod(t-PI, -TWOPI) + PI; + } + return t; +} + +void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, double& roll) { + yaw = standardRad(atan2(wRo(1,0), wRo(0,0))); + double c = cos(yaw); + double s = sin(yaw); + pitch = standardRad(atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s)); + roll = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c)); + } + + +class Demo { + + AprilTags::TagDetector* m_tagDetector; + AprilTags::TagCodes m_tagCodes; + + bool m_draw; // draw image and April tag detections? + bool m_arduino; // send tag detections to serial port? + + int m_width; // image size in pixels + int m_height; + double m_tagSize; // April tag side length in meters of square black frame + double m_fx; // camera focal length in pixels + double m_fy; + double m_px; // camera principal point + double m_py; + + int m_deviceId; // camera id (in case of multiple cameras) + cv::VideoCapture m_cap; + + int m_exposure; + int m_gain; + int m_brightness; + + Serial m_serial; + +public: + + // default constructor + Demo() : + // default settings, most can be modified through command line options (see below) + m_tagDetector(NULL), + m_tagCodes(AprilTags::tagCodes36h11), + + m_draw(true), + m_arduino(false), + + m_width(640), + m_height(480), + m_tagSize(0.166), + m_fx(600), + m_fy(600), + m_px(m_width/2), + m_py(m_height/2), + + m_exposure(-1), + m_gain(-1), + m_brightness(-1), + + m_deviceId(0) + {} + + // changing the tag family + void setTagCodes(string s) { + if (s=="16h5") { + m_tagCodes = AprilTags::tagCodes16h5; + } else if (s=="25h7") { + m_tagCodes = AprilTags::tagCodes25h7; + } else if (s=="25h9") { + m_tagCodes = AprilTags::tagCodes25h9; + } else if (s=="36h9") { + m_tagCodes = AprilTags::tagCodes36h9; + } else if (s=="36h11") { + m_tagCodes = AprilTags::tagCodes36h11; + } else { + cout << "Invalid tag family specified" << endl; + exit(1); + } + } + + // parse command line options to change default behavior + void parseOptions(int argc, char* argv[]) { + int c; + while ((c = getopt(argc, argv, ":h?adC:F:H:S:W:E:G:B:")) != -1) { + // Each option character has to be in the string in getopt(); + // the first colon changes the error character from '?' to ':'; + // a colon after an option means that there is an extra + // parameter to this option; 'W' is a reserved character + switch (c) { + case 'h': + case '?': + cout << intro; + cout << usage; + exit(0); + break; + case 'a': + m_arduino = true; + break; + case 'd': + m_draw = false; + break; + case 'C': + setTagCodes(optarg); + break; + case 'F': + m_fx = atof(optarg); + m_fy = m_fx; + break; + case 'H': + m_height = atoi(optarg); + m_py = m_height/2; + break; + case 'S': + m_tagSize = atof(optarg); + break; + case 'W': + m_width = atoi(optarg); + m_px = m_width/2; + break; + case 'E': +#ifndef EXPOSURE_CONTROL + cout << "Error: Exposure option (-E) not available" << endl; + exit(1); +#endif + m_exposure = atoi(optarg); + break; + case 'G': +#ifndef EXPOSURE_CONTROL + cout << "Error: Gain option (-G) not available" << endl; + exit(1); +#endif + m_gain = atoi(optarg); + break; + case 'B': +#ifndef EXPOSURE_CONTROL + cout << "Error: Brightness option (-B) not available" << endl; + exit(1); +#endif + m_brightness = atoi(optarg); + break; + case ':': // unknown option, from getopt + cout << intro; + cout << usage; + exit(1); + break; + } + } + + if (argc == optind + 1) { + m_deviceId = atoi(argv[optind]); + } + } + + void setup() { + m_tagDetector = new AprilTags::TagDetector(m_tagCodes); + +#ifdef EXPOSURE_CONTROL + // manually setting camera exposure settings; OpenCV/v4l1 doesn't + // support exposure control; so here we manually use v4l2 before + // opening the device via OpenCV; confirmed to work with Logitech + // C270; try exposure=20, gain=100, brightness=150 + + string video_str = "/dev/video0"; + video_str[10] = '0' + m_deviceId; + int device = v4l2_open(video_str.c_str(), O_RDWR | O_NONBLOCK); + + if (m_exposure >= 0) { + // not sure why, but v4l2_set_control() does not work for + // V4L2_CID_EXPOSURE_AUTO... + struct v4l2_control c; + c.id = V4L2_CID_EXPOSURE_AUTO; + c.value = 1; // 1=manual, 3=auto; V4L2_EXPOSURE_AUTO fails... + if (v4l2_ioctl(device, VIDIOC_S_CTRL, &c) != 0) { + cout << "Failed to set... " << strerror(errno) << endl; + } + cout << "exposure: " << m_exposure << endl; + v4l2_set_control(device, V4L2_CID_EXPOSURE_ABSOLUTE, m_exposure*6); + } + if (m_gain >= 0) { + cout << "gain: " << m_gain << endl; + v4l2_set_control(device, V4L2_CID_GAIN, m_gain*256); + } + if (m_brightness >= 0) { + cout << "brightness: " << m_brightness << endl; + v4l2_set_control(device, V4L2_CID_BRIGHTNESS, m_brightness*256); + } + v4l2_close(device); +#endif + + // find and open a USB camera (built in laptop camera, web cam etc) + m_cap = cv::VideoCapture(m_deviceId); + if(!m_cap.isOpened()) { + cerr << "ERROR: Can't find video device " << m_deviceId << "\n"; + exit(1); + } + m_cap.set(CV_CAP_PROP_FRAME_WIDTH, m_width); + m_cap.set(CV_CAP_PROP_FRAME_HEIGHT, m_height); + cout << "Camera successfully opened (ignore error messages above...)" << endl; + cout << "Actual resolution: " + << m_cap.get(CV_CAP_PROP_FRAME_WIDTH) << "x" + << m_cap.get(CV_CAP_PROP_FRAME_HEIGHT) << endl; + + // prepare window for drawing the camera images + if (m_draw) { + cv::namedWindow(window_name, 1); + } + + // optional: prepare serial port for communication with Arduino + if (m_arduino) { + m_serial.open("/dev/ttyACM0"); + } + } + + void print_detection(AprilTags::TagDetection& detection) const { + cout << " Id: " << detection.id + << " (Hamming: " << detection.hammingDistance << ")"; + + // recovering the relative pose of a tag: + + // NOTE: for this to be accurate, it is necessary to use the + // actual camera parameters here as well as the actual tag size + // (m_fx, m_fy, m_px, m_py, m_tagSize) + + Eigen::Vector3d translation; + Eigen::Matrix3d rotation; + detection.getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py, + translation, rotation); + + Eigen::Matrix3d F; + F << + 1, 0, 0, + 0, -1, 0, + 0, 0, 1; + Eigen::Matrix3d fixed_rot = F*rotation; + double yaw, pitch, roll; + wRo_to_euler(fixed_rot, yaw, pitch, roll); + + cout << " distance=" << translation.norm() + << "m, x=" << translation(0) + << ", y=" << translation(1) + << ", z=" << translation(2) + << ", yaw=" << yaw + << ", pitch=" << pitch + << ", roll=" << roll + << endl; + + // Also note that for SLAM/multi-view application it is better to + // use reprojection error of corner points, because the noise in + // this relative pose is very non-Gaussian; see iSAM source code + // for suitable factors. + } + + // The processing loop where images are retrieved, tags detected, + // and information about detections generated + void loop() { + + cv::Mat image; + cv::Mat image_gray; + + int frame = 0; + double last_t = tic(); + while (true) { + + // capture frame + m_cap >> image; + + // alternative way is to grab, then retrieve; allows for + // multiple grab when processing below frame rate - v4l keeps a + // number of frames buffered, which can lead to significant lag + // m_cap.grab(); + // m_cap.retrieve(image); + + // detect April tags (requires a gray scale image) + cv::cvtColor(image, image_gray, CV_BGR2GRAY); + vector detections = m_tagDetector->extractTags(image_gray); + + // print out each detection + cout << detections.size() << " tags detected:" << endl; + for (int i=0; i 0) { + // only the first detected tag is sent out for now + Eigen::Vector3d translation; + Eigen::Matrix3d rotation; + detections[0].getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py, + translation, rotation); + m_serial.print(detections[0].id); + m_serial.print(","); + m_serial.print(translation(0)); + m_serial.print(","); + m_serial.print(translation(1)); + m_serial.print(","); + m_serial.print(translation(2)); + m_serial.print("\n"); + } else { + // no tag detected: tag ID = -1 + m_serial.print("-1,0.0,0.0,0.0\n"); + } + } + + // print out the frame rate at which image frames are being processed + frame++; + if (frame % 10 == 0) { + double t = tic(); + cout << " " << 10./(t-last_t) << " fps" << endl; + last_t = t; + } + + // exit if any key is pressed + if (cv::waitKey(1) >= 0) break; + } + } + +}; // Demo + + +// here is were everything begins +int main(int argc, char* argv[]) { + Demo demo; + + // process command line options + demo.parseOptions(argc, argv); + + // setup image source, window for drawing, serial port... + demo.setup(); + + // the actual processing loop where tags are detected and visualized + demo.loop(); + + return 0; +} diff --git a/thirdparty/apriltag/ethz_apriltag2/src/example/arduino_tags/arduino_tags.ino b/thirdparty/apriltag/ethz_apriltag2/src/example/arduino_tags/arduino_tags.ino new file mode 100644 index 0000000..208d888 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/example/arduino_tags/arduino_tags.ino @@ -0,0 +1,40 @@ +/* + ArduinoTag + Turns on an LED whenever Raspberry Pi sees an Apriltag. + + Michael Kaess 04/13 + */ + +// pin number for LED +int led = 9; + +// information about detected tag +int tagId; +float x, y, z; + +// runs once when the program starts +void setup() { + // open serial port + Serial.begin(115200); + // initialize pin as output + pinMode(led, OUTPUT); +} + +// runs over and over again +void loop() { + // check if new data is available + if (Serial.available() > 0) { + tagId = Serial.parseInt(); + x = Serial.parseFloat(); + y = Serial.parseFloat(); + z = Serial.parseFloat(); + Serial.read(); // ignore newline character + if (tagId >= 0) { + digitalWrite(led, HIGH); + } else { + digitalWrite(led, LOW); + } + } + // wait for 20ms before checking again for new data + delay(20); +} diff --git a/thirdparty/apriltag/ethz_apriltag2/src/example/imu.cpp b/thirdparty/apriltag/ethz_apriltag2/src/example/imu.cpp new file mode 100644 index 0000000..d4af157 --- /dev/null +++ b/thirdparty/apriltag/ethz_apriltag2/src/example/imu.cpp @@ -0,0 +1,64 @@ +// Example of reading from ArduIMU (v3) via serial port +// Michael Kaess, May 2013 + +#include +#include +#include +#include + +#include "Serial.h" + +using namespace std; + + +void parse(string s) { + // note that ex (the rotation matrix) and mg? (the magnetormeter + // readings) below are optional, depending on how the ArduIMU was + // configured + + float version; // ArduIMU code version + float gyroX, gyroY, gyroZ; // raw gyroscope values + float accelX, accelY, accelZ; // raw accelerometer values + // int ex[9]; // rotation matrix (scaled by 10^7) + float roll, pitch, yaw; // Euler angles + int imuh; // IMU health + // float mgx, mgy, mgz, mgh; // magnetometer readings + int tow; // GPS time + + // cout << s; + + // try to parse the line + int num = sscanf(s.c_str(), + //"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,EX0:%i,EX1:%i,EX2:%i,EX3:%i,EX4:%i,EX5:%i,EX6:%i,EX7:%i,EX8:%i,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,TOW:%i***", + "!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,TOW:%i", + //"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,MGX:%g,MGY:%g,MGZ:%g,MGH:%g,TOW:%i", + &version, &gyroX, &gyroY, &gyroZ, &accelX, &accelY, &accelZ, + // &ex[0], &ex[1], &ex[2], &ex[3], &ex[4], &ex[5], &ex[6], &ex[7], &ex[8], + &roll, &pitch, &yaw, + &imuh, + // &mgx, &mgy, &mgz, &mgh, + &tow); + + // did we read the correct number of entries, or did the line contain other information? + if (num==12 || num==16 || num==21) { + cout << "Euler angles: " << yaw << "," << pitch << "," << roll << endl; + } else { + // cout << "Could not parse string " << num << endl; + } + +} + + +int main() { + + Serial serial; + serial.open("/dev/ttyUSB0", 38400); // might need to change to your USB port + + // read and parse one line at a time + while (true) { + string s = serial.readBytesUntil('\n'); + parse(s); + } + + return 0; +} diff --git a/thirdparty/apriltag/include/basalt/utils/apriltag.h b/thirdparty/apriltag/include/basalt/utils/apriltag.h new file mode 100644 index 0000000..0a5c60b --- /dev/null +++ b/thirdparty/apriltag/include/basalt/utils/apriltag.h @@ -0,0 +1,30 @@ + +#include +#include +#include + + +namespace basalt { + +struct ApriltagDetectorData; + +class ApriltagDetector { + public: + ApriltagDetector(); + + ~ApriltagDetector(); + + void detectTags(basalt::ManagedImage& img_raw, + Eigen::vector& corners, + std::vector& ids, + std::vector& radii, + Eigen::vector& corners_rejected, + std::vector& ids_rejected, + std::vector& radii_rejected); + + private: + ApriltagDetectorData* data; + }; + +} + diff --git a/thirdparty/apriltag/src/apriltag.cpp b/thirdparty/apriltag/src/apriltag.cpp new file mode 100644 index 0000000..391766d --- /dev/null +++ b/thirdparty/apriltag/src/apriltag.cpp @@ -0,0 +1,249 @@ + +#include + +#include + +#include + +namespace basalt { + +struct ApriltagDetectorData { + ApriltagDetectorData() + : doSubpixRefinement(true), + maxSubpixDisplacement(0), + minTagsForValidObs(4), + minBorderDistance(4.0), + blackTagBorder(2), + _tagCodes(AprilTags::tagCodes36h11) { + _tagDetector = + std::make_shared(_tagCodes, blackTagBorder); + } + + bool doSubpixRefinement; + double + maxSubpixDisplacement; //!< maximum displacement for subpixel refinement. + //!< If 0, only base it on tag size. + unsigned int minTagsForValidObs; + double minBorderDistance; + unsigned int blackTagBorder; + + AprilTags::TagCodes _tagCodes; + std::shared_ptr _tagDetector; + + inline int size() { return 36 * 4; } +}; + +ApriltagDetector::ApriltagDetector() { data = new ApriltagDetectorData; } + +ApriltagDetector::~ApriltagDetector() { delete data; } + +void ApriltagDetector::detectTags( + basalt::ManagedImage& img_raw, + Eigen::vector& corners, std::vector& ids, + std::vector& radii, + Eigen::vector& corners_rejected, + std::vector& ids_rejected, std::vector& radii_rejected) { + corners.clear(); + ids.clear(); + radii.clear(); + corners_rejected.clear(); + ids_rejected.clear(); + radii_rejected.clear(); + + cv::Mat image(img_raw.h, img_raw.w, CV_8U); + + uint8_t* dst = image.ptr(); + const uint16_t* src = img_raw.ptr; + + for (size_t i = 0; i < img_raw.size(); i++) { + dst[i] = (src[i] >> 8); + } + + // detect the tags + std::vector detections = + data->_tagDetector->extractTags(image); + + /* handle the case in which a tag is identified but not all tag + * corners are in the image (all data bits in image but border + * outside). tagCorners should still be okay as apriltag-lib + * extrapolates them, only the subpix refinement will fail + */ + + // min. distance [px] of tag corners from image border (tag is not used if + // violated) + std::vector::iterator iter = detections.begin(); + for (iter = detections.begin(); iter != detections.end();) { + // check all four corners for violation + bool remove = false; + + for (int j = 0; j < 4; j++) { + remove |= iter->p[j].first < data->minBorderDistance; + remove |= iter->p[j].first > + (float)(image.cols) - data->minBorderDistance; // width + remove |= iter->p[j].second < data->minBorderDistance; + remove |= iter->p[j].second > + (float)(image.rows) - data->minBorderDistance; // height + } + + // also remove tags that are flagged as bad + if (iter->good != 1) remove |= true; + + // also remove if the tag ID is out-of-range for this grid (faulty + // detection) + if (iter->id >= (int)data->size() / 4) remove |= true; + + // delete flagged tags + if (remove) { + // delete the tag and advance in list + iter = detections.erase(iter); + } else { + // advance in list + ++iter; + } + } + + // did we find enough tags? + if (detections.size() < data->minTagsForValidObs) return; + + // sort detections by tagId + std::sort(detections.begin(), detections.end(), + AprilTags::TagDetection::sortByIdCompare); + + // check for duplicate tagIds (--> if found: wild Apriltags in image not + // belonging to calibration target) + // (only if we have more than 1 tag...) + if (detections.size() > 1) { + for (unsigned i = 0; i < detections.size() - 1; i++) + if (detections[i].id == detections[i + 1].id) { + std::cerr << "Wild Apriltag detected. Hide them!" << std::endl; + return; + } + } + + // compute search radius for sub-pixel refinement depending on size of tag in + // image + std::vector radiiRaw; + for (unsigned i = 0; i < detections.size(); i++) { + const double minimalRadius = 2.0; + const double percentOfSideLength = 7.5; + const double avgSideLength = + static_cast(detections[i].observedPerimeter) / 4.0; + // use certain percentage of average side length as radius + // subtract 1.0 since this radius is for displacement threshold; Search + // region is slightly larger + radiiRaw.emplace_back(std::max( + minimalRadius, (percentOfSideLength / 100.0 * avgSideLength) - 1.0)); + } + + // convert corners to cv::Mat (4 consecutive corners form one tag) + /// point ordering here + /// 11-----10 15-----14 + /// | TAG 2 | | TAG 3 | + /// 8-------9 12-----13 + /// 3-------2 7-------6 + /// y | TAG 0 | | TAG 1 | + /// ^ 0-------1 4-------5 + /// |-->x + cv::Mat tagCorners(4 * detections.size(), 2, CV_32F); + + for (unsigned i = 0; i < detections.size(); i++) { + for (unsigned j = 0; j < 4; j++) { + tagCorners.at(4 * i + j, 0) = detections[i].p[j].first; + tagCorners.at(4 * i + j, 1) = detections[i].p[j].second; + } + } + + // store a copy of the corner list before subpix refinement + cv::Mat tagCornersRaw = tagCorners.clone(); + + // optional subpixel refinement on all tag corners (four corners each tag) + if (data->doSubpixRefinement) { + for (size_t i = 0; i < detections.size(); i++) { + cv::Mat currentCorners(4, 2, CV_32F); + for (unsigned j = 0; j < 4; j++) { + currentCorners.at(j, 0) = tagCorners.at(4 * i + j, 0); + currentCorners.at(j, 1) = tagCorners.at(4 * i + j, 1); + } + + const int radius = static_cast(std::ceil(radiiRaw[i] + 1.0)); + cv::cornerSubPix( + image, currentCorners, cv::Size(radius, radius), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, + 100, 0.01)); + + for (unsigned j = 0; j < 4; j++) { + tagCorners.at(4 * i + j, 0) = currentCorners.at(j, 0); + tagCorners.at(4 * i + j, 1) = currentCorners.at(j, 1); + } + } + } + + // insert the observed points into the correct location of the grid point + // array + /// point ordering + /// 12-----13 14-----15 + /// | TAG 2 | | TAG 3 | + /// 8-------9 10-----11 + /// 4-------5 6-------7 + /// y | TAG 0 | | TAG 1 | + /// ^ 0-------1 2-------3 + /// |-->x + + for (unsigned int i = 0; i < detections.size(); i++) { + // get the tag id + int tagId = detections[i].id; + + // check maximum displacement from subpixel refinement + const double radius = radiiRaw[i]; + const double tagMaxDispl2 = radius * radius; + const double globalMaxDispl2 = + data->maxSubpixDisplacement * data->maxSubpixDisplacement; + const double subpixRefinementThreshold2 = + globalMaxDispl2 > 0 ? std::min(globalMaxDispl2, tagMaxDispl2) + : tagMaxDispl2; + + // add four points per tag + for (int j = 0; j < 4; j++) { + int pointId = (tagId << 2) + j; + + // refined corners + double corner_x = tagCorners.row(4 * i + j).at(0); + double corner_y = tagCorners.row(4 * i + j).at(1); + + // raw corners + double cornerRaw_x = tagCornersRaw.row(4 * i + j).at(0); + double cornerRaw_y = tagCornersRaw.row(4 * i + j).at(1); + + // only add point if the displacement in the subpixel refinement is below + // a given threshold + double subpix_displacement_squarred = + (corner_x - cornerRaw_x) * (corner_x - cornerRaw_x) + + (corner_y - cornerRaw_y) * (corner_y - cornerRaw_y); + + // add all points, but only set active if the point has not moved to far + // in the subpix refinement + + // TODO: We still get a few false positives here, e.g. when the whole + // search region lies on an edge but the actual corner is not included. + // Maybe what we would need to do is actually checking a "corner + // score" vs "edge score" after refinement and discard all corners + // that are not more "cornery" than "edgy". Another possible issue + // might be corners, where (due to fisheye distortion), neighboring + // corners are in the search radius. For those we should check if in + // the radius there is really a clear single maximum in the corner + // score and otherwise discard the corner. + + if (subpix_displacement_squarred <= subpixRefinementThreshold2) { + corners.emplace_back(corner_x, corner_y); + ids.emplace_back(pointId); + radii.emplace_back(radius); + } else { + corners_rejected.emplace_back(corner_x, corner_y); + ids_rejected.emplace_back(pointId); + radii_rejected.emplace_back(radius); + } + } + } +} + +} // namespace basalt diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers new file mode 160000 index 0000000..bb7300d --- /dev/null +++ b/thirdparty/basalt-headers @@ -0,0 +1 @@ +Subproject commit bb7300d51f7f5af8988f162e80e9481f27c5b8ed diff --git a/thirdparty/opengv b/thirdparty/opengv new file mode 160000 index 0000000..306a54e --- /dev/null +++ b/thirdparty/opengv @@ -0,0 +1 @@ +Subproject commit 306a54e6c6b94e2048f820cdf77ef5281d4b48ad diff --git a/thirdparty/ros/CMakeLists.txt b/thirdparty/ros/CMakeLists.txt new file mode 100644 index 0000000..7709a5f --- /dev/null +++ b/thirdparty/ros/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.2) + + +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") + +#find_package(catkin REQUIRED COMPONENTS cpp_common roscpp_serialization roscpp_traits rostime roslz4) +find_package(Boost REQUIRED COMPONENTS date_time filesystem program_options regex) +find_package(BZip2 REQUIRED) +find_library(lz4_LIBRARIES NAMES lz4) + +file(GLOB CONSOLE_BRIDGE_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/console_bridge/src/*.cpp") +file(GLOB CPP_COMMON_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/roscpp_core/cpp_common/src/*.cpp") +file(GLOB ROSCPP_SERIALIZATION_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/roscpp_core/roscpp_serialization/src/*.cpp") +file(GLOB ROSTIME_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/roscpp_core/rostime/src/*.cpp") + +file(GLOB ROSBAG_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/ros_comm/tools/rosbag_storage/src/*.cpp") +file(GLOB ROSLZ4_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/ros_comm/utilities/roslz4/src/[a-z]*.c") + +add_library(rosbag STATIC ${ROSBAG_SRCS} ${ROSTIME_SRCS} ${CPP_COMMON_SRCS} ${ROSCPP_SERIALIZATION_SRCS} ${ROSLZ4_SRCS} ${CONSOLE_BRIDGE_SRCS}) + +target_include_directories(rosbag PUBLIC + include + console_bridge/include + roscpp_core/cpp_common/include + roscpp_core/rostime/include + roscpp_core/roscpp_serialization/include + roscpp_core/roscpp_traits/include + ros_comm/utilities/roslz4/include + ros_comm/tools/rosbag_storage/include) + +target_link_libraries(rosbag PUBLIC ${Boost_LIBRARIES} ${BZIP2_LIBRARIES} ${lz4_LIBRARIES}) + diff --git a/thirdparty/ros/console_bridge b/thirdparty/ros/console_bridge new file mode 160000 index 0000000..ad25f73 --- /dev/null +++ b/thirdparty/ros/console_bridge @@ -0,0 +1 @@ +Subproject commit ad25f7307da76be2857545e7e5c2a20727eee542 diff --git a/thirdparty/ros/include/console_bridge_export.h b/thirdparty/ros/include/console_bridge_export.h new file mode 100644 index 0000000..4b3754d --- /dev/null +++ b/thirdparty/ros/include/console_bridge_export.h @@ -0,0 +1,41 @@ + +#ifndef CONSOLE_BRIDGE_DLLAPI_H +#define CONSOLE_BRIDGE_DLLAPI_H + +#ifdef CONSOLE_BRIDGE_STATIC_DEFINE +# define CONSOLE_BRIDGE_DLLAPI +# define CONSOLE_BRIDGE_NO_EXPORT +#else +# ifndef CONSOLE_BRIDGE_DLLAPI +# ifdef console_bridge_EXPORTS + /* We are building this library */ +# define CONSOLE_BRIDGE_DLLAPI __attribute__((visibility("default"))) +# else + /* We are using this library */ +# define CONSOLE_BRIDGE_DLLAPI __attribute__((visibility("default"))) +# endif +# endif + +# ifndef CONSOLE_BRIDGE_NO_EXPORT +# define CONSOLE_BRIDGE_NO_EXPORT __attribute__((visibility("hidden"))) +# endif +#endif + +#ifndef CONSOLE_BRIDGE_DEPRECATED +# define CONSOLE_BRIDGE_DEPRECATED __attribute__ ((__deprecated__)) +#endif + +#ifndef CONSOLE_BRIDGE_DEPRECATED_EXPORT +# define CONSOLE_BRIDGE_DEPRECATED_EXPORT CONSOLE_BRIDGE_DLLAPI CONSOLE_BRIDGE_DEPRECATED +#endif + +#ifndef CONSOLE_BRIDGE_DEPRECATED_NO_EXPORT +# define CONSOLE_BRIDGE_DEPRECATED_NO_EXPORT CONSOLE_BRIDGE_NO_EXPORT CONSOLE_BRIDGE_DEPRECATED +#endif + +#define DEFINE_NO_DEPRECATED 0 +#if DEFINE_NO_DEPRECATED +# define CONSOLE_BRIDGE_NO_DEPRECATED +#endif + +#endif diff --git a/thirdparty/ros/include/geometry_msgs/Accel.h b/thirdparty/ros/include/geometry_msgs/Accel.h new file mode 100644 index 0000000..e721cc3 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Accel.h @@ -0,0 +1,214 @@ +// Generated by gencpp from file geometry_msgs/Accel.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_ACCEL_H +#define GEOMETRY_MSGS_MESSAGE_ACCEL_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Accel_ +{ + typedef Accel_ Type; + + Accel_() + : linear() + , angular() { + } + Accel_(const ContainerAllocator& _alloc) + : linear(_alloc) + , angular(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Vector3_ _linear_type; + _linear_type linear; + + typedef ::geometry_msgs::Vector3_ _angular_type; + _angular_type angular; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Accel_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Accel_ const> ConstPtr; + +}; // struct Accel_ + +typedef ::geometry_msgs::Accel_ > Accel; + +typedef boost::shared_ptr< ::geometry_msgs::Accel > AccelPtr; +typedef boost::shared_ptr< ::geometry_msgs::Accel const> AccelConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Accel_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Accel_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Accel_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Accel_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Accel_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Accel_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Accel_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Accel_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Accel_ > +{ + static const char* value() + { + return "9f195f881246fdfa2798d1d3eebca84a"; + } + + static const char* value(const ::geometry_msgs::Accel_&) { return value(); } + static const uint64_t static_value1 = 0x9f195f881246fdfaULL; + static const uint64_t static_value2 = 0x2798d1d3eebca84aULL; +}; + +template +struct DataType< ::geometry_msgs::Accel_ > +{ + static const char* value() + { + return "geometry_msgs/Accel"; + } + + static const char* value(const ::geometry_msgs::Accel_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Accel_ > +{ + static const char* value() + { + return "# This expresses acceleration in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Accel_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Accel_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.linear); + stream.next(m.angular); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Accel_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Accel_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Accel_& v) + { + s << indent << "linear: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.linear); + s << indent << "angular: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.angular); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_ACCEL_H diff --git a/thirdparty/ros/include/geometry_msgs/AccelStamped.h b/thirdparty/ros/include/geometry_msgs/AccelStamped.h new file mode 100644 index 0000000..dd451fd --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/AccelStamped.h @@ -0,0 +1,238 @@ +// Generated by gencpp from file geometry_msgs/AccelStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_ACCELSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_ACCELSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct AccelStamped_ +{ + typedef AccelStamped_ Type; + + AccelStamped_() + : header() + , accel() { + } + AccelStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , accel(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Accel_ _accel_type; + _accel_type accel; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::AccelStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::AccelStamped_ const> ConstPtr; + +}; // struct AccelStamped_ + +typedef ::geometry_msgs::AccelStamped_ > AccelStamped; + +typedef boost::shared_ptr< ::geometry_msgs::AccelStamped > AccelStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::AccelStamped const> AccelStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::AccelStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::AccelStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::AccelStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::AccelStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::AccelStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::AccelStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::AccelStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::AccelStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::AccelStamped_ > +{ + static const char* value() + { + return "d8a98a5d81351b6eb0578c78557e7659"; + } + + static const char* value(const ::geometry_msgs::AccelStamped_&) { return value(); } + static const uint64_t static_value1 = 0xd8a98a5d81351b6eULL; + static const uint64_t static_value2 = 0xb0578c78557e7659ULL; +}; + +template +struct DataType< ::geometry_msgs::AccelStamped_ > +{ + static const char* value() + { + return "geometry_msgs/AccelStamped"; + } + + static const char* value(const ::geometry_msgs::AccelStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::AccelStamped_ > +{ + static const char* value() + { + return "# An accel with reference coordinate frame and timestamp\n\ +Header header\n\ +Accel accel\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Accel\n\ +# This expresses acceleration in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::AccelStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::AccelStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.accel); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct AccelStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::AccelStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::AccelStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "accel: "; + s << std::endl; + Printer< ::geometry_msgs::Accel_ >::stream(s, indent + " ", v.accel); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_ACCELSTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/AccelWithCovariance.h b/thirdparty/ros/include/geometry_msgs/AccelWithCovariance.h new file mode 100644 index 0000000..611555f --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/AccelWithCovariance.h @@ -0,0 +1,230 @@ +// Generated by gencpp from file geometry_msgs/AccelWithCovariance.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCE_H +#define GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct AccelWithCovariance_ +{ + typedef AccelWithCovariance_ Type; + + AccelWithCovariance_() + : accel() + , covariance() { + covariance.assign(0.0); + } + AccelWithCovariance_(const ContainerAllocator& _alloc) + : accel(_alloc) + , covariance() { + (void)_alloc; + covariance.assign(0.0); + } + + + + typedef ::geometry_msgs::Accel_ _accel_type; + _accel_type accel; + + typedef boost::array _covariance_type; + _covariance_type covariance; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovariance_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovariance_ const> ConstPtr; + +}; // struct AccelWithCovariance_ + +typedef ::geometry_msgs::AccelWithCovariance_ > AccelWithCovariance; + +typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovariance > AccelWithCovariancePtr; +typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovariance const> AccelWithCovarianceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::AccelWithCovariance_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::AccelWithCovariance_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::AccelWithCovariance_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::AccelWithCovariance_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::AccelWithCovariance_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::AccelWithCovariance_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::AccelWithCovariance_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::AccelWithCovariance_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::AccelWithCovariance_ > +{ + static const char* value() + { + return "ad5a718d699c6be72a02b8d6a139f334"; + } + + static const char* value(const ::geometry_msgs::AccelWithCovariance_&) { return value(); } + static const uint64_t static_value1 = 0xad5a718d699c6be7ULL; + static const uint64_t static_value2 = 0x2a02b8d6a139f334ULL; +}; + +template +struct DataType< ::geometry_msgs::AccelWithCovariance_ > +{ + static const char* value() + { + return "geometry_msgs/AccelWithCovariance"; + } + + static const char* value(const ::geometry_msgs::AccelWithCovariance_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::AccelWithCovariance_ > +{ + static const char* value() + { + return "# This expresses acceleration in free space with uncertainty.\n\ +\n\ +Accel accel\n\ +\n\ +# Row-major representation of the 6x6 covariance matrix\n\ +# The orientation parameters use a fixed-axis representation.\n\ +# In order, the parameters are:\n\ +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n\ +float64[36] covariance\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Accel\n\ +# This expresses acceleration in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::AccelWithCovariance_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::AccelWithCovariance_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.accel); + stream.next(m.covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct AccelWithCovariance_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::AccelWithCovariance_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::AccelWithCovariance_& v) + { + s << indent << "accel: "; + s << std::endl; + Printer< ::geometry_msgs::Accel_ >::stream(s, indent + " ", v.accel); + s << indent << "covariance[]" << std::endl; + for (size_t i = 0; i < v.covariance.size(); ++i) + { + s << indent << " covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCE_H diff --git a/thirdparty/ros/include/geometry_msgs/AccelWithCovarianceStamped.h b/thirdparty/ros/include/geometry_msgs/AccelWithCovarianceStamped.h new file mode 100644 index 0000000..4a559a3 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/AccelWithCovarianceStamped.h @@ -0,0 +1,250 @@ +// Generated by gencpp from file geometry_msgs/AccelWithCovarianceStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCESTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCESTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct AccelWithCovarianceStamped_ +{ + typedef AccelWithCovarianceStamped_ Type; + + AccelWithCovarianceStamped_() + : header() + , accel() { + } + AccelWithCovarianceStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , accel(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::AccelWithCovariance_ _accel_type; + _accel_type accel; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovarianceStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovarianceStamped_ const> ConstPtr; + +}; // struct AccelWithCovarianceStamped_ + +typedef ::geometry_msgs::AccelWithCovarianceStamped_ > AccelWithCovarianceStamped; + +typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovarianceStamped > AccelWithCovarianceStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::AccelWithCovarianceStamped const> AccelWithCovarianceStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::AccelWithCovarianceStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::AccelWithCovarianceStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::AccelWithCovarianceStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::AccelWithCovarianceStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::AccelWithCovarianceStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::AccelWithCovarianceStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::AccelWithCovarianceStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::AccelWithCovarianceStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::AccelWithCovarianceStamped_ > +{ + static const char* value() + { + return "96adb295225031ec8d57fb4251b0a886"; + } + + static const char* value(const ::geometry_msgs::AccelWithCovarianceStamped_&) { return value(); } + static const uint64_t static_value1 = 0x96adb295225031ecULL; + static const uint64_t static_value2 = 0x8d57fb4251b0a886ULL; +}; + +template +struct DataType< ::geometry_msgs::AccelWithCovarianceStamped_ > +{ + static const char* value() + { + return "geometry_msgs/AccelWithCovarianceStamped"; + } + + static const char* value(const ::geometry_msgs::AccelWithCovarianceStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::AccelWithCovarianceStamped_ > +{ + static const char* value() + { + return "# This represents an estimated accel with reference coordinate frame and timestamp.\n\ +Header header\n\ +AccelWithCovariance accel\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/AccelWithCovariance\n\ +# This expresses acceleration in free space with uncertainty.\n\ +\n\ +Accel accel\n\ +\n\ +# Row-major representation of the 6x6 covariance matrix\n\ +# The orientation parameters use a fixed-axis representation.\n\ +# In order, the parameters are:\n\ +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n\ +float64[36] covariance\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Accel\n\ +# This expresses acceleration in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::AccelWithCovarianceStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::AccelWithCovarianceStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.accel); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct AccelWithCovarianceStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::AccelWithCovarianceStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::AccelWithCovarianceStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "accel: "; + s << std::endl; + Printer< ::geometry_msgs::AccelWithCovariance_ >::stream(s, indent + " ", v.accel); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCESTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Inertia.h b/thirdparty/ros/include/geometry_msgs/Inertia.h new file mode 100644 index 0000000..7b66083 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Inertia.h @@ -0,0 +1,273 @@ +// Generated by gencpp from file geometry_msgs/Inertia.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_INERTIA_H +#define GEOMETRY_MSGS_MESSAGE_INERTIA_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct Inertia_ +{ + typedef Inertia_ Type; + + Inertia_() + : m(0.0) + , com() + , ixx(0.0) + , ixy(0.0) + , ixz(0.0) + , iyy(0.0) + , iyz(0.0) + , izz(0.0) { + } + Inertia_(const ContainerAllocator& _alloc) + : m(0.0) + , com(_alloc) + , ixx(0.0) + , ixy(0.0) + , ixz(0.0) + , iyy(0.0) + , iyz(0.0) + , izz(0.0) { + (void)_alloc; + } + + + + typedef double _m_type; + _m_type m; + + typedef ::geometry_msgs::Vector3_ _com_type; + _com_type com; + + typedef double _ixx_type; + _ixx_type ixx; + + typedef double _ixy_type; + _ixy_type ixy; + + typedef double _ixz_type; + _ixz_type ixz; + + typedef double _iyy_type; + _iyy_type iyy; + + typedef double _iyz_type; + _iyz_type iyz; + + typedef double _izz_type; + _izz_type izz; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Inertia_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Inertia_ const> ConstPtr; + +}; // struct Inertia_ + +typedef ::geometry_msgs::Inertia_ > Inertia; + +typedef boost::shared_ptr< ::geometry_msgs::Inertia > InertiaPtr; +typedef boost::shared_ptr< ::geometry_msgs::Inertia const> InertiaConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Inertia_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Inertia_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Inertia_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Inertia_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Inertia_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Inertia_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Inertia_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Inertia_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Inertia_ > +{ + static const char* value() + { + return "1d26e4bb6c83ff141c5cf0d883c2b0fe"; + } + + static const char* value(const ::geometry_msgs::Inertia_&) { return value(); } + static const uint64_t static_value1 = 0x1d26e4bb6c83ff14ULL; + static const uint64_t static_value2 = 0x1c5cf0d883c2b0feULL; +}; + +template +struct DataType< ::geometry_msgs::Inertia_ > +{ + static const char* value() + { + return "geometry_msgs/Inertia"; + } + + static const char* value(const ::geometry_msgs::Inertia_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Inertia_ > +{ + static const char* value() + { + return "# Mass [kg]\n\ +float64 m\n\ +\n\ +# Center of mass [m]\n\ +geometry_msgs/Vector3 com\n\ +\n\ +# Inertia Tensor [kg-m^2]\n\ +# | ixx ixy ixz |\n\ +# I = | ixy iyy iyz |\n\ +# | ixz iyz izz |\n\ +float64 ixx\n\ +float64 ixy\n\ +float64 ixz\n\ +float64 iyy\n\ +float64 iyz\n\ +float64 izz\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Inertia_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Inertia_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.m); + stream.next(m.com); + stream.next(m.ixx); + stream.next(m.ixy); + stream.next(m.ixz); + stream.next(m.iyy); + stream.next(m.iyz); + stream.next(m.izz); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Inertia_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Inertia_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Inertia_& v) + { + s << indent << "m: "; + Printer::stream(s, indent + " ", v.m); + s << indent << "com: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.com); + s << indent << "ixx: "; + Printer::stream(s, indent + " ", v.ixx); + s << indent << "ixy: "; + Printer::stream(s, indent + " ", v.ixy); + s << indent << "ixz: "; + Printer::stream(s, indent + " ", v.ixz); + s << indent << "iyy: "; + Printer::stream(s, indent + " ", v.iyy); + s << indent << "iyz: "; + Printer::stream(s, indent + " ", v.iyz); + s << indent << "izz: "; + Printer::stream(s, indent + " ", v.izz); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_INERTIA_H diff --git a/thirdparty/ros/include/geometry_msgs/InertiaStamped.h b/thirdparty/ros/include/geometry_msgs/InertiaStamped.h new file mode 100644 index 0000000..3b2e2c6 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/InertiaStamped.h @@ -0,0 +1,250 @@ +// Generated by gencpp from file geometry_msgs/InertiaStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_INERTIASTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_INERTIASTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct InertiaStamped_ +{ + typedef InertiaStamped_ Type; + + InertiaStamped_() + : header() + , inertia() { + } + InertiaStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , inertia(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Inertia_ _inertia_type; + _inertia_type inertia; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::InertiaStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::InertiaStamped_ const> ConstPtr; + +}; // struct InertiaStamped_ + +typedef ::geometry_msgs::InertiaStamped_ > InertiaStamped; + +typedef boost::shared_ptr< ::geometry_msgs::InertiaStamped > InertiaStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::InertiaStamped const> InertiaStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::InertiaStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::InertiaStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::InertiaStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::InertiaStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::InertiaStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::InertiaStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::InertiaStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::InertiaStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::InertiaStamped_ > +{ + static const char* value() + { + return "ddee48caeab5a966c5e8d166654a9ac7"; + } + + static const char* value(const ::geometry_msgs::InertiaStamped_&) { return value(); } + static const uint64_t static_value1 = 0xddee48caeab5a966ULL; + static const uint64_t static_value2 = 0xc5e8d166654a9ac7ULL; +}; + +template +struct DataType< ::geometry_msgs::InertiaStamped_ > +{ + static const char* value() + { + return "geometry_msgs/InertiaStamped"; + } + + static const char* value(const ::geometry_msgs::InertiaStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::InertiaStamped_ > +{ + static const char* value() + { + return "Header header\n\ +Inertia inertia\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Inertia\n\ +# Mass [kg]\n\ +float64 m\n\ +\n\ +# Center of mass [m]\n\ +geometry_msgs/Vector3 com\n\ +\n\ +# Inertia Tensor [kg-m^2]\n\ +# | ixx ixy ixz |\n\ +# I = | ixy iyy iyz |\n\ +# | ixz iyz izz |\n\ +float64 ixx\n\ +float64 ixy\n\ +float64 ixz\n\ +float64 iyy\n\ +float64 iyz\n\ +float64 izz\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::InertiaStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::InertiaStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.inertia); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct InertiaStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::InertiaStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::InertiaStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "inertia: "; + s << std::endl; + Printer< ::geometry_msgs::Inertia_ >::stream(s, indent + " ", v.inertia); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_INERTIASTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Point.h b/thirdparty/ros/include/geometry_msgs/Point.h new file mode 100644 index 0000000..da5933d --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Point.h @@ -0,0 +1,206 @@ +// Generated by gencpp from file geometry_msgs/Point.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POINT_H +#define GEOMETRY_MSGS_MESSAGE_POINT_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Point_ +{ + typedef Point_ Type; + + Point_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Point_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Point_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Point_ const> ConstPtr; + +}; // struct Point_ + +typedef ::geometry_msgs::Point_ > Point; + +typedef boost::shared_ptr< ::geometry_msgs::Point > PointPtr; +typedef boost::shared_ptr< ::geometry_msgs::Point const> PointConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Point_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Point_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Point_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Point_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Point_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Point_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Point_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Point_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Point_ > +{ + static const char* value() + { + return "4a842b65f413084dc2b10fb484ea7f17"; + } + + static const char* value(const ::geometry_msgs::Point_&) { return value(); } + static const uint64_t static_value1 = 0x4a842b65f413084dULL; + static const uint64_t static_value2 = 0xc2b10fb484ea7f17ULL; +}; + +template +struct DataType< ::geometry_msgs::Point_ > +{ + static const char* value() + { + return "geometry_msgs/Point"; + } + + static const char* value(const ::geometry_msgs::Point_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Point_ > +{ + static const char* value() + { + return "# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Point_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Point_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Point_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Point_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Point_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POINT_H diff --git a/thirdparty/ros/include/geometry_msgs/Point32.h b/thirdparty/ros/include/geometry_msgs/Point32.h new file mode 100644 index 0000000..6fa1aa1 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Point32.h @@ -0,0 +1,213 @@ +// Generated by gencpp from file geometry_msgs/Point32.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POINT32_H +#define GEOMETRY_MSGS_MESSAGE_POINT32_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Point32_ +{ + typedef Point32_ Type; + + Point32_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Point32_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Point32_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Point32_ const> ConstPtr; + +}; // struct Point32_ + +typedef ::geometry_msgs::Point32_ > Point32; + +typedef boost::shared_ptr< ::geometry_msgs::Point32 > Point32Ptr; +typedef boost::shared_ptr< ::geometry_msgs::Point32 const> Point32ConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Point32_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Point32_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Point32_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Point32_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Point32_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Point32_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Point32_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Point32_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Point32_ > +{ + static const char* value() + { + return "cc153912f1453b708d221682bc23d9ac"; + } + + static const char* value(const ::geometry_msgs::Point32_&) { return value(); } + static const uint64_t static_value1 = 0xcc153912f1453b70ULL; + static const uint64_t static_value2 = 0x8d221682bc23d9acULL; +}; + +template +struct DataType< ::geometry_msgs::Point32_ > +{ + static const char* value() + { + return "geometry_msgs/Point32"; + } + + static const char* value(const ::geometry_msgs::Point32_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Point32_ > +{ + static const char* value() + { + return "# This contains the position of a point in free space(with 32 bits of precision).\n\ +# It is recommeded to use Point wherever possible instead of Point32. \n\ +# \n\ +# This recommendation is to promote interoperability. \n\ +#\n\ +# This message is designed to take up less space when sending\n\ +# lots of points at once, as in the case of a PointCloud. \n\ +\n\ +float32 x\n\ +float32 y\n\ +float32 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Point32_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Point32_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Point32_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Point32_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Point32_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POINT32_H diff --git a/thirdparty/ros/include/geometry_msgs/PointStamped.h b/thirdparty/ros/include/geometry_msgs/PointStamped.h new file mode 100644 index 0000000..4a037e6 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/PointStamped.h @@ -0,0 +1,226 @@ +// Generated by gencpp from file geometry_msgs/PointStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct PointStamped_ +{ + typedef PointStamped_ Type; + + PointStamped_() + : header() + , point() { + } + PointStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , point(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Point_ _point_type; + _point_type point; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PointStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PointStamped_ const> ConstPtr; + +}; // struct PointStamped_ + +typedef ::geometry_msgs::PointStamped_ > PointStamped; + +typedef boost::shared_ptr< ::geometry_msgs::PointStamped > PointStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::PointStamped const> PointStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PointStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PointStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::PointStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PointStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::PointStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PointStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PointStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PointStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PointStamped_ > +{ + static const char* value() + { + return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; + } + + static const char* value(const ::geometry_msgs::PointStamped_&) { return value(); } + static const uint64_t static_value1 = 0xc63aecb41bfdfd6bULL; + static const uint64_t static_value2 = 0x7e1fac37c7cbe7bfULL; +}; + +template +struct DataType< ::geometry_msgs::PointStamped_ > +{ + static const char* value() + { + return "geometry_msgs/PointStamped"; + } + + static const char* value(const ::geometry_msgs::PointStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PointStamped_ > +{ + static const char* value() + { + return "# This represents a Point with reference coordinate frame and timestamp\n\ +Header header\n\ +Point point\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::PointStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PointStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.point); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PointStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PointStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PointStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "point: "; + s << std::endl; + Printer< ::geometry_msgs::Point_ >::stream(s, indent + " ", v.point); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Polygon.h b/thirdparty/ros/include/geometry_msgs/Polygon.h new file mode 100644 index 0000000..1a82282 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Polygon.h @@ -0,0 +1,209 @@ +// Generated by gencpp from file geometry_msgs/Polygon.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POLYGON_H +#define GEOMETRY_MSGS_MESSAGE_POLYGON_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct Polygon_ +{ + typedef Polygon_ Type; + + Polygon_() + : points() { + } + Polygon_(const ContainerAllocator& _alloc) + : points(_alloc) { + (void)_alloc; + } + + + + typedef std::vector< ::geometry_msgs::Point32_ , typename ContainerAllocator::template rebind< ::geometry_msgs::Point32_ >::other > _points_type; + _points_type points; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Polygon_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Polygon_ const> ConstPtr; + +}; // struct Polygon_ + +typedef ::geometry_msgs::Polygon_ > Polygon; + +typedef boost::shared_ptr< ::geometry_msgs::Polygon > PolygonPtr; +typedef boost::shared_ptr< ::geometry_msgs::Polygon const> PolygonConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Polygon_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Polygon_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Polygon_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Polygon_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::Polygon_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Polygon_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Polygon_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Polygon_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Polygon_ > +{ + static const char* value() + { + return "cd60a26494a087f577976f0329fa120e"; + } + + static const char* value(const ::geometry_msgs::Polygon_&) { return value(); } + static const uint64_t static_value1 = 0xcd60a26494a087f5ULL; + static const uint64_t static_value2 = 0x77976f0329fa120eULL; +}; + +template +struct DataType< ::geometry_msgs::Polygon_ > +{ + static const char* value() + { + return "geometry_msgs/Polygon"; + } + + static const char* value(const ::geometry_msgs::Polygon_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Polygon_ > +{ + static const char* value() + { + return "#A specification of a polygon where the first and last points are assumed to be connected\n\ +Point32[] points\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point32\n\ +# This contains the position of a point in free space(with 32 bits of precision).\n\ +# It is recommeded to use Point wherever possible instead of Point32. \n\ +# \n\ +# This recommendation is to promote interoperability. \n\ +#\n\ +# This message is designed to take up less space when sending\n\ +# lots of points at once, as in the case of a PointCloud. \n\ +\n\ +float32 x\n\ +float32 y\n\ +float32 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Polygon_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Polygon_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.points); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Polygon_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Polygon_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Polygon_& v) + { + s << indent << "points[]" << std::endl; + for (size_t i = 0; i < v.points.size(); ++i) + { + s << indent << " points[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::Point32_ >::stream(s, indent + " ", v.points[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POLYGON_H diff --git a/thirdparty/ros/include/geometry_msgs/PolygonStamped.h b/thirdparty/ros/include/geometry_msgs/PolygonStamped.h new file mode 100644 index 0000000..45f0b5d --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/PolygonStamped.h @@ -0,0 +1,238 @@ +// Generated by gencpp from file geometry_msgs/PolygonStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POLYGONSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_POLYGONSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct PolygonStamped_ +{ + typedef PolygonStamped_ Type; + + PolygonStamped_() + : header() + , polygon() { + } + PolygonStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , polygon(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Polygon_ _polygon_type; + _polygon_type polygon; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PolygonStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PolygonStamped_ const> ConstPtr; + +}; // struct PolygonStamped_ + +typedef ::geometry_msgs::PolygonStamped_ > PolygonStamped; + +typedef boost::shared_ptr< ::geometry_msgs::PolygonStamped > PolygonStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::PolygonStamped const> PolygonStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PolygonStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PolygonStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::PolygonStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PolygonStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::PolygonStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PolygonStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PolygonStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PolygonStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PolygonStamped_ > +{ + static const char* value() + { + return "c6be8f7dc3bee7fe9e8d296070f53340"; + } + + static const char* value(const ::geometry_msgs::PolygonStamped_&) { return value(); } + static const uint64_t static_value1 = 0xc6be8f7dc3bee7feULL; + static const uint64_t static_value2 = 0x9e8d296070f53340ULL; +}; + +template +struct DataType< ::geometry_msgs::PolygonStamped_ > +{ + static const char* value() + { + return "geometry_msgs/PolygonStamped"; + } + + static const char* value(const ::geometry_msgs::PolygonStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PolygonStamped_ > +{ + static const char* value() + { + return "# This represents a Polygon with reference coordinate frame and timestamp\n\ +Header header\n\ +Polygon polygon\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Polygon\n\ +#A specification of a polygon where the first and last points are assumed to be connected\n\ +Point32[] points\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point32\n\ +# This contains the position of a point in free space(with 32 bits of precision).\n\ +# It is recommeded to use Point wherever possible instead of Point32. \n\ +# \n\ +# This recommendation is to promote interoperability. \n\ +#\n\ +# This message is designed to take up less space when sending\n\ +# lots of points at once, as in the case of a PointCloud. \n\ +\n\ +float32 x\n\ +float32 y\n\ +float32 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::PolygonStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PolygonStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.polygon); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PolygonStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PolygonStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PolygonStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "polygon: "; + s << std::endl; + Printer< ::geometry_msgs::Polygon_ >::stream(s, indent + " ", v.polygon); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POLYGONSTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Pose.h b/thirdparty/ros/include/geometry_msgs/Pose.h new file mode 100644 index 0000000..8e6a610 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Pose.h @@ -0,0 +1,217 @@ +// Generated by gencpp from file geometry_msgs/Pose.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSE_H +#define GEOMETRY_MSGS_MESSAGE_POSE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Pose_ +{ + typedef Pose_ Type; + + Pose_() + : position() + , orientation() { + } + Pose_(const ContainerAllocator& _alloc) + : position(_alloc) + , orientation(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Point_ _position_type; + _position_type position; + + typedef ::geometry_msgs::Quaternion_ _orientation_type; + _orientation_type orientation; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Pose_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Pose_ const> ConstPtr; + +}; // struct Pose_ + +typedef ::geometry_msgs::Pose_ > Pose; + +typedef boost::shared_ptr< ::geometry_msgs::Pose > PosePtr; +typedef boost::shared_ptr< ::geometry_msgs::Pose const> PoseConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Pose_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Pose_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Pose_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Pose_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Pose_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Pose_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Pose_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Pose_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Pose_ > +{ + static const char* value() + { + return "e45d45a5a1ce597b249e23fb30fc871f"; + } + + static const char* value(const ::geometry_msgs::Pose_&) { return value(); } + static const uint64_t static_value1 = 0xe45d45a5a1ce597bULL; + static const uint64_t static_value2 = 0x249e23fb30fc871fULL; +}; + +template +struct DataType< ::geometry_msgs::Pose_ > +{ + static const char* value() + { + return "geometry_msgs/Pose"; + } + + static const char* value(const ::geometry_msgs::Pose_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Pose_ > +{ + static const char* value() + { + return "# A representation of pose in free space, composed of position and orientation. \n\ +Point position\n\ +Quaternion orientation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::Pose_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Pose_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.position); + stream.next(m.orientation); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Pose_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Pose_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Pose_& v) + { + s << indent << "position: "; + s << std::endl; + Printer< ::geometry_msgs::Point_ >::stream(s, indent + " ", v.position); + s << indent << "orientation: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.orientation); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSE_H diff --git a/thirdparty/ros/include/geometry_msgs/Pose2D.h b/thirdparty/ros/include/geometry_msgs/Pose2D.h new file mode 100644 index 0000000..041fe13 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Pose2D.h @@ -0,0 +1,207 @@ +// Generated by gencpp from file geometry_msgs/Pose2D.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSE2D_H +#define GEOMETRY_MSGS_MESSAGE_POSE2D_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Pose2D_ +{ + typedef Pose2D_ Type; + + Pose2D_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Pose2D_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _theta_type; + _theta_type theta; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Pose2D_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Pose2D_ const> ConstPtr; + +}; // struct Pose2D_ + +typedef ::geometry_msgs::Pose2D_ > Pose2D; + +typedef boost::shared_ptr< ::geometry_msgs::Pose2D > Pose2DPtr; +typedef boost::shared_ptr< ::geometry_msgs::Pose2D const> Pose2DConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Pose2D_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Pose2D_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Pose2D_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Pose2D_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Pose2D_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Pose2D_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Pose2D_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Pose2D_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Pose2D_ > +{ + static const char* value() + { + return "938fa65709584ad8e77d238529be13b8"; + } + + static const char* value(const ::geometry_msgs::Pose2D_&) { return value(); } + static const uint64_t static_value1 = 0x938fa65709584ad8ULL; + static const uint64_t static_value2 = 0xe77d238529be13b8ULL; +}; + +template +struct DataType< ::geometry_msgs::Pose2D_ > +{ + static const char* value() + { + return "geometry_msgs/Pose2D"; + } + + static const char* value(const ::geometry_msgs::Pose2D_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Pose2D_ > +{ + static const char* value() + { + return "# This expresses a position and orientation on a 2D manifold.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 theta\n\ +"; + } + + static const char* value(const ::geometry_msgs::Pose2D_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Pose2D_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.theta); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Pose2D_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Pose2D_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Pose2D_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "theta: "; + Printer::stream(s, indent + " ", v.theta); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSE2D_H diff --git a/thirdparty/ros/include/geometry_msgs/PoseArray.h b/thirdparty/ros/include/geometry_msgs/PoseArray.h new file mode 100644 index 0000000..2f77767 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/PoseArray.h @@ -0,0 +1,248 @@ +// Generated by gencpp from file geometry_msgs/PoseArray.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSEARRAY_H +#define GEOMETRY_MSGS_MESSAGE_POSEARRAY_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct PoseArray_ +{ + typedef PoseArray_ Type; + + PoseArray_() + : header() + , poses() { + } + PoseArray_(const ContainerAllocator& _alloc) + : header(_alloc) + , poses(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector< ::geometry_msgs::Pose_ , typename ContainerAllocator::template rebind< ::geometry_msgs::Pose_ >::other > _poses_type; + _poses_type poses; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PoseArray_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PoseArray_ const> ConstPtr; + +}; // struct PoseArray_ + +typedef ::geometry_msgs::PoseArray_ > PoseArray; + +typedef boost::shared_ptr< ::geometry_msgs::PoseArray > PoseArrayPtr; +typedef boost::shared_ptr< ::geometry_msgs::PoseArray const> PoseArrayConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PoseArray_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PoseArray_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::PoseArray_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PoseArray_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseArray_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseArray_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseArray_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseArray_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PoseArray_ > +{ + static const char* value() + { + return "916c28c5764443f268b296bb671b9d97"; + } + + static const char* value(const ::geometry_msgs::PoseArray_&) { return value(); } + static const uint64_t static_value1 = 0x916c28c5764443f2ULL; + static const uint64_t static_value2 = 0x68b296bb671b9d97ULL; +}; + +template +struct DataType< ::geometry_msgs::PoseArray_ > +{ + static const char* value() + { + return "geometry_msgs/PoseArray"; + } + + static const char* value(const ::geometry_msgs::PoseArray_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PoseArray_ > +{ + static const char* value() + { + return "# An array of poses with a header for global reference.\n\ +\n\ +Header header\n\ +\n\ +Pose[] poses\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Pose\n\ +# A representation of pose in free space, composed of position and orientation. \n\ +Point position\n\ +Quaternion orientation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::PoseArray_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PoseArray_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.poses); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PoseArray_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PoseArray_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PoseArray_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "poses[]" << std::endl; + for (size_t i = 0; i < v.poses.size(); ++i) + { + s << indent << " poses[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::Pose_ >::stream(s, indent + " ", v.poses[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSEARRAY_H diff --git a/thirdparty/ros/include/geometry_msgs/PoseStamped.h b/thirdparty/ros/include/geometry_msgs/PoseStamped.h new file mode 100644 index 0000000..f476a10 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/PoseStamped.h @@ -0,0 +1,241 @@ +// Generated by gencpp from file geometry_msgs/PoseStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct PoseStamped_ +{ + typedef PoseStamped_ Type; + + PoseStamped_() + : header() + , pose() { + } + PoseStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , pose(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Pose_ _pose_type; + _pose_type pose; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_ const> ConstPtr; + +}; // struct PoseStamped_ + +typedef ::geometry_msgs::PoseStamped_ > PoseStamped; + +typedef boost::shared_ptr< ::geometry_msgs::PoseStamped > PoseStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::PoseStamped const> PoseStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PoseStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PoseStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::PoseStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PoseStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PoseStamped_ > +{ + static const char* value() + { + return "d3812c3cbc69362b77dc0b19b345f8f5"; + } + + static const char* value(const ::geometry_msgs::PoseStamped_&) { return value(); } + static const uint64_t static_value1 = 0xd3812c3cbc69362bULL; + static const uint64_t static_value2 = 0x77dc0b19b345f8f5ULL; +}; + +template +struct DataType< ::geometry_msgs::PoseStamped_ > +{ + static const char* value() + { + return "geometry_msgs/PoseStamped"; + } + + static const char* value(const ::geometry_msgs::PoseStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PoseStamped_ > +{ + static const char* value() + { + return "# A Pose with reference coordinate frame and timestamp\n\ +Header header\n\ +Pose pose\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Pose\n\ +# A representation of pose in free space, composed of position and orientation. \n\ +Point position\n\ +Quaternion orientation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::PoseStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PoseStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.pose); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PoseStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PoseStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PoseStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "pose: "; + s << std::endl; + Printer< ::geometry_msgs::Pose_ >::stream(s, indent + " ", v.pose); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/PoseWithCovariance.h b/thirdparty/ros/include/geometry_msgs/PoseWithCovariance.h new file mode 100644 index 0000000..4382743 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/PoseWithCovariance.h @@ -0,0 +1,233 @@ +// Generated by gencpp from file geometry_msgs/PoseWithCovariance.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H +#define GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct PoseWithCovariance_ +{ + typedef PoseWithCovariance_ Type; + + PoseWithCovariance_() + : pose() + , covariance() { + covariance.assign(0.0); + } + PoseWithCovariance_(const ContainerAllocator& _alloc) + : pose(_alloc) + , covariance() { + (void)_alloc; + covariance.assign(0.0); + } + + + + typedef ::geometry_msgs::Pose_ _pose_type; + _pose_type pose; + + typedef boost::array _covariance_type; + _covariance_type covariance; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_ const> ConstPtr; + +}; // struct PoseWithCovariance_ + +typedef ::geometry_msgs::PoseWithCovariance_ > PoseWithCovariance; + +typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance > PoseWithCovariancePtr; +typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance const> PoseWithCovarianceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PoseWithCovariance_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PoseWithCovariance_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::PoseWithCovariance_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PoseWithCovariance_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseWithCovariance_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseWithCovariance_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseWithCovariance_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseWithCovariance_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PoseWithCovariance_ > +{ + static const char* value() + { + return "c23e848cf1b7533a8d7c259073a97e6f"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovariance_&) { return value(); } + static const uint64_t static_value1 = 0xc23e848cf1b7533aULL; + static const uint64_t static_value2 = 0x8d7c259073a97e6fULL; +}; + +template +struct DataType< ::geometry_msgs::PoseWithCovariance_ > +{ + static const char* value() + { + return "geometry_msgs/PoseWithCovariance"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovariance_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PoseWithCovariance_ > +{ + static const char* value() + { + return "# This represents a pose in free space with uncertainty.\n\ +\n\ +Pose pose\n\ +\n\ +# Row-major representation of the 6x6 covariance matrix\n\ +# The orientation parameters use a fixed-axis representation.\n\ +# In order, the parameters are:\n\ +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n\ +float64[36] covariance\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Pose\n\ +# A representation of pose in free space, composed of position and orientation. \n\ +Point position\n\ +Quaternion orientation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovariance_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PoseWithCovariance_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.pose); + stream.next(m.covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PoseWithCovariance_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PoseWithCovariance_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PoseWithCovariance_& v) + { + s << indent << "pose: "; + s << std::endl; + Printer< ::geometry_msgs::Pose_ >::stream(s, indent + " ", v.pose); + s << indent << "covariance[]" << std::endl; + for (size_t i = 0; i < v.covariance.size(); ++i) + { + s << indent << " covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H diff --git a/thirdparty/ros/include/geometry_msgs/PoseWithCovarianceStamped.h b/thirdparty/ros/include/geometry_msgs/PoseWithCovarianceStamped.h new file mode 100644 index 0000000..6662115 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/PoseWithCovarianceStamped.h @@ -0,0 +1,254 @@ +// Generated by gencpp from file geometry_msgs/PoseWithCovarianceStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCESTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCESTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct PoseWithCovarianceStamped_ +{ + typedef PoseWithCovarianceStamped_ Type; + + PoseWithCovarianceStamped_() + : header() + , pose() { + } + PoseWithCovarianceStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , pose(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::PoseWithCovariance_ _pose_type; + _pose_type pose; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovarianceStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovarianceStamped_ const> ConstPtr; + +}; // struct PoseWithCovarianceStamped_ + +typedef ::geometry_msgs::PoseWithCovarianceStamped_ > PoseWithCovarianceStamped; + +typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovarianceStamped > PoseWithCovarianceStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovarianceStamped const> PoseWithCovarianceStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PoseWithCovarianceStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PoseWithCovarianceStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::PoseWithCovarianceStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PoseWithCovarianceStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseWithCovarianceStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseWithCovarianceStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseWithCovarianceStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseWithCovarianceStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PoseWithCovarianceStamped_ > +{ + static const char* value() + { + return "953b798c0f514ff060a53a3498ce6246"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovarianceStamped_&) { return value(); } + static const uint64_t static_value1 = 0x953b798c0f514ff0ULL; + static const uint64_t static_value2 = 0x60a53a3498ce6246ULL; +}; + +template +struct DataType< ::geometry_msgs::PoseWithCovarianceStamped_ > +{ + static const char* value() + { + return "geometry_msgs/PoseWithCovarianceStamped"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovarianceStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PoseWithCovarianceStamped_ > +{ + static const char* value() + { + return "# This expresses an estimated pose with a reference coordinate frame and timestamp\n\ +\n\ +Header header\n\ +PoseWithCovariance pose\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/PoseWithCovariance\n\ +# This represents a pose in free space with uncertainty.\n\ +\n\ +Pose pose\n\ +\n\ +# Row-major representation of the 6x6 covariance matrix\n\ +# The orientation parameters use a fixed-axis representation.\n\ +# In order, the parameters are:\n\ +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n\ +float64[36] covariance\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Pose\n\ +# A representation of pose in free space, composed of position and orientation. \n\ +Point position\n\ +Quaternion orientation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovarianceStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PoseWithCovarianceStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.pose); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PoseWithCovarianceStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PoseWithCovarianceStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PoseWithCovarianceStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "pose: "; + s << std::endl; + Printer< ::geometry_msgs::PoseWithCovariance_ >::stream(s, indent + " ", v.pose); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCESTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Quaternion.h b/thirdparty/ros/include/geometry_msgs/Quaternion.h new file mode 100644 index 0000000..8085528 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Quaternion.h @@ -0,0 +1,216 @@ +// Generated by gencpp from file geometry_msgs/Quaternion.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_QUATERNION_H +#define GEOMETRY_MSGS_MESSAGE_QUATERNION_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Quaternion_ +{ + typedef Quaternion_ Type; + + Quaternion_() + : x(0.0) + , y(0.0) + , z(0.0) + , w(0.0) { + } + Quaternion_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) + , w(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _z_type; + _z_type z; + + typedef double _w_type; + _w_type w; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ const> ConstPtr; + +}; // struct Quaternion_ + +typedef ::geometry_msgs::Quaternion_ > Quaternion; + +typedef boost::shared_ptr< ::geometry_msgs::Quaternion > QuaternionPtr; +typedef boost::shared_ptr< ::geometry_msgs::Quaternion const> QuaternionConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Quaternion_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Quaternion_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Quaternion_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Quaternion_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Quaternion_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Quaternion_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Quaternion_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Quaternion_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Quaternion_ > +{ + static const char* value() + { + return "a779879fadf0160734f906b8c19c7004"; + } + + static const char* value(const ::geometry_msgs::Quaternion_&) { return value(); } + static const uint64_t static_value1 = 0xa779879fadf01607ULL; + static const uint64_t static_value2 = 0x34f906b8c19c7004ULL; +}; + +template +struct DataType< ::geometry_msgs::Quaternion_ > +{ + static const char* value() + { + return "geometry_msgs/Quaternion"; + } + + static const char* value(const ::geometry_msgs::Quaternion_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Quaternion_ > +{ + static const char* value() + { + return "# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::Quaternion_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Quaternion_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + stream.next(m.w); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Quaternion_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Quaternion_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Quaternion_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + s << indent << "w: "; + Printer::stream(s, indent + " ", v.w); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_QUATERNION_H diff --git a/thirdparty/ros/include/geometry_msgs/QuaternionStamped.h b/thirdparty/ros/include/geometry_msgs/QuaternionStamped.h new file mode 100644 index 0000000..e40ef5a --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/QuaternionStamped.h @@ -0,0 +1,229 @@ +// Generated by gencpp from file geometry_msgs/QuaternionStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_QUATERNIONSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_QUATERNIONSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct QuaternionStamped_ +{ + typedef QuaternionStamped_ Type; + + QuaternionStamped_() + : header() + , quaternion() { + } + QuaternionStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , quaternion(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Quaternion_ _quaternion_type; + _quaternion_type quaternion; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::QuaternionStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::QuaternionStamped_ const> ConstPtr; + +}; // struct QuaternionStamped_ + +typedef ::geometry_msgs::QuaternionStamped_ > QuaternionStamped; + +typedef boost::shared_ptr< ::geometry_msgs::QuaternionStamped > QuaternionStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::QuaternionStamped const> QuaternionStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::QuaternionStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::QuaternionStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::QuaternionStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::QuaternionStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::QuaternionStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::QuaternionStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::QuaternionStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::QuaternionStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::QuaternionStamped_ > +{ + static const char* value() + { + return "e57f1e547e0e1fd13504588ffc8334e2"; + } + + static const char* value(const ::geometry_msgs::QuaternionStamped_&) { return value(); } + static const uint64_t static_value1 = 0xe57f1e547e0e1fd1ULL; + static const uint64_t static_value2 = 0x3504588ffc8334e2ULL; +}; + +template +struct DataType< ::geometry_msgs::QuaternionStamped_ > +{ + static const char* value() + { + return "geometry_msgs/QuaternionStamped"; + } + + static const char* value(const ::geometry_msgs::QuaternionStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::QuaternionStamped_ > +{ + static const char* value() + { + return "# This represents an orientation with reference coordinate frame and timestamp.\n\ +\n\ +Header header\n\ +Quaternion quaternion\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::QuaternionStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::QuaternionStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.quaternion); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct QuaternionStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::QuaternionStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::QuaternionStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "quaternion: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.quaternion); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_QUATERNIONSTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Transform.h b/thirdparty/ros/include/geometry_msgs/Transform.h new file mode 100644 index 0000000..aa3b340 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Transform.h @@ -0,0 +1,223 @@ +// Generated by gencpp from file geometry_msgs/Transform.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TRANSFORM_H +#define GEOMETRY_MSGS_MESSAGE_TRANSFORM_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Transform_ +{ + typedef Transform_ Type; + + Transform_() + : translation() + , rotation() { + } + Transform_(const ContainerAllocator& _alloc) + : translation(_alloc) + , rotation(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Vector3_ _translation_type; + _translation_type translation; + + typedef ::geometry_msgs::Quaternion_ _rotation_type; + _rotation_type rotation; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Transform_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Transform_ const> ConstPtr; + +}; // struct Transform_ + +typedef ::geometry_msgs::Transform_ > Transform; + +typedef boost::shared_ptr< ::geometry_msgs::Transform > TransformPtr; +typedef boost::shared_ptr< ::geometry_msgs::Transform const> TransformConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Transform_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Transform_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Transform_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Transform_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Transform_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Transform_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Transform_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Transform_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Transform_ > +{ + static const char* value() + { + return "ac9eff44abf714214112b05d54a3cf9b"; + } + + static const char* value(const ::geometry_msgs::Transform_&) { return value(); } + static const uint64_t static_value1 = 0xac9eff44abf71421ULL; + static const uint64_t static_value2 = 0x4112b05d54a3cf9bULL; +}; + +template +struct DataType< ::geometry_msgs::Transform_ > +{ + static const char* value() + { + return "geometry_msgs/Transform"; + } + + static const char* value(const ::geometry_msgs::Transform_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Transform_ > +{ + static const char* value() + { + return "# This represents the transform between two coordinate frames in free space.\n\ +\n\ +Vector3 translation\n\ +Quaternion rotation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::Transform_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Transform_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.translation); + stream.next(m.rotation); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Transform_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Transform_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Transform_& v) + { + s << indent << "translation: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.translation); + s << indent << "rotation: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.rotation); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TRANSFORM_H diff --git a/thirdparty/ros/include/geometry_msgs/TransformStamped.h b/thirdparty/ros/include/geometry_msgs/TransformStamped.h new file mode 100644 index 0000000..7e2ac71 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/TransformStamped.h @@ -0,0 +1,262 @@ +// Generated by gencpp from file geometry_msgs/TransformStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TRANSFORMSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_TRANSFORMSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct TransformStamped_ +{ + typedef TransformStamped_ Type; + + TransformStamped_() + : header() + , child_frame_id() + , transform() { + } + TransformStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , child_frame_id(_alloc) + , transform(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _child_frame_id_type; + _child_frame_id_type child_frame_id; + + typedef ::geometry_msgs::Transform_ _transform_type; + _transform_type transform; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::TransformStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::TransformStamped_ const> ConstPtr; + +}; // struct TransformStamped_ + +typedef ::geometry_msgs::TransformStamped_ > TransformStamped; + +typedef boost::shared_ptr< ::geometry_msgs::TransformStamped > TransformStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::TransformStamped const> TransformStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::TransformStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::TransformStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::TransformStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TransformStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::TransformStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TransformStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TransformStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TransformStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::TransformStamped_ > +{ + static const char* value() + { + return "b5764a33bfeb3588febc2682852579b0"; + } + + static const char* value(const ::geometry_msgs::TransformStamped_&) { return value(); } + static const uint64_t static_value1 = 0xb5764a33bfeb3588ULL; + static const uint64_t static_value2 = 0xfebc2682852579b0ULL; +}; + +template +struct DataType< ::geometry_msgs::TransformStamped_ > +{ + static const char* value() + { + return "geometry_msgs/TransformStamped"; + } + + static const char* value(const ::geometry_msgs::TransformStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::TransformStamped_ > +{ + static const char* value() + { + return "# This expresses a transform from coordinate frame header.frame_id\n\ +# to the coordinate frame child_frame_id\n\ +#\n\ +# This message is mostly used by the \n\ +# tf package. \n\ +# See its documentation for more information.\n\ +\n\ +Header header\n\ +string child_frame_id # the frame id of the child frame\n\ +Transform transform\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Transform\n\ +# This represents the transform between two coordinate frames in free space.\n\ +\n\ +Vector3 translation\n\ +Quaternion rotation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +"; + } + + static const char* value(const ::geometry_msgs::TransformStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::TransformStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.child_frame_id); + stream.next(m.transform); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TransformStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::TransformStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::TransformStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "child_frame_id: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.child_frame_id); + s << indent << "transform: "; + s << std::endl; + Printer< ::geometry_msgs::Transform_ >::stream(s, indent + " ", v.transform); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TRANSFORMSTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Twist.h b/thirdparty/ros/include/geometry_msgs/Twist.h new file mode 100644 index 0000000..6cb41a3 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Twist.h @@ -0,0 +1,214 @@ +// Generated by gencpp from file geometry_msgs/Twist.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TWIST_H +#define GEOMETRY_MSGS_MESSAGE_TWIST_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Twist_ +{ + typedef Twist_ Type; + + Twist_() + : linear() + , angular() { + } + Twist_(const ContainerAllocator& _alloc) + : linear(_alloc) + , angular(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Vector3_ _linear_type; + _linear_type linear; + + typedef ::geometry_msgs::Vector3_ _angular_type; + _angular_type angular; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Twist_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Twist_ const> ConstPtr; + +}; // struct Twist_ + +typedef ::geometry_msgs::Twist_ > Twist; + +typedef boost::shared_ptr< ::geometry_msgs::Twist > TwistPtr; +typedef boost::shared_ptr< ::geometry_msgs::Twist const> TwistConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Twist_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Twist_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Twist_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Twist_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Twist_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Twist_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Twist_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Twist_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Twist_ > +{ + static const char* value() + { + return "9f195f881246fdfa2798d1d3eebca84a"; + } + + static const char* value(const ::geometry_msgs::Twist_&) { return value(); } + static const uint64_t static_value1 = 0x9f195f881246fdfaULL; + static const uint64_t static_value2 = 0x2798d1d3eebca84aULL; +}; + +template +struct DataType< ::geometry_msgs::Twist_ > +{ + static const char* value() + { + return "geometry_msgs/Twist"; + } + + static const char* value(const ::geometry_msgs::Twist_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Twist_ > +{ + static const char* value() + { + return "# This expresses velocity in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Twist_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Twist_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.linear); + stream.next(m.angular); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Twist_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Twist_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Twist_& v) + { + s << indent << "linear: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.linear); + s << indent << "angular: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.angular); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TWIST_H diff --git a/thirdparty/ros/include/geometry_msgs/TwistStamped.h b/thirdparty/ros/include/geometry_msgs/TwistStamped.h new file mode 100644 index 0000000..2f542e2 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/TwistStamped.h @@ -0,0 +1,238 @@ +// Generated by gencpp from file geometry_msgs/TwistStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TWISTSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_TWISTSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct TwistStamped_ +{ + typedef TwistStamped_ Type; + + TwistStamped_() + : header() + , twist() { + } + TwistStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , twist(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Twist_ _twist_type; + _twist_type twist; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::TwistStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::TwistStamped_ const> ConstPtr; + +}; // struct TwistStamped_ + +typedef ::geometry_msgs::TwistStamped_ > TwistStamped; + +typedef boost::shared_ptr< ::geometry_msgs::TwistStamped > TwistStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::TwistStamped const> TwistStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::TwistStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::TwistStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::TwistStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TwistStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::TwistStamped_ > +{ + static const char* value() + { + return "98d34b0043a2093cf9d9345ab6eef12e"; + } + + static const char* value(const ::geometry_msgs::TwistStamped_&) { return value(); } + static const uint64_t static_value1 = 0x98d34b0043a2093cULL; + static const uint64_t static_value2 = 0xf9d9345ab6eef12eULL; +}; + +template +struct DataType< ::geometry_msgs::TwistStamped_ > +{ + static const char* value() + { + return "geometry_msgs/TwistStamped"; + } + + static const char* value(const ::geometry_msgs::TwistStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::TwistStamped_ > +{ + static const char* value() + { + return "# A twist with reference coordinate frame and timestamp\n\ +Header header\n\ +Twist twist\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Twist\n\ +# This expresses velocity in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::TwistStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::TwistStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.twist); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TwistStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::TwistStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::TwistStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "twist: "; + s << std::endl; + Printer< ::geometry_msgs::Twist_ >::stream(s, indent + " ", v.twist); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TWISTSTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/TwistWithCovariance.h b/thirdparty/ros/include/geometry_msgs/TwistWithCovariance.h new file mode 100644 index 0000000..912656a --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/TwistWithCovariance.h @@ -0,0 +1,230 @@ +// Generated by gencpp from file geometry_msgs/TwistWithCovariance.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H +#define GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct TwistWithCovariance_ +{ + typedef TwistWithCovariance_ Type; + + TwistWithCovariance_() + : twist() + , covariance() { + covariance.assign(0.0); + } + TwistWithCovariance_(const ContainerAllocator& _alloc) + : twist(_alloc) + , covariance() { + (void)_alloc; + covariance.assign(0.0); + } + + + + typedef ::geometry_msgs::Twist_ _twist_type; + _twist_type twist; + + typedef boost::array _covariance_type; + _covariance_type covariance; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance_ const> ConstPtr; + +}; // struct TwistWithCovariance_ + +typedef ::geometry_msgs::TwistWithCovariance_ > TwistWithCovariance; + +typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance > TwistWithCovariancePtr; +typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance const> TwistWithCovarianceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::TwistWithCovariance_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::TwistWithCovariance_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::TwistWithCovariance_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TwistWithCovariance_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistWithCovariance_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistWithCovariance_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistWithCovariance_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistWithCovariance_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::TwistWithCovariance_ > +{ + static const char* value() + { + return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovariance_&) { return value(); } + static const uint64_t static_value1 = 0x1fe8a28e6890a4ccULL; + static const uint64_t static_value2 = 0x3ae4c3ca5c7d82e6ULL; +}; + +template +struct DataType< ::geometry_msgs::TwistWithCovariance_ > +{ + static const char* value() + { + return "geometry_msgs/TwistWithCovariance"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovariance_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::TwistWithCovariance_ > +{ + static const char* value() + { + return "# This expresses velocity in free space with uncertainty.\n\ +\n\ +Twist twist\n\ +\n\ +# Row-major representation of the 6x6 covariance matrix\n\ +# The orientation parameters use a fixed-axis representation.\n\ +# In order, the parameters are:\n\ +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n\ +float64[36] covariance\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Twist\n\ +# This expresses velocity in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovariance_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::TwistWithCovariance_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.twist); + stream.next(m.covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TwistWithCovariance_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::TwistWithCovariance_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::TwistWithCovariance_& v) + { + s << indent << "twist: "; + s << std::endl; + Printer< ::geometry_msgs::Twist_ >::stream(s, indent + " ", v.twist); + s << indent << "covariance[]" << std::endl; + for (size_t i = 0; i < v.covariance.size(); ++i) + { + s << indent << " covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H diff --git a/thirdparty/ros/include/geometry_msgs/TwistWithCovarianceStamped.h b/thirdparty/ros/include/geometry_msgs/TwistWithCovarianceStamped.h new file mode 100644 index 0000000..107a806 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/TwistWithCovarianceStamped.h @@ -0,0 +1,250 @@ +// Generated by gencpp from file geometry_msgs/TwistWithCovarianceStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCESTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCESTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct TwistWithCovarianceStamped_ +{ + typedef TwistWithCovarianceStamped_ Type; + + TwistWithCovarianceStamped_() + : header() + , twist() { + } + TwistWithCovarianceStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , twist(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::TwistWithCovariance_ _twist_type; + _twist_type twist; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovarianceStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovarianceStamped_ const> ConstPtr; + +}; // struct TwistWithCovarianceStamped_ + +typedef ::geometry_msgs::TwistWithCovarianceStamped_ > TwistWithCovarianceStamped; + +typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovarianceStamped > TwistWithCovarianceStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovarianceStamped const> TwistWithCovarianceStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::TwistWithCovarianceStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::TwistWithCovarianceStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::TwistWithCovarianceStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TwistWithCovarianceStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistWithCovarianceStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistWithCovarianceStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistWithCovarianceStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistWithCovarianceStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::TwistWithCovarianceStamped_ > +{ + static const char* value() + { + return "8927a1a12fb2607ceea095b2dc440a96"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovarianceStamped_&) { return value(); } + static const uint64_t static_value1 = 0x8927a1a12fb2607cULL; + static const uint64_t static_value2 = 0xeea095b2dc440a96ULL; +}; + +template +struct DataType< ::geometry_msgs::TwistWithCovarianceStamped_ > +{ + static const char* value() + { + return "geometry_msgs/TwistWithCovarianceStamped"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovarianceStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::TwistWithCovarianceStamped_ > +{ + static const char* value() + { + return "# This represents an estimated twist with reference coordinate frame and timestamp.\n\ +Header header\n\ +TwistWithCovariance twist\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/TwistWithCovariance\n\ +# This expresses velocity in free space with uncertainty.\n\ +\n\ +Twist twist\n\ +\n\ +# Row-major representation of the 6x6 covariance matrix\n\ +# The orientation parameters use a fixed-axis representation.\n\ +# In order, the parameters are:\n\ +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n\ +float64[36] covariance\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Twist\n\ +# This expresses velocity in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovarianceStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::TwistWithCovarianceStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.twist); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TwistWithCovarianceStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::TwistWithCovarianceStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::TwistWithCovarianceStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "twist: "; + s << std::endl; + Printer< ::geometry_msgs::TwistWithCovariance_ >::stream(s, indent + " ", v.twist); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCESTAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Vector3.h b/thirdparty/ros/include/geometry_msgs/Vector3.h new file mode 100644 index 0000000..7dbb6e9 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Vector3.h @@ -0,0 +1,212 @@ +// Generated by gencpp from file geometry_msgs/Vector3.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_VECTOR3_H +#define GEOMETRY_MSGS_MESSAGE_VECTOR3_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Vector3_ +{ + typedef Vector3_ Type; + + Vector3_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Vector3_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Vector3_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Vector3_ const> ConstPtr; + +}; // struct Vector3_ + +typedef ::geometry_msgs::Vector3_ > Vector3; + +typedef boost::shared_ptr< ::geometry_msgs::Vector3 > Vector3Ptr; +typedef boost::shared_ptr< ::geometry_msgs::Vector3 const> Vector3ConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Vector3_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Vector3_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Vector3_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Vector3_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Vector3_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Vector3_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Vector3_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Vector3_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Vector3_ > +{ + static const char* value() + { + return "4a842b65f413084dc2b10fb484ea7f17"; + } + + static const char* value(const ::geometry_msgs::Vector3_&) { return value(); } + static const uint64_t static_value1 = 0x4a842b65f413084dULL; + static const uint64_t static_value2 = 0xc2b10fb484ea7f17ULL; +}; + +template +struct DataType< ::geometry_msgs::Vector3_ > +{ + static const char* value() + { + return "geometry_msgs/Vector3"; + } + + static const char* value(const ::geometry_msgs::Vector3_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Vector3_ > +{ + static const char* value() + { + return "# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Vector3_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Vector3_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Vector3_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Vector3_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Vector3_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_VECTOR3_H diff --git a/thirdparty/ros/include/geometry_msgs/Vector3Stamped.h b/thirdparty/ros/include/geometry_msgs/Vector3Stamped.h new file mode 100644 index 0000000..4502761 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Vector3Stamped.h @@ -0,0 +1,232 @@ +// Generated by gencpp from file geometry_msgs/Vector3Stamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_VECTOR3STAMPED_H +#define GEOMETRY_MSGS_MESSAGE_VECTOR3STAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Vector3Stamped_ +{ + typedef Vector3Stamped_ Type; + + Vector3Stamped_() + : header() + , vector() { + } + Vector3Stamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , vector(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Vector3_ _vector_type; + _vector_type vector; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Vector3Stamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Vector3Stamped_ const> ConstPtr; + +}; // struct Vector3Stamped_ + +typedef ::geometry_msgs::Vector3Stamped_ > Vector3Stamped; + +typedef boost::shared_ptr< ::geometry_msgs::Vector3Stamped > Vector3StampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::Vector3Stamped const> Vector3StampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Vector3Stamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Vector3Stamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Vector3Stamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Vector3Stamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::Vector3Stamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Vector3Stamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Vector3Stamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Vector3Stamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Vector3Stamped_ > +{ + static const char* value() + { + return "7b324c7325e683bf02a9b14b01090ec7"; + } + + static const char* value(const ::geometry_msgs::Vector3Stamped_&) { return value(); } + static const uint64_t static_value1 = 0x7b324c7325e683bfULL; + static const uint64_t static_value2 = 0x02a9b14b01090ec7ULL; +}; + +template +struct DataType< ::geometry_msgs::Vector3Stamped_ > +{ + static const char* value() + { + return "geometry_msgs/Vector3Stamped"; + } + + static const char* value(const ::geometry_msgs::Vector3Stamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Vector3Stamped_ > +{ + static const char* value() + { + return "# This represents a Vector3 with reference coordinate frame and timestamp\n\ +Header header\n\ +Vector3 vector\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Vector3Stamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Vector3Stamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.vector); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Vector3Stamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Vector3Stamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Vector3Stamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "vector: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.vector); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_VECTOR3STAMPED_H diff --git a/thirdparty/ros/include/geometry_msgs/Wrench.h b/thirdparty/ros/include/geometry_msgs/Wrench.h new file mode 100644 index 0000000..499b647 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/Wrench.h @@ -0,0 +1,215 @@ +// Generated by gencpp from file geometry_msgs/Wrench.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_WRENCH_H +#define GEOMETRY_MSGS_MESSAGE_WRENCH_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Wrench_ +{ + typedef Wrench_ Type; + + Wrench_() + : force() + , torque() { + } + Wrench_(const ContainerAllocator& _alloc) + : force(_alloc) + , torque(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Vector3_ _force_type; + _force_type force; + + typedef ::geometry_msgs::Vector3_ _torque_type; + _torque_type torque; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Wrench_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Wrench_ const> ConstPtr; + +}; // struct Wrench_ + +typedef ::geometry_msgs::Wrench_ > Wrench; + +typedef boost::shared_ptr< ::geometry_msgs::Wrench > WrenchPtr; +typedef boost::shared_ptr< ::geometry_msgs::Wrench const> WrenchConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Wrench_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Wrench_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::Wrench_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Wrench_ const> + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Wrench_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Wrench_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Wrench_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Wrench_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Wrench_ > +{ + static const char* value() + { + return "4f539cf138b23283b520fd271b567936"; + } + + static const char* value(const ::geometry_msgs::Wrench_&) { return value(); } + static const uint64_t static_value1 = 0x4f539cf138b23283ULL; + static const uint64_t static_value2 = 0xb520fd271b567936ULL; +}; + +template +struct DataType< ::geometry_msgs::Wrench_ > +{ + static const char* value() + { + return "geometry_msgs/Wrench"; + } + + static const char* value(const ::geometry_msgs::Wrench_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Wrench_ > +{ + static const char* value() + { + return "# This represents force in free space, separated into\n\ +# its linear and angular parts.\n\ +Vector3 force\n\ +Vector3 torque\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::Wrench_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Wrench_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.force); + stream.next(m.torque); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Wrench_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Wrench_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Wrench_& v) + { + s << indent << "force: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.force); + s << indent << "torque: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.torque); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_WRENCH_H diff --git a/thirdparty/ros/include/geometry_msgs/WrenchStamped.h b/thirdparty/ros/include/geometry_msgs/WrenchStamped.h new file mode 100644 index 0000000..a07dc97 --- /dev/null +++ b/thirdparty/ros/include/geometry_msgs/WrenchStamped.h @@ -0,0 +1,239 @@ +// Generated by gencpp from file geometry_msgs/WrenchStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_WRENCHSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_WRENCHSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct WrenchStamped_ +{ + typedef WrenchStamped_ Type; + + WrenchStamped_() + : header() + , wrench() { + } + WrenchStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , wrench(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Wrench_ _wrench_type; + _wrench_type wrench; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::WrenchStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::WrenchStamped_ const> ConstPtr; + +}; // struct WrenchStamped_ + +typedef ::geometry_msgs::WrenchStamped_ > WrenchStamped; + +typedef boost::shared_ptr< ::geometry_msgs::WrenchStamped > WrenchStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::WrenchStamped const> WrenchStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::WrenchStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::WrenchStamped_ >::stream(s, "", v); +return s; +} + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/tmp/binarydeb/ros-kinetic-geometry-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::geometry_msgs::WrenchStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::WrenchStamped_ const> + : FalseType + { }; + +template +struct IsMessage< ::geometry_msgs::WrenchStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::WrenchStamped_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::WrenchStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::WrenchStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::WrenchStamped_ > +{ + static const char* value() + { + return "d78d3cb249ce23087ade7e7d0c40cfa7"; + } + + static const char* value(const ::geometry_msgs::WrenchStamped_&) { return value(); } + static const uint64_t static_value1 = 0xd78d3cb249ce2308ULL; + static const uint64_t static_value2 = 0x7ade7e7d0c40cfa7ULL; +}; + +template +struct DataType< ::geometry_msgs::WrenchStamped_ > +{ + static const char* value() + { + return "geometry_msgs/WrenchStamped"; + } + + static const char* value(const ::geometry_msgs::WrenchStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::WrenchStamped_ > +{ + static const char* value() + { + return "# A wrench with reference coordinate frame and timestamp\n\ +Header header\n\ +Wrench wrench\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Wrench\n\ +# This represents force in free space, separated into\n\ +# its linear and angular parts.\n\ +Vector3 force\n\ +Vector3 torque\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::geometry_msgs::WrenchStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::WrenchStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.wrench); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct WrenchStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::WrenchStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::WrenchStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "wrench: "; + s << std::endl; + Printer< ::geometry_msgs::Wrench_ >::stream(s, indent + " ", v.wrench); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_WRENCHSTAMPED_H diff --git a/thirdparty/ros/include/sensor_msgs/BatteryState.h b/thirdparty/ros/include/sensor_msgs/BatteryState.h new file mode 100644 index 0000000..9e8a83c --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/BatteryState.h @@ -0,0 +1,428 @@ +// Generated by gencpp from file sensor_msgs/BatteryState.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_BATTERYSTATE_H +#define SENSOR_MSGS_MESSAGE_BATTERYSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct BatteryState_ +{ + typedef BatteryState_ Type; + + BatteryState_() + : header() + , voltage(0.0) + , current(0.0) + , charge(0.0) + , capacity(0.0) + , design_capacity(0.0) + , percentage(0.0) + , power_supply_status(0) + , power_supply_health(0) + , power_supply_technology(0) + , present(false) + , cell_voltage() + , location() + , serial_number() { + } + BatteryState_(const ContainerAllocator& _alloc) + : header(_alloc) + , voltage(0.0) + , current(0.0) + , charge(0.0) + , capacity(0.0) + , design_capacity(0.0) + , percentage(0.0) + , power_supply_status(0) + , power_supply_health(0) + , power_supply_technology(0) + , present(false) + , cell_voltage(_alloc) + , location(_alloc) + , serial_number(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef float _voltage_type; + _voltage_type voltage; + + typedef float _current_type; + _current_type current; + + typedef float _charge_type; + _charge_type charge; + + typedef float _capacity_type; + _capacity_type capacity; + + typedef float _design_capacity_type; + _design_capacity_type design_capacity; + + typedef float _percentage_type; + _percentage_type percentage; + + typedef uint8_t _power_supply_status_type; + _power_supply_status_type power_supply_status; + + typedef uint8_t _power_supply_health_type; + _power_supply_health_type power_supply_health; + + typedef uint8_t _power_supply_technology_type; + _power_supply_technology_type power_supply_technology; + + typedef uint8_t _present_type; + _present_type present; + + typedef std::vector::other > _cell_voltage_type; + _cell_voltage_type cell_voltage; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _location_type; + _location_type location; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _serial_number_type; + _serial_number_type serial_number; + + + + enum { + POWER_SUPPLY_STATUS_UNKNOWN = 0u, + POWER_SUPPLY_STATUS_CHARGING = 1u, + POWER_SUPPLY_STATUS_DISCHARGING = 2u, + POWER_SUPPLY_STATUS_NOT_CHARGING = 3u, + POWER_SUPPLY_STATUS_FULL = 4u, + POWER_SUPPLY_HEALTH_UNKNOWN = 0u, + POWER_SUPPLY_HEALTH_GOOD = 1u, + POWER_SUPPLY_HEALTH_OVERHEAT = 2u, + POWER_SUPPLY_HEALTH_DEAD = 3u, + POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4u, + POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5u, + POWER_SUPPLY_HEALTH_COLD = 6u, + POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7u, + POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8u, + POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0u, + POWER_SUPPLY_TECHNOLOGY_NIMH = 1u, + POWER_SUPPLY_TECHNOLOGY_LION = 2u, + POWER_SUPPLY_TECHNOLOGY_LIPO = 3u, + POWER_SUPPLY_TECHNOLOGY_LIFE = 4u, + POWER_SUPPLY_TECHNOLOGY_NICD = 5u, + POWER_SUPPLY_TECHNOLOGY_LIMN = 6u, + }; + + + typedef boost::shared_ptr< ::sensor_msgs::BatteryState_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::BatteryState_ const> ConstPtr; + +}; // struct BatteryState_ + +typedef ::sensor_msgs::BatteryState_ > BatteryState; + +typedef boost::shared_ptr< ::sensor_msgs::BatteryState > BatteryStatePtr; +typedef boost::shared_ptr< ::sensor_msgs::BatteryState const> BatteryStateConstPtr; + +// constants requiring out of line definition + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::BatteryState_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::BatteryState_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::BatteryState_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::BatteryState_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::BatteryState_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::BatteryState_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::BatteryState_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::BatteryState_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::BatteryState_ > +{ + static const char* value() + { + return "476f837fa6771f6e16e3bf4ef96f8770"; + } + + static const char* value(const ::sensor_msgs::BatteryState_&) { return value(); } + static const uint64_t static_value1 = 0x476f837fa6771f6eULL; + static const uint64_t static_value2 = 0x16e3bf4ef96f8770ULL; +}; + +template +struct DataType< ::sensor_msgs::BatteryState_ > +{ + static const char* value() + { + return "sensor_msgs/BatteryState"; + } + + static const char* value(const ::sensor_msgs::BatteryState_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::BatteryState_ > +{ + static const char* value() + { + return "\n\ +# Constants are chosen to match the enums in the linux kernel\n\ +# defined in include/linux/power_supply.h as of version 3.7\n\ +# The one difference is for style reasons the constants are\n\ +# all uppercase not mixed case.\n\ +\n\ +# Power supply status constants\n\ +uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0\n\ +uint8 POWER_SUPPLY_STATUS_CHARGING = 1\n\ +uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2\n\ +uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3\n\ +uint8 POWER_SUPPLY_STATUS_FULL = 4\n\ +\n\ +# Power supply health constants\n\ +uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0\n\ +uint8 POWER_SUPPLY_HEALTH_GOOD = 1\n\ +uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2\n\ +uint8 POWER_SUPPLY_HEALTH_DEAD = 3\n\ +uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4\n\ +uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5\n\ +uint8 POWER_SUPPLY_HEALTH_COLD = 6\n\ +uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7\n\ +uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8\n\ +\n\ +# Power supply technology (chemistry) constants\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5\n\ +uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6\n\ +\n\ +Header header\n\ +float32 voltage # Voltage in Volts (Mandatory)\n\ +float32 current # Negative when discharging (A) (If unmeasured NaN)\n\ +float32 charge # Current charge in Ah (If unmeasured NaN)\n\ +float32 capacity # Capacity in Ah (last full capacity) (If unmeasured NaN)\n\ +float32 design_capacity # Capacity in Ah (design capacity) (If unmeasured NaN)\n\ +float32 percentage # Charge percentage on 0 to 1 range (If unmeasured NaN)\n\ +uint8 power_supply_status # The charging status as reported. Values defined above\n\ +uint8 power_supply_health # The battery health metric. Values defined above\n\ +uint8 power_supply_technology # The battery chemistry. Values defined above\n\ +bool present # True if the battery is present\n\ +\n\ +float32[] cell_voltage # An array of individual cell voltages for each cell in the pack\n\ + # If individual voltages unknown but number of cells known set each to NaN\n\ +string location # The location into which the battery is inserted. (slot number or plug)\n\ +string serial_number # The best approximation of the battery serial number\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::BatteryState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::BatteryState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.voltage); + stream.next(m.current); + stream.next(m.charge); + stream.next(m.capacity); + stream.next(m.design_capacity); + stream.next(m.percentage); + stream.next(m.power_supply_status); + stream.next(m.power_supply_health); + stream.next(m.power_supply_technology); + stream.next(m.present); + stream.next(m.cell_voltage); + stream.next(m.location); + stream.next(m.serial_number); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct BatteryState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::BatteryState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::BatteryState_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "voltage: "; + Printer::stream(s, indent + " ", v.voltage); + s << indent << "current: "; + Printer::stream(s, indent + " ", v.current); + s << indent << "charge: "; + Printer::stream(s, indent + " ", v.charge); + s << indent << "capacity: "; + Printer::stream(s, indent + " ", v.capacity); + s << indent << "design_capacity: "; + Printer::stream(s, indent + " ", v.design_capacity); + s << indent << "percentage: "; + Printer::stream(s, indent + " ", v.percentage); + s << indent << "power_supply_status: "; + Printer::stream(s, indent + " ", v.power_supply_status); + s << indent << "power_supply_health: "; + Printer::stream(s, indent + " ", v.power_supply_health); + s << indent << "power_supply_technology: "; + Printer::stream(s, indent + " ", v.power_supply_technology); + s << indent << "present: "; + Printer::stream(s, indent + " ", v.present); + s << indent << "cell_voltage[]" << std::endl; + for (size_t i = 0; i < v.cell_voltage.size(); ++i) + { + s << indent << " cell_voltage[" << i << "]: "; + Printer::stream(s, indent + " ", v.cell_voltage[i]); + } + s << indent << "location: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.location); + s << indent << "serial_number: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.serial_number); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_BATTERYSTATE_H diff --git a/thirdparty/ros/include/sensor_msgs/CameraInfo.h b/thirdparty/ros/include/sensor_msgs/CameraInfo.h new file mode 100644 index 0000000..7ac7278 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/CameraInfo.h @@ -0,0 +1,467 @@ +// Generated by gencpp from file sensor_msgs/CameraInfo.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_CAMERAINFO_H +#define SENSOR_MSGS_MESSAGE_CAMERAINFO_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace sensor_msgs +{ +template +struct CameraInfo_ +{ + typedef CameraInfo_ Type; + + CameraInfo_() + : header() + , height(0) + , width(0) + , distortion_model() + , D() + , K() + , R() + , P() + , binning_x(0) + , binning_y(0) + , roi() { + K.assign(0.0); + + R.assign(0.0); + + P.assign(0.0); + } + CameraInfo_(const ContainerAllocator& _alloc) + : header(_alloc) + , height(0) + , width(0) + , distortion_model(_alloc) + , D(_alloc) + , K() + , R() + , P() + , binning_x(0) + , binning_y(0) + , roi(_alloc) { + (void)_alloc; + K.assign(0.0); + + R.assign(0.0); + + P.assign(0.0); + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint32_t _height_type; + _height_type height; + + typedef uint32_t _width_type; + _width_type width; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _distortion_model_type; + _distortion_model_type distortion_model; + + typedef std::vector::other > _D_type; + _D_type D; + + typedef boost::array _K_type; + _K_type K; + + typedef boost::array _R_type; + _R_type R; + + typedef boost::array _P_type; + _P_type P; + + typedef uint32_t _binning_x_type; + _binning_x_type binning_x; + + typedef uint32_t _binning_y_type; + _binning_y_type binning_y; + + typedef ::sensor_msgs::RegionOfInterest_ _roi_type; + _roi_type roi; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::CameraInfo_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::CameraInfo_ const> ConstPtr; + +}; // struct CameraInfo_ + +typedef ::sensor_msgs::CameraInfo_ > CameraInfo; + +typedef boost::shared_ptr< ::sensor_msgs::CameraInfo > CameraInfoPtr; +typedef boost::shared_ptr< ::sensor_msgs::CameraInfo const> CameraInfoConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::CameraInfo_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::CameraInfo_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::CameraInfo_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::CameraInfo_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::CameraInfo_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::CameraInfo_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::CameraInfo_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::CameraInfo_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::CameraInfo_ > +{ + static const char* value() + { + return "c9a58c1b0b154e0e6da7578cb991d214"; + } + + static const char* value(const ::sensor_msgs::CameraInfo_&) { return value(); } + static const uint64_t static_value1 = 0xc9a58c1b0b154e0eULL; + static const uint64_t static_value2 = 0x6da7578cb991d214ULL; +}; + +template +struct DataType< ::sensor_msgs::CameraInfo_ > +{ + static const char* value() + { + return "sensor_msgs/CameraInfo"; + } + + static const char* value(const ::sensor_msgs::CameraInfo_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::CameraInfo_ > +{ + static const char* value() + { + return "# This message defines meta information for a camera. It should be in a\n\ +# camera namespace on topic \"camera_info\" and accompanied by up to five\n\ +# image topics named:\n\ +#\n\ +# image_raw - raw data from the camera driver, possibly Bayer encoded\n\ +# image - monochrome, distorted\n\ +# image_color - color, distorted\n\ +# image_rect - monochrome, rectified\n\ +# image_rect_color - color, rectified\n\ +#\n\ +# The image_pipeline contains packages (image_proc, stereo_image_proc)\n\ +# for producing the four processed image topics from image_raw and\n\ +# camera_info. The meaning of the camera parameters are described in\n\ +# detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.\n\ +#\n\ +# The image_geometry package provides a user-friendly interface to\n\ +# common operations using this meta information. If you want to, e.g.,\n\ +# project a 3d point into image coordinates, we strongly recommend\n\ +# using image_geometry.\n\ +#\n\ +# If the camera is uncalibrated, the matrices D, K, R, P should be left\n\ +# zeroed out. In particular, clients may assume that K[0] == 0.0\n\ +# indicates an uncalibrated camera.\n\ +\n\ +#######################################################################\n\ +# Image acquisition info #\n\ +#######################################################################\n\ +\n\ +# Time of image acquisition, camera coordinate frame ID\n\ +Header header # Header timestamp should be acquisition time of image\n\ + # Header frame_id should be optical frame of camera\n\ + # origin of frame should be optical center of camera\n\ + # +x should point to the right in the image\n\ + # +y should point down in the image\n\ + # +z should point into the plane of the image\n\ +\n\ +\n\ +#######################################################################\n\ +# Calibration Parameters #\n\ +#######################################################################\n\ +# These are fixed during camera calibration. Their values will be the #\n\ +# same in all messages until the camera is recalibrated. Note that #\n\ +# self-calibrating systems may \"recalibrate\" frequently. #\n\ +# #\n\ +# The internal parameters can be used to warp a raw (distorted) image #\n\ +# to: #\n\ +# 1. An undistorted image (requires D and K) #\n\ +# 2. A rectified image (requires D, K, R) #\n\ +# The projection matrix P projects 3D points into the rectified image.#\n\ +#######################################################################\n\ +\n\ +# The image dimensions with which the camera was calibrated. Normally\n\ +# this will be the full camera resolution in pixels.\n\ +uint32 height\n\ +uint32 width\n\ +\n\ +# The distortion model used. Supported models are listed in\n\ +# sensor_msgs/distortion_models.h. For most cameras, \"plumb_bob\" - a\n\ +# simple model of radial and tangential distortion - is sufficient.\n\ +string distortion_model\n\ +\n\ +# The distortion parameters, size depending on the distortion model.\n\ +# For \"plumb_bob\", the 5 parameters are: (k1, k2, t1, t2, k3).\n\ +float64[] D\n\ +\n\ +# Intrinsic camera matrix for the raw (distorted) images.\n\ +# [fx 0 cx]\n\ +# K = [ 0 fy cy]\n\ +# [ 0 0 1]\n\ +# Projects 3D points in the camera coordinate frame to 2D pixel\n\ +# coordinates using the focal lengths (fx, fy) and principal point\n\ +# (cx, cy).\n\ +float64[9] K # 3x3 row-major matrix\n\ +\n\ +# Rectification matrix (stereo cameras only)\n\ +# A rotation matrix aligning the camera coordinate system to the ideal\n\ +# stereo image plane so that epipolar lines in both stereo images are\n\ +# parallel.\n\ +float64[9] R # 3x3 row-major matrix\n\ +\n\ +# Projection/camera matrix\n\ +# [fx' 0 cx' Tx]\n\ +# P = [ 0 fy' cy' Ty]\n\ +# [ 0 0 1 0]\n\ +# By convention, this matrix specifies the intrinsic (camera) matrix\n\ +# of the processed (rectified) image. That is, the left 3x3 portion\n\ +# is the normal camera intrinsic matrix for the rectified image.\n\ +# It projects 3D points in the camera coordinate frame to 2D pixel\n\ +# coordinates using the focal lengths (fx', fy') and principal point\n\ +# (cx', cy') - these may differ from the values in K.\n\ +# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will\n\ +# also have R = the identity and P[1:3,1:3] = K.\n\ +# For a stereo pair, the fourth column [Tx Ty 0]' is related to the\n\ +# position of the optical center of the second camera in the first\n\ +# camera's frame. We assume Tz = 0 so both cameras are in the same\n\ +# stereo image plane. The first camera always has Tx = Ty = 0. For\n\ +# the right (second) camera of a horizontal stereo pair, Ty = 0 and\n\ +# Tx = -fx' * B, where B is the baseline between the cameras.\n\ +# Given a 3D point [X Y Z]', the projection (x, y) of the point onto\n\ +# the rectified image is given by:\n\ +# [u v w]' = P * [X Y Z 1]'\n\ +# x = u / w\n\ +# y = v / w\n\ +# This holds for both images of a stereo pair.\n\ +float64[12] P # 3x4 row-major matrix\n\ +\n\ +\n\ +#######################################################################\n\ +# Operational Parameters #\n\ +#######################################################################\n\ +# These define the image region actually captured by the camera #\n\ +# driver. Although they affect the geometry of the output image, they #\n\ +# may be changed freely without recalibrating the camera. #\n\ +#######################################################################\n\ +\n\ +# Binning refers here to any camera setting which combines rectangular\n\ +# neighborhoods of pixels into larger \"super-pixels.\" It reduces the\n\ +# resolution of the output image to\n\ +# (width / binning_x) x (height / binning_y).\n\ +# The default values binning_x = binning_y = 0 is considered the same\n\ +# as binning_x = binning_y = 1 (no subsampling).\n\ +uint32 binning_x\n\ +uint32 binning_y\n\ +\n\ +# Region of interest (subwindow of full camera resolution), given in\n\ +# full resolution (unbinned) image coordinates. A particular ROI\n\ +# always denotes the same window of pixels on the camera sensor,\n\ +# regardless of binning settings.\n\ +# The default setting of roi (all values 0) is considered the same as\n\ +# full resolution (roi.width = width, roi.height = height).\n\ +RegionOfInterest roi\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/RegionOfInterest\n\ +# This message is used to specify a region of interest within an image.\n\ +#\n\ +# When used to specify the ROI setting of the camera when the image was\n\ +# taken, the height and width fields should either match the height and\n\ +# width fields for the associated image; or height = width = 0\n\ +# indicates that the full resolution image was captured.\n\ +\n\ +uint32 x_offset # Leftmost pixel of the ROI\n\ + # (0 if the ROI includes the left edge of the image)\n\ +uint32 y_offset # Topmost pixel of the ROI\n\ + # (0 if the ROI includes the top edge of the image)\n\ +uint32 height # Height of ROI\n\ +uint32 width # Width of ROI\n\ +\n\ +# True if a distinct rectified ROI should be calculated from the \"raw\"\n\ +# ROI in this message. Typically this should be False if the full image\n\ +# is captured (ROI not used), and True if a subwindow is captured (ROI\n\ +# used).\n\ +bool do_rectify\n\ +"; + } + + static const char* value(const ::sensor_msgs::CameraInfo_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::CameraInfo_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.height); + stream.next(m.width); + stream.next(m.distortion_model); + stream.next(m.D); + stream.next(m.K); + stream.next(m.R); + stream.next(m.P); + stream.next(m.binning_x); + stream.next(m.binning_y); + stream.next(m.roi); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct CameraInfo_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::CameraInfo_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::CameraInfo_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "height: "; + Printer::stream(s, indent + " ", v.height); + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "distortion_model: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.distortion_model); + s << indent << "D[]" << std::endl; + for (size_t i = 0; i < v.D.size(); ++i) + { + s << indent << " D[" << i << "]: "; + Printer::stream(s, indent + " ", v.D[i]); + } + s << indent << "K[]" << std::endl; + for (size_t i = 0; i < v.K.size(); ++i) + { + s << indent << " K[" << i << "]: "; + Printer::stream(s, indent + " ", v.K[i]); + } + s << indent << "R[]" << std::endl; + for (size_t i = 0; i < v.R.size(); ++i) + { + s << indent << " R[" << i << "]: "; + Printer::stream(s, indent + " ", v.R[i]); + } + s << indent << "P[]" << std::endl; + for (size_t i = 0; i < v.P.size(); ++i) + { + s << indent << " P[" << i << "]: "; + Printer::stream(s, indent + " ", v.P[i]); + } + s << indent << "binning_x: "; + Printer::stream(s, indent + " ", v.binning_x); + s << indent << "binning_y: "; + Printer::stream(s, indent + " ", v.binning_y); + s << indent << "roi: "; + s << std::endl; + Printer< ::sensor_msgs::RegionOfInterest_ >::stream(s, indent + " ", v.roi); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_CAMERAINFO_H diff --git a/thirdparty/ros/include/sensor_msgs/ChannelFloat32.h b/thirdparty/ros/include/sensor_msgs/ChannelFloat32.h new file mode 100644 index 0000000..d599c7a --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/ChannelFloat32.h @@ -0,0 +1,222 @@ +// Generated by gencpp from file sensor_msgs/ChannelFloat32.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H +#define SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct ChannelFloat32_ +{ + typedef ChannelFloat32_ Type; + + ChannelFloat32_() + : name() + , values() { + } + ChannelFloat32_(const ContainerAllocator& _alloc) + : name(_alloc) + , values(_alloc) { + (void)_alloc; + } + + + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _name_type; + _name_type name; + + typedef std::vector::other > _values_type; + _values_type values; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::ChannelFloat32_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::ChannelFloat32_ const> ConstPtr; + +}; // struct ChannelFloat32_ + +typedef ::sensor_msgs::ChannelFloat32_ > ChannelFloat32; + +typedef boost::shared_ptr< ::sensor_msgs::ChannelFloat32 > ChannelFloat32Ptr; +typedef boost::shared_ptr< ::sensor_msgs::ChannelFloat32 const> ChannelFloat32ConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::ChannelFloat32_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::ChannelFloat32_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::ChannelFloat32_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::ChannelFloat32_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::ChannelFloat32_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::ChannelFloat32_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::ChannelFloat32_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::ChannelFloat32_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::ChannelFloat32_ > +{ + static const char* value() + { + return "3d40139cdd33dfedcb71ffeeeb42ae7f"; + } + + static const char* value(const ::sensor_msgs::ChannelFloat32_&) { return value(); } + static const uint64_t static_value1 = 0x3d40139cdd33dfedULL; + static const uint64_t static_value2 = 0xcb71ffeeeb42ae7fULL; +}; + +template +struct DataType< ::sensor_msgs::ChannelFloat32_ > +{ + static const char* value() + { + return "sensor_msgs/ChannelFloat32"; + } + + static const char* value(const ::sensor_msgs::ChannelFloat32_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::ChannelFloat32_ > +{ + static const char* value() + { + return "# This message is used by the PointCloud message to hold optional data\n\ +# associated with each point in the cloud. The length of the values\n\ +# array should be the same as the length of the points array in the\n\ +# PointCloud, and each value should be associated with the corresponding\n\ +# point.\n\ +\n\ +# Channel names in existing practice include:\n\ +# \"u\", \"v\" - row and column (respectively) in the left stereo image.\n\ +# This is opposite to usual conventions but remains for\n\ +# historical reasons. The newer PointCloud2 message has no\n\ +# such problem.\n\ +# \"rgb\" - For point clouds produced by color stereo cameras. uint8\n\ +# (R,G,B) values packed into the least significant 24 bits,\n\ +# in order.\n\ +# \"intensity\" - laser or pixel intensity.\n\ +# \"distance\"\n\ +\n\ +# The channel name should give semantics of the channel (e.g.\n\ +# \"intensity\" instead of \"value\").\n\ +string name\n\ +\n\ +# The values array should be 1-1 with the elements of the associated\n\ +# PointCloud.\n\ +float32[] values\n\ +"; + } + + static const char* value(const ::sensor_msgs::ChannelFloat32_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::ChannelFloat32_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.name); + stream.next(m.values); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct ChannelFloat32_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::ChannelFloat32_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::ChannelFloat32_& v) + { + s << indent << "name: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.name); + s << indent << "values[]" << std::endl; + for (size_t i = 0; i < v.values.size(); ++i) + { + s << indent << " values[" << i << "]: "; + Printer::stream(s, indent + " ", v.values[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H diff --git a/thirdparty/ros/include/sensor_msgs/CompressedImage.h b/thirdparty/ros/include/sensor_msgs/CompressedImage.h new file mode 100644 index 0000000..ffed790 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/CompressedImage.h @@ -0,0 +1,239 @@ +// Generated by gencpp from file sensor_msgs/CompressedImage.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_COMPRESSEDIMAGE_H +#define SENSOR_MSGS_MESSAGE_COMPRESSEDIMAGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct CompressedImage_ +{ + typedef CompressedImage_ Type; + + CompressedImage_() + : header() + , format() + , data() { + } + CompressedImage_(const ContainerAllocator& _alloc) + : header(_alloc) + , format(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _format_type; + _format_type format; + + typedef std::vector::other > _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::CompressedImage_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::CompressedImage_ const> ConstPtr; + +}; // struct CompressedImage_ + +typedef ::sensor_msgs::CompressedImage_ > CompressedImage; + +typedef boost::shared_ptr< ::sensor_msgs::CompressedImage > CompressedImagePtr; +typedef boost::shared_ptr< ::sensor_msgs::CompressedImage const> CompressedImageConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::CompressedImage_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::CompressedImage_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::CompressedImage_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::CompressedImage_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::CompressedImage_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::CompressedImage_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::CompressedImage_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::CompressedImage_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::CompressedImage_ > +{ + static const char* value() + { + return "8f7a12909da2c9d3332d540a0977563f"; + } + + static const char* value(const ::sensor_msgs::CompressedImage_&) { return value(); } + static const uint64_t static_value1 = 0x8f7a12909da2c9d3ULL; + static const uint64_t static_value2 = 0x332d540a0977563fULL; +}; + +template +struct DataType< ::sensor_msgs::CompressedImage_ > +{ + static const char* value() + { + return "sensor_msgs/CompressedImage"; + } + + static const char* value(const ::sensor_msgs::CompressedImage_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::CompressedImage_ > +{ + static const char* value() + { + return "# This message contains a compressed image\n\ +\n\ +Header header # Header timestamp should be acquisition time of image\n\ + # Header frame_id should be optical frame of camera\n\ + # origin of frame should be optical center of cameara\n\ + # +x should point to the right in the image\n\ + # +y should point down in the image\n\ + # +z should point into to plane of the image\n\ +\n\ +string format # Specifies the format of the data\n\ + # Acceptable values:\n\ + # jpeg, png\n\ +uint8[] data # Compressed image buffer\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::CompressedImage_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::CompressedImage_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.format); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct CompressedImage_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::CompressedImage_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::CompressedImage_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "format: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.format); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_COMPRESSEDIMAGE_H diff --git a/thirdparty/ros/include/sensor_msgs/FluidPressure.h b/thirdparty/ros/include/sensor_msgs/FluidPressure.h new file mode 100644 index 0000000..98cec3f --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/FluidPressure.h @@ -0,0 +1,233 @@ +// Generated by gencpp from file sensor_msgs/FluidPressure.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_FLUIDPRESSURE_H +#define SENSOR_MSGS_MESSAGE_FLUIDPRESSURE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct FluidPressure_ +{ + typedef FluidPressure_ Type; + + FluidPressure_() + : header() + , fluid_pressure(0.0) + , variance(0.0) { + } + FluidPressure_(const ContainerAllocator& _alloc) + : header(_alloc) + , fluid_pressure(0.0) + , variance(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef double _fluid_pressure_type; + _fluid_pressure_type fluid_pressure; + + typedef double _variance_type; + _variance_type variance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::FluidPressure_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::FluidPressure_ const> ConstPtr; + +}; // struct FluidPressure_ + +typedef ::sensor_msgs::FluidPressure_ > FluidPressure; + +typedef boost::shared_ptr< ::sensor_msgs::FluidPressure > FluidPressurePtr; +typedef boost::shared_ptr< ::sensor_msgs::FluidPressure const> FluidPressureConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::FluidPressure_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::FluidPressure_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::FluidPressure_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::FluidPressure_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::FluidPressure_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::FluidPressure_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::FluidPressure_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::FluidPressure_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::FluidPressure_ > +{ + static const char* value() + { + return "804dc5cea1c5306d6a2eb80b9833befe"; + } + + static const char* value(const ::sensor_msgs::FluidPressure_&) { return value(); } + static const uint64_t static_value1 = 0x804dc5cea1c5306dULL; + static const uint64_t static_value2 = 0x6a2eb80b9833befeULL; +}; + +template +struct DataType< ::sensor_msgs::FluidPressure_ > +{ + static const char* value() + { + return "sensor_msgs/FluidPressure"; + } + + static const char* value(const ::sensor_msgs::FluidPressure_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::FluidPressure_ > +{ + static const char* value() + { + return " # Single pressure reading. This message is appropriate for measuring the\n\ + # pressure inside of a fluid (air, water, etc). This also includes\n\ + # atmospheric or barometric pressure.\n\ +\n\ + # This message is not appropriate for force/pressure contact sensors.\n\ +\n\ + Header header # timestamp of the measurement\n\ + # frame_id is the location of the pressure sensor\n\ +\n\ + float64 fluid_pressure # Absolute pressure reading in Pascals.\n\ +\n\ + float64 variance # 0 is interpreted as variance unknown\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::FluidPressure_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::FluidPressure_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.fluid_pressure); + stream.next(m.variance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct FluidPressure_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::FluidPressure_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::FluidPressure_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "fluid_pressure: "; + Printer::stream(s, indent + " ", v.fluid_pressure); + s << indent << "variance: "; + Printer::stream(s, indent + " ", v.variance); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_FLUIDPRESSURE_H diff --git a/thirdparty/ros/include/sensor_msgs/Illuminance.h b/thirdparty/ros/include/sensor_msgs/Illuminance.h new file mode 100644 index 0000000..ad98737 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/Illuminance.h @@ -0,0 +1,242 @@ +// Generated by gencpp from file sensor_msgs/Illuminance.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_ILLUMINANCE_H +#define SENSOR_MSGS_MESSAGE_ILLUMINANCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Illuminance_ +{ + typedef Illuminance_ Type; + + Illuminance_() + : header() + , illuminance(0.0) + , variance(0.0) { + } + Illuminance_(const ContainerAllocator& _alloc) + : header(_alloc) + , illuminance(0.0) + , variance(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef double _illuminance_type; + _illuminance_type illuminance; + + typedef double _variance_type; + _variance_type variance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Illuminance_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Illuminance_ const> ConstPtr; + +}; // struct Illuminance_ + +typedef ::sensor_msgs::Illuminance_ > Illuminance; + +typedef boost::shared_ptr< ::sensor_msgs::Illuminance > IlluminancePtr; +typedef boost::shared_ptr< ::sensor_msgs::Illuminance const> IlluminanceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Illuminance_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Illuminance_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::Illuminance_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Illuminance_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::Illuminance_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Illuminance_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Illuminance_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Illuminance_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Illuminance_ > +{ + static const char* value() + { + return "8cf5febb0952fca9d650c3d11a81a188"; + } + + static const char* value(const ::sensor_msgs::Illuminance_&) { return value(); } + static const uint64_t static_value1 = 0x8cf5febb0952fca9ULL; + static const uint64_t static_value2 = 0xd650c3d11a81a188ULL; +}; + +template +struct DataType< ::sensor_msgs::Illuminance_ > +{ + static const char* value() + { + return "sensor_msgs/Illuminance"; + } + + static const char* value(const ::sensor_msgs::Illuminance_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Illuminance_ > +{ + static const char* value() + { + return " # Single photometric illuminance measurement. Light should be assumed to be\n\ + # measured along the sensor's x-axis (the area of detection is the y-z plane).\n\ + # The illuminance should have a 0 or positive value and be received with\n\ + # the sensor's +X axis pointing toward the light source.\n\ +\n\ + # Photometric illuminance is the measure of the human eye's sensitivity of the\n\ + # intensity of light encountering or passing through a surface.\n\ +\n\ + # All other Photometric and Radiometric measurements should\n\ + # not use this message.\n\ + # This message cannot represent:\n\ + # Luminous intensity (candela/light source output)\n\ + # Luminance (nits/light output per area)\n\ + # Irradiance (watt/area), etc.\n\ +\n\ + Header header # timestamp is the time the illuminance was measured\n\ + # frame_id is the location and direction of the reading\n\ +\n\ + float64 illuminance # Measurement of the Photometric Illuminance in Lux.\n\ +\n\ + float64 variance # 0 is interpreted as variance unknown\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::Illuminance_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Illuminance_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.illuminance); + stream.next(m.variance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Illuminance_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Illuminance_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Illuminance_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "illuminance: "; + Printer::stream(s, indent + " ", v.illuminance); + s << indent << "variance: "; + Printer::stream(s, indent + " ", v.variance); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_ILLUMINANCE_H diff --git a/thirdparty/ros/include/sensor_msgs/Image.h b/thirdparty/ros/include/sensor_msgs/Image.h new file mode 100644 index 0000000..e6f17df --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/Image.h @@ -0,0 +1,285 @@ +// Generated by gencpp from file sensor_msgs/Image.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_IMAGE_H +#define SENSOR_MSGS_MESSAGE_IMAGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Image_ +{ + typedef Image_ Type; + + Image_() + : header() + , height(0) + , width(0) + , encoding() + , is_bigendian(0) + , step(0) + , data() { + } + Image_(const ContainerAllocator& _alloc) + : header(_alloc) + , height(0) + , width(0) + , encoding(_alloc) + , is_bigendian(0) + , step(0) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint32_t _height_type; + _height_type height; + + typedef uint32_t _width_type; + _width_type width; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _encoding_type; + _encoding_type encoding; + + typedef uint8_t _is_bigendian_type; + _is_bigendian_type is_bigendian; + + typedef uint32_t _step_type; + _step_type step; + + typedef std::vector::other > _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Image_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Image_ const> ConstPtr; + +}; // struct Image_ + +typedef ::sensor_msgs::Image_ > Image; + +typedef boost::shared_ptr< ::sensor_msgs::Image > ImagePtr; +typedef boost::shared_ptr< ::sensor_msgs::Image const> ImageConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Image_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Image_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::Image_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Image_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::Image_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Image_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Image_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Image_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Image_ > +{ + static const char* value() + { + return "060021388200f6f0f447d0fcd9c64743"; + } + + static const char* value(const ::sensor_msgs::Image_&) { return value(); } + static const uint64_t static_value1 = 0x060021388200f6f0ULL; + static const uint64_t static_value2 = 0xf447d0fcd9c64743ULL; +}; + +template +struct DataType< ::sensor_msgs::Image_ > +{ + static const char* value() + { + return "sensor_msgs/Image"; + } + + static const char* value(const ::sensor_msgs::Image_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Image_ > +{ + static const char* value() + { + return "# This message contains an uncompressed image\n\ +# (0, 0) is at top-left corner of image\n\ +#\n\ +\n\ +Header header # Header timestamp should be acquisition time of image\n\ + # Header frame_id should be optical frame of camera\n\ + # origin of frame should be optical center of cameara\n\ + # +x should point to the right in the image\n\ + # +y should point down in the image\n\ + # +z should point into to plane of the image\n\ + # If the frame_id here and the frame_id of the CameraInfo\n\ + # message associated with the image conflict\n\ + # the behavior is undefined\n\ +\n\ +uint32 height # image height, that is, number of rows\n\ +uint32 width # image width, that is, number of columns\n\ +\n\ +# The legal values for encoding are in file src/image_encodings.cpp\n\ +# If you want to standardize a new string format, join\n\ +# ros-users@lists.sourceforge.net and send an email proposing a new encoding.\n\ +\n\ +string encoding # Encoding of pixels -- channel meaning, ordering, size\n\ + # taken from the list of strings in include/sensor_msgs/image_encodings.h\n\ +\n\ +uint8 is_bigendian # is this data bigendian?\n\ +uint32 step # Full row length in bytes\n\ +uint8[] data # actual matrix data, size is (step * rows)\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::Image_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Image_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.height); + stream.next(m.width); + stream.next(m.encoding); + stream.next(m.is_bigendian); + stream.next(m.step); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Image_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Image_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Image_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "height: "; + Printer::stream(s, indent + " ", v.height); + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "encoding: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.encoding); + s << indent << "is_bigendian: "; + Printer::stream(s, indent + " ", v.is_bigendian); + s << indent << "step: "; + Printer::stream(s, indent + " ", v.step); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_IMAGE_H diff --git a/thirdparty/ros/include/sensor_msgs/Imu.h b/thirdparty/ros/include/sensor_msgs/Imu.h new file mode 100644 index 0000000..013043c --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/Imu.h @@ -0,0 +1,328 @@ +// Generated by gencpp from file sensor_msgs/Imu.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_IMU_H +#define SENSOR_MSGS_MESSAGE_IMU_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace sensor_msgs +{ +template +struct Imu_ +{ + typedef Imu_ Type; + + Imu_() + : header() + , orientation() + , orientation_covariance() + , angular_velocity() + , angular_velocity_covariance() + , linear_acceleration() + , linear_acceleration_covariance() { + orientation_covariance.assign(0.0); + + angular_velocity_covariance.assign(0.0); + + linear_acceleration_covariance.assign(0.0); + } + Imu_(const ContainerAllocator& _alloc) + : header(_alloc) + , orientation(_alloc) + , orientation_covariance() + , angular_velocity(_alloc) + , angular_velocity_covariance() + , linear_acceleration(_alloc) + , linear_acceleration_covariance() { + (void)_alloc; + orientation_covariance.assign(0.0); + + angular_velocity_covariance.assign(0.0); + + linear_acceleration_covariance.assign(0.0); + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Quaternion_ _orientation_type; + _orientation_type orientation; + + typedef boost::array _orientation_covariance_type; + _orientation_covariance_type orientation_covariance; + + typedef ::geometry_msgs::Vector3_ _angular_velocity_type; + _angular_velocity_type angular_velocity; + + typedef boost::array _angular_velocity_covariance_type; + _angular_velocity_covariance_type angular_velocity_covariance; + + typedef ::geometry_msgs::Vector3_ _linear_acceleration_type; + _linear_acceleration_type linear_acceleration; + + typedef boost::array _linear_acceleration_covariance_type; + _linear_acceleration_covariance_type linear_acceleration_covariance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Imu_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Imu_ const> ConstPtr; + +}; // struct Imu_ + +typedef ::sensor_msgs::Imu_ > Imu; + +typedef boost::shared_ptr< ::sensor_msgs::Imu > ImuPtr; +typedef boost::shared_ptr< ::sensor_msgs::Imu const> ImuConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Imu_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Imu_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::Imu_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Imu_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::Imu_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Imu_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Imu_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Imu_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Imu_ > +{ + static const char* value() + { + return "6a62c6daae103f4ff57a132d6f95cec2"; + } + + static const char* value(const ::sensor_msgs::Imu_&) { return value(); } + static const uint64_t static_value1 = 0x6a62c6daae103f4fULL; + static const uint64_t static_value2 = 0xf57a132d6f95cec2ULL; +}; + +template +struct DataType< ::sensor_msgs::Imu_ > +{ + static const char* value() + { + return "sensor_msgs/Imu"; + } + + static const char* value(const ::sensor_msgs::Imu_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Imu_ > +{ + static const char* value() + { + return "# This is a message to hold data from an IMU (Inertial Measurement Unit)\n\ +#\n\ +# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec\n\ +#\n\ +# If the covariance of the measurement is known, it should be filled in (if all you know is the \n\ +# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)\n\ +# A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the\n\ +# data a covariance will have to be assumed or gotten from some other source\n\ +#\n\ +# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation \n\ +# estimate), please set element 0 of the associated covariance matrix to -1\n\ +# If you are interpreting this message, please check for a value of -1 in the first element of each \n\ +# covariance matrix, and disregard the associated estimate.\n\ +\n\ +Header header\n\ +\n\ +geometry_msgs/Quaternion orientation\n\ +float64[9] orientation_covariance # Row major about x, y, z axes\n\ +\n\ +geometry_msgs/Vector3 angular_velocity\n\ +float64[9] angular_velocity_covariance # Row major about x, y, z axes\n\ +\n\ +geometry_msgs/Vector3 linear_acceleration\n\ +float64[9] linear_acceleration_covariance # Row major x, y z \n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::sensor_msgs::Imu_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Imu_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.orientation); + stream.next(m.orientation_covariance); + stream.next(m.angular_velocity); + stream.next(m.angular_velocity_covariance); + stream.next(m.linear_acceleration); + stream.next(m.linear_acceleration_covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Imu_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Imu_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Imu_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "orientation: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.orientation); + s << indent << "orientation_covariance[]" << std::endl; + for (size_t i = 0; i < v.orientation_covariance.size(); ++i) + { + s << indent << " orientation_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.orientation_covariance[i]); + } + s << indent << "angular_velocity: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.angular_velocity); + s << indent << "angular_velocity_covariance[]" << std::endl; + for (size_t i = 0; i < v.angular_velocity_covariance.size(); ++i) + { + s << indent << " angular_velocity_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.angular_velocity_covariance[i]); + } + s << indent << "linear_acceleration: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.linear_acceleration); + s << indent << "linear_acceleration_covariance[]" << std::endl; + for (size_t i = 0; i < v.linear_acceleration_covariance.size(); ++i) + { + s << indent << " linear_acceleration_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.linear_acceleration_covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_IMU_H diff --git a/thirdparty/ros/include/sensor_msgs/JointState.h b/thirdparty/ros/include/sensor_msgs/JointState.h new file mode 100644 index 0000000..5aef117 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/JointState.h @@ -0,0 +1,280 @@ +// Generated by gencpp from file sensor_msgs/JointState.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_JOINTSTATE_H +#define SENSOR_MSGS_MESSAGE_JOINTSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct JointState_ +{ + typedef JointState_ Type; + + JointState_() + : header() + , name() + , position() + , velocity() + , effort() { + } + JointState_(const ContainerAllocator& _alloc) + : header(_alloc) + , name(_alloc) + , position(_alloc) + , velocity(_alloc) + , effort(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector, typename ContainerAllocator::template rebind::other > , typename ContainerAllocator::template rebind, typename ContainerAllocator::template rebind::other > >::other > _name_type; + _name_type name; + + typedef std::vector::other > _position_type; + _position_type position; + + typedef std::vector::other > _velocity_type; + _velocity_type velocity; + + typedef std::vector::other > _effort_type; + _effort_type effort; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::JointState_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::JointState_ const> ConstPtr; + +}; // struct JointState_ + +typedef ::sensor_msgs::JointState_ > JointState; + +typedef boost::shared_ptr< ::sensor_msgs::JointState > JointStatePtr; +typedef boost::shared_ptr< ::sensor_msgs::JointState const> JointStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::JointState_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::JointState_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::JointState_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::JointState_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::JointState_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::JointState_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::JointState_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::JointState_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::JointState_ > +{ + static const char* value() + { + return "3066dcd76a6cfaef579bd0f34173e9fd"; + } + + static const char* value(const ::sensor_msgs::JointState_&) { return value(); } + static const uint64_t static_value1 = 0x3066dcd76a6cfaefULL; + static const uint64_t static_value2 = 0x579bd0f34173e9fdULL; +}; + +template +struct DataType< ::sensor_msgs::JointState_ > +{ + static const char* value() + { + return "sensor_msgs/JointState"; + } + + static const char* value(const ::sensor_msgs::JointState_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::JointState_ > +{ + static const char* value() + { + return "# This is a message that holds data to describe the state of a set of torque controlled joints. \n\ +#\n\ +# The state of each joint (revolute or prismatic) is defined by:\n\ +# * the position of the joint (rad or m),\n\ +# * the velocity of the joint (rad/s or m/s) and \n\ +# * the effort that is applied in the joint (Nm or N).\n\ +#\n\ +# Each joint is uniquely identified by its name\n\ +# The header specifies the time at which the joint states were recorded. All the joint states\n\ +# in one message have to be recorded at the same time.\n\ +#\n\ +# This message consists of a multiple arrays, one for each part of the joint state. \n\ +# The goal is to make each of the fields optional. When e.g. your joints have no\n\ +# effort associated with them, you can leave the effort array empty. \n\ +#\n\ +# All arrays in this message should have the same size, or be empty.\n\ +# This is the only way to uniquely associate the joint name with the correct\n\ +# states.\n\ +\n\ +\n\ +Header header\n\ +\n\ +string[] name\n\ +float64[] position\n\ +float64[] velocity\n\ +float64[] effort\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::JointState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::JointState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.name); + stream.next(m.position); + stream.next(m.velocity); + stream.next(m.effort); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct JointState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::JointState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::JointState_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "name[]" << std::endl; + for (size_t i = 0; i < v.name.size(); ++i) + { + s << indent << " name[" << i << "]: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.name[i]); + } + s << indent << "position[]" << std::endl; + for (size_t i = 0; i < v.position.size(); ++i) + { + s << indent << " position[" << i << "]: "; + Printer::stream(s, indent + " ", v.position[i]); + } + s << indent << "velocity[]" << std::endl; + for (size_t i = 0; i < v.velocity.size(); ++i) + { + s << indent << " velocity[" << i << "]: "; + Printer::stream(s, indent + " ", v.velocity[i]); + } + s << indent << "effort[]" << std::endl; + for (size_t i = 0; i < v.effort.size(); ++i) + { + s << indent << " effort[" << i << "]: "; + Printer::stream(s, indent + " ", v.effort[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_JOINTSTATE_H diff --git a/thirdparty/ros/include/sensor_msgs/Joy.h b/thirdparty/ros/include/sensor_msgs/Joy.h new file mode 100644 index 0000000..78e2754 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/Joy.h @@ -0,0 +1,234 @@ +// Generated by gencpp from file sensor_msgs/Joy.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_JOY_H +#define SENSOR_MSGS_MESSAGE_JOY_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Joy_ +{ + typedef Joy_ Type; + + Joy_() + : header() + , axes() + , buttons() { + } + Joy_(const ContainerAllocator& _alloc) + : header(_alloc) + , axes(_alloc) + , buttons(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector::other > _axes_type; + _axes_type axes; + + typedef std::vector::other > _buttons_type; + _buttons_type buttons; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Joy_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Joy_ const> ConstPtr; + +}; // struct Joy_ + +typedef ::sensor_msgs::Joy_ > Joy; + +typedef boost::shared_ptr< ::sensor_msgs::Joy > JoyPtr; +typedef boost::shared_ptr< ::sensor_msgs::Joy const> JoyConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Joy_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Joy_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::Joy_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Joy_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::Joy_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Joy_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Joy_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Joy_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Joy_ > +{ + static const char* value() + { + return "5a9ea5f83505693b71e785041e67a8bb"; + } + + static const char* value(const ::sensor_msgs::Joy_&) { return value(); } + static const uint64_t static_value1 = 0x5a9ea5f83505693bULL; + static const uint64_t static_value2 = 0x71e785041e67a8bbULL; +}; + +template +struct DataType< ::sensor_msgs::Joy_ > +{ + static const char* value() + { + return "sensor_msgs/Joy"; + } + + static const char* value(const ::sensor_msgs::Joy_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Joy_ > +{ + static const char* value() + { + return "# Reports the state of a joysticks axes and buttons.\n\ +Header header # timestamp in the header is the time the data is received from the joystick\n\ +float32[] axes # the axes measurements from a joystick\n\ +int32[] buttons # the buttons measurements from a joystick \n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::Joy_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Joy_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.axes); + stream.next(m.buttons); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Joy_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Joy_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Joy_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "axes[]" << std::endl; + for (size_t i = 0; i < v.axes.size(); ++i) + { + s << indent << " axes[" << i << "]: "; + Printer::stream(s, indent + " ", v.axes[i]); + } + s << indent << "buttons[]" << std::endl; + for (size_t i = 0; i < v.buttons.size(); ++i) + { + s << indent << " buttons[" << i << "]: "; + Printer::stream(s, indent + " ", v.buttons[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_JOY_H diff --git a/thirdparty/ros/include/sensor_msgs/JoyFeedback.h b/thirdparty/ros/include/sensor_msgs/JoyFeedback.h new file mode 100644 index 0000000..fa5df07 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/JoyFeedback.h @@ -0,0 +1,228 @@ +// Generated by gencpp from file sensor_msgs/JoyFeedback.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_JOYFEEDBACK_H +#define SENSOR_MSGS_MESSAGE_JOYFEEDBACK_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct JoyFeedback_ +{ + typedef JoyFeedback_ Type; + + JoyFeedback_() + : type(0) + , id(0) + , intensity(0.0) { + } + JoyFeedback_(const ContainerAllocator& _alloc) + : type(0) + , id(0) + , intensity(0.0) { + (void)_alloc; + } + + + + typedef uint8_t _type_type; + _type_type type; + + typedef uint8_t _id_type; + _id_type id; + + typedef float _intensity_type; + _intensity_type intensity; + + + + enum { + TYPE_LED = 0u, + TYPE_RUMBLE = 1u, + TYPE_BUZZER = 2u, + }; + + + typedef boost::shared_ptr< ::sensor_msgs::JoyFeedback_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::JoyFeedback_ const> ConstPtr; + +}; // struct JoyFeedback_ + +typedef ::sensor_msgs::JoyFeedback_ > JoyFeedback; + +typedef boost::shared_ptr< ::sensor_msgs::JoyFeedback > JoyFeedbackPtr; +typedef boost::shared_ptr< ::sensor_msgs::JoyFeedback const> JoyFeedbackConstPtr; + +// constants requiring out of line definition + + + + + + + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::JoyFeedback_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::JoyFeedback_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::JoyFeedback_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sensor_msgs::JoyFeedback_ const> + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::JoyFeedback_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::JoyFeedback_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::JoyFeedback_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::JoyFeedback_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::JoyFeedback_ > +{ + static const char* value() + { + return "f4dcd73460360d98f36e55ee7f2e46f1"; + } + + static const char* value(const ::sensor_msgs::JoyFeedback_&) { return value(); } + static const uint64_t static_value1 = 0xf4dcd73460360d98ULL; + static const uint64_t static_value2 = 0xf36e55ee7f2e46f1ULL; +}; + +template +struct DataType< ::sensor_msgs::JoyFeedback_ > +{ + static const char* value() + { + return "sensor_msgs/JoyFeedback"; + } + + static const char* value(const ::sensor_msgs::JoyFeedback_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::JoyFeedback_ > +{ + static const char* value() + { + return "# Declare of the type of feedback\n\ +uint8 TYPE_LED = 0\n\ +uint8 TYPE_RUMBLE = 1\n\ +uint8 TYPE_BUZZER = 2\n\ +\n\ +uint8 type\n\ +\n\ +# This will hold an id number for each type of each feedback.\n\ +# Example, the first led would be id=0, the second would be id=1\n\ +uint8 id\n\ +\n\ +# Intensity of the feedback, from 0.0 to 1.0, inclusive. If device is\n\ +# actually binary, driver should treat 0<=x<0.5 as off, 0.5<=x<=1 as on.\n\ +float32 intensity\n\ +\n\ +"; + } + + static const char* value(const ::sensor_msgs::JoyFeedback_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::JoyFeedback_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.type); + stream.next(m.id); + stream.next(m.intensity); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct JoyFeedback_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::JoyFeedback_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::JoyFeedback_& v) + { + s << indent << "type: "; + Printer::stream(s, indent + " ", v.type); + s << indent << "id: "; + Printer::stream(s, indent + " ", v.id); + s << indent << "intensity: "; + Printer::stream(s, indent + " ", v.intensity); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_JOYFEEDBACK_H diff --git a/thirdparty/ros/include/sensor_msgs/JoyFeedbackArray.h b/thirdparty/ros/include/sensor_msgs/JoyFeedbackArray.h new file mode 100644 index 0000000..5debaf4 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/JoyFeedbackArray.h @@ -0,0 +1,212 @@ +// Generated by gencpp from file sensor_msgs/JoyFeedbackArray.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_JOYFEEDBACKARRAY_H +#define SENSOR_MSGS_MESSAGE_JOYFEEDBACKARRAY_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct JoyFeedbackArray_ +{ + typedef JoyFeedbackArray_ Type; + + JoyFeedbackArray_() + : array() { + } + JoyFeedbackArray_(const ContainerAllocator& _alloc) + : array(_alloc) { + (void)_alloc; + } + + + + typedef std::vector< ::sensor_msgs::JoyFeedback_ , typename ContainerAllocator::template rebind< ::sensor_msgs::JoyFeedback_ >::other > _array_type; + _array_type array; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::JoyFeedbackArray_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::JoyFeedbackArray_ const> ConstPtr; + +}; // struct JoyFeedbackArray_ + +typedef ::sensor_msgs::JoyFeedbackArray_ > JoyFeedbackArray; + +typedef boost::shared_ptr< ::sensor_msgs::JoyFeedbackArray > JoyFeedbackArrayPtr; +typedef boost::shared_ptr< ::sensor_msgs::JoyFeedbackArray const> JoyFeedbackArrayConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::JoyFeedbackArray_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::JoyFeedbackArray_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::JoyFeedbackArray_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::JoyFeedbackArray_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::JoyFeedbackArray_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::JoyFeedbackArray_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::JoyFeedbackArray_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::JoyFeedbackArray_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::JoyFeedbackArray_ > +{ + static const char* value() + { + return "cde5730a895b1fc4dee6f91b754b213d"; + } + + static const char* value(const ::sensor_msgs::JoyFeedbackArray_&) { return value(); } + static const uint64_t static_value1 = 0xcde5730a895b1fc4ULL; + static const uint64_t static_value2 = 0xdee6f91b754b213dULL; +}; + +template +struct DataType< ::sensor_msgs::JoyFeedbackArray_ > +{ + static const char* value() + { + return "sensor_msgs/JoyFeedbackArray"; + } + + static const char* value(const ::sensor_msgs::JoyFeedbackArray_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::JoyFeedbackArray_ > +{ + static const char* value() + { + return "# This message publishes values for multiple feedback at once. \n\ +JoyFeedback[] array\n\ +================================================================================\n\ +MSG: sensor_msgs/JoyFeedback\n\ +# Declare of the type of feedback\n\ +uint8 TYPE_LED = 0\n\ +uint8 TYPE_RUMBLE = 1\n\ +uint8 TYPE_BUZZER = 2\n\ +\n\ +uint8 type\n\ +\n\ +# This will hold an id number for each type of each feedback.\n\ +# Example, the first led would be id=0, the second would be id=1\n\ +uint8 id\n\ +\n\ +# Intensity of the feedback, from 0.0 to 1.0, inclusive. If device is\n\ +# actually binary, driver should treat 0<=x<0.5 as off, 0.5<=x<=1 as on.\n\ +float32 intensity\n\ +\n\ +"; + } + + static const char* value(const ::sensor_msgs::JoyFeedbackArray_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::JoyFeedbackArray_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.array); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct JoyFeedbackArray_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::JoyFeedbackArray_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::JoyFeedbackArray_& v) + { + s << indent << "array[]" << std::endl; + for (size_t i = 0; i < v.array.size(); ++i) + { + s << indent << " array[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::sensor_msgs::JoyFeedback_ >::stream(s, indent + " ", v.array[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_JOYFEEDBACKARRAY_H diff --git a/thirdparty/ros/include/sensor_msgs/LaserEcho.h b/thirdparty/ros/include/sensor_msgs/LaserEcho.h new file mode 100644 index 0000000..fb5dfdb --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/LaserEcho.h @@ -0,0 +1,195 @@ +// Generated by gencpp from file sensor_msgs/LaserEcho.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_LASERECHO_H +#define SENSOR_MSGS_MESSAGE_LASERECHO_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct LaserEcho_ +{ + typedef LaserEcho_ Type; + + LaserEcho_() + : echoes() { + } + LaserEcho_(const ContainerAllocator& _alloc) + : echoes(_alloc) { + (void)_alloc; + } + + + + typedef std::vector::other > _echoes_type; + _echoes_type echoes; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::LaserEcho_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::LaserEcho_ const> ConstPtr; + +}; // struct LaserEcho_ + +typedef ::sensor_msgs::LaserEcho_ > LaserEcho; + +typedef boost::shared_ptr< ::sensor_msgs::LaserEcho > LaserEchoPtr; +typedef boost::shared_ptr< ::sensor_msgs::LaserEcho const> LaserEchoConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::LaserEcho_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::LaserEcho_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::LaserEcho_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::LaserEcho_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::LaserEcho_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::LaserEcho_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::LaserEcho_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::LaserEcho_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::LaserEcho_ > +{ + static const char* value() + { + return "8bc5ae449b200fba4d552b4225586696"; + } + + static const char* value(const ::sensor_msgs::LaserEcho_&) { return value(); } + static const uint64_t static_value1 = 0x8bc5ae449b200fbaULL; + static const uint64_t static_value2 = 0x4d552b4225586696ULL; +}; + +template +struct DataType< ::sensor_msgs::LaserEcho_ > +{ + static const char* value() + { + return "sensor_msgs/LaserEcho"; + } + + static const char* value(const ::sensor_msgs::LaserEcho_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::LaserEcho_ > +{ + static const char* value() + { + return "# This message is a submessage of MultiEchoLaserScan and is not intended\n\ +# to be used separately.\n\ +\n\ +float32[] echoes # Multiple values of ranges or intensities.\n\ + # Each array represents data from the same angle increment.\n\ +"; + } + + static const char* value(const ::sensor_msgs::LaserEcho_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::LaserEcho_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.echoes); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LaserEcho_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::LaserEcho_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::LaserEcho_& v) + { + s << indent << "echoes[]" << std::endl; + for (size_t i = 0; i < v.echoes.size(); ++i) + { + s << indent << " echoes[" << i << "]: "; + Printer::stream(s, indent + " ", v.echoes[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_LASERECHO_H diff --git a/thirdparty/ros/include/sensor_msgs/LaserScan.h b/thirdparty/ros/include/sensor_msgs/LaserScan.h new file mode 100644 index 0000000..ca9dd5d --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/LaserScan.h @@ -0,0 +1,315 @@ +// Generated by gencpp from file sensor_msgs/LaserScan.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_LASERSCAN_H +#define SENSOR_MSGS_MESSAGE_LASERSCAN_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct LaserScan_ +{ + typedef LaserScan_ Type; + + LaserScan_() + : header() + , angle_min(0.0) + , angle_max(0.0) + , angle_increment(0.0) + , time_increment(0.0) + , scan_time(0.0) + , range_min(0.0) + , range_max(0.0) + , ranges() + , intensities() { + } + LaserScan_(const ContainerAllocator& _alloc) + : header(_alloc) + , angle_min(0.0) + , angle_max(0.0) + , angle_increment(0.0) + , time_increment(0.0) + , scan_time(0.0) + , range_min(0.0) + , range_max(0.0) + , ranges(_alloc) + , intensities(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef float _angle_min_type; + _angle_min_type angle_min; + + typedef float _angle_max_type; + _angle_max_type angle_max; + + typedef float _angle_increment_type; + _angle_increment_type angle_increment; + + typedef float _time_increment_type; + _time_increment_type time_increment; + + typedef float _scan_time_type; + _scan_time_type scan_time; + + typedef float _range_min_type; + _range_min_type range_min; + + typedef float _range_max_type; + _range_max_type range_max; + + typedef std::vector::other > _ranges_type; + _ranges_type ranges; + + typedef std::vector::other > _intensities_type; + _intensities_type intensities; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::LaserScan_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::LaserScan_ const> ConstPtr; + +}; // struct LaserScan_ + +typedef ::sensor_msgs::LaserScan_ > LaserScan; + +typedef boost::shared_ptr< ::sensor_msgs::LaserScan > LaserScanPtr; +typedef boost::shared_ptr< ::sensor_msgs::LaserScan const> LaserScanConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::LaserScan_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::LaserScan_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::LaserScan_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::LaserScan_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::LaserScan_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::LaserScan_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::LaserScan_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::LaserScan_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::LaserScan_ > +{ + static const char* value() + { + return "90c7ef2dc6895d81024acba2ac42f369"; + } + + static const char* value(const ::sensor_msgs::LaserScan_&) { return value(); } + static const uint64_t static_value1 = 0x90c7ef2dc6895d81ULL; + static const uint64_t static_value2 = 0x024acba2ac42f369ULL; +}; + +template +struct DataType< ::sensor_msgs::LaserScan_ > +{ + static const char* value() + { + return "sensor_msgs/LaserScan"; + } + + static const char* value(const ::sensor_msgs::LaserScan_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::LaserScan_ > +{ + static const char* value() + { + return "# Single scan from a planar laser range-finder\n\ +#\n\ +# If you have another ranging device with different behavior (e.g. a sonar\n\ +# array), please find or create a different message, since applications\n\ +# will make fairly laser-specific assumptions about this data\n\ +\n\ +Header header # timestamp in the header is the acquisition time of \n\ + # the first ray in the scan.\n\ + #\n\ + # in frame frame_id, angles are measured around \n\ + # the positive Z axis (counterclockwise, if Z is up)\n\ + # with zero angle being forward along the x axis\n\ + \n\ +float32 angle_min # start angle of the scan [rad]\n\ +float32 angle_max # end angle of the scan [rad]\n\ +float32 angle_increment # angular distance between measurements [rad]\n\ +\n\ +float32 time_increment # time between measurements [seconds] - if your scanner\n\ + # is moving, this will be used in interpolating position\n\ + # of 3d points\n\ +float32 scan_time # time between scans [seconds]\n\ +\n\ +float32 range_min # minimum range value [m]\n\ +float32 range_max # maximum range value [m]\n\ +\n\ +float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)\n\ +float32[] intensities # intensity data [device-specific units]. If your\n\ + # device does not provide intensities, please leave\n\ + # the array empty.\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::LaserScan_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::LaserScan_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.angle_min); + stream.next(m.angle_max); + stream.next(m.angle_increment); + stream.next(m.time_increment); + stream.next(m.scan_time); + stream.next(m.range_min); + stream.next(m.range_max); + stream.next(m.ranges); + stream.next(m.intensities); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LaserScan_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::LaserScan_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::LaserScan_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "angle_min: "; + Printer::stream(s, indent + " ", v.angle_min); + s << indent << "angle_max: "; + Printer::stream(s, indent + " ", v.angle_max); + s << indent << "angle_increment: "; + Printer::stream(s, indent + " ", v.angle_increment); + s << indent << "time_increment: "; + Printer::stream(s, indent + " ", v.time_increment); + s << indent << "scan_time: "; + Printer::stream(s, indent + " ", v.scan_time); + s << indent << "range_min: "; + Printer::stream(s, indent + " ", v.range_min); + s << indent << "range_max: "; + Printer::stream(s, indent + " ", v.range_max); + s << indent << "ranges[]" << std::endl; + for (size_t i = 0; i < v.ranges.size(); ++i) + { + s << indent << " ranges[" << i << "]: "; + Printer::stream(s, indent + " ", v.ranges[i]); + } + s << indent << "intensities[]" << std::endl; + for (size_t i = 0; i < v.intensities.size(); ++i) + { + s << indent << " intensities[" << i << "]: "; + Printer::stream(s, indent + " ", v.intensities[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_LASERSCAN_H diff --git a/thirdparty/ros/include/sensor_msgs/MagneticField.h b/thirdparty/ros/include/sensor_msgs/MagneticField.h new file mode 100644 index 0000000..a1b1106 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/MagneticField.h @@ -0,0 +1,264 @@ +// Generated by gencpp from file sensor_msgs/MagneticField.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_MAGNETICFIELD_H +#define SENSOR_MSGS_MESSAGE_MAGNETICFIELD_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace sensor_msgs +{ +template +struct MagneticField_ +{ + typedef MagneticField_ Type; + + MagneticField_() + : header() + , magnetic_field() + , magnetic_field_covariance() { + magnetic_field_covariance.assign(0.0); + } + MagneticField_(const ContainerAllocator& _alloc) + : header(_alloc) + , magnetic_field(_alloc) + , magnetic_field_covariance() { + (void)_alloc; + magnetic_field_covariance.assign(0.0); + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Vector3_ _magnetic_field_type; + _magnetic_field_type magnetic_field; + + typedef boost::array _magnetic_field_covariance_type; + _magnetic_field_covariance_type magnetic_field_covariance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::MagneticField_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::MagneticField_ const> ConstPtr; + +}; // struct MagneticField_ + +typedef ::sensor_msgs::MagneticField_ > MagneticField; + +typedef boost::shared_ptr< ::sensor_msgs::MagneticField > MagneticFieldPtr; +typedef boost::shared_ptr< ::sensor_msgs::MagneticField const> MagneticFieldConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::MagneticField_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::MagneticField_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::MagneticField_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::MagneticField_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::MagneticField_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::MagneticField_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::MagneticField_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::MagneticField_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::MagneticField_ > +{ + static const char* value() + { + return "2f3b0b43eed0c9501de0fa3ff89a45aa"; + } + + static const char* value(const ::sensor_msgs::MagneticField_&) { return value(); } + static const uint64_t static_value1 = 0x2f3b0b43eed0c950ULL; + static const uint64_t static_value2 = 0x1de0fa3ff89a45aaULL; +}; + +template +struct DataType< ::sensor_msgs::MagneticField_ > +{ + static const char* value() + { + return "sensor_msgs/MagneticField"; + } + + static const char* value(const ::sensor_msgs::MagneticField_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::MagneticField_ > +{ + static const char* value() + { + return " # Measurement of the Magnetic Field vector at a specific location.\n\ +\n\ + # If the covariance of the measurement is known, it should be filled in\n\ + # (if all you know is the variance of each measurement, e.g. from the datasheet,\n\ + #just put those along the diagonal)\n\ + # A covariance matrix of all zeros will be interpreted as \"covariance unknown\",\n\ + # and to use the data a covariance will have to be assumed or gotten from some\n\ + # other source\n\ +\n\ +\n\ + Header header # timestamp is the time the\n\ + # field was measured\n\ + # frame_id is the location and orientation\n\ + # of the field measurement\n\ +\n\ + geometry_msgs/Vector3 magnetic_field # x, y, and z components of the\n\ + # field vector in Tesla\n\ + # If your sensor does not output 3 axes,\n\ + # put NaNs in the components not reported.\n\ +\n\ + float64[9] magnetic_field_covariance # Row major about x, y, z axes\n\ + # 0 is interpreted as variance unknown\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +"; + } + + static const char* value(const ::sensor_msgs::MagneticField_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::MagneticField_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.magnetic_field); + stream.next(m.magnetic_field_covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MagneticField_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::MagneticField_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::MagneticField_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "magnetic_field: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.magnetic_field); + s << indent << "magnetic_field_covariance[]" << std::endl; + for (size_t i = 0; i < v.magnetic_field_covariance.size(); ++i) + { + s << indent << " magnetic_field_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.magnetic_field_covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_MAGNETICFIELD_H diff --git a/thirdparty/ros/include/sensor_msgs/MultiDOFJointState.h b/thirdparty/ros/include/sensor_msgs/MultiDOFJointState.h new file mode 100644 index 0000000..7bec3f7 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/MultiDOFJointState.h @@ -0,0 +1,330 @@ +// Generated by gencpp from file sensor_msgs/MultiDOFJointState.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_MULTIDOFJOINTSTATE_H +#define SENSOR_MSGS_MESSAGE_MULTIDOFJOINTSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace sensor_msgs +{ +template +struct MultiDOFJointState_ +{ + typedef MultiDOFJointState_ Type; + + MultiDOFJointState_() + : header() + , joint_names() + , transforms() + , twist() + , wrench() { + } + MultiDOFJointState_(const ContainerAllocator& _alloc) + : header(_alloc) + , joint_names(_alloc) + , transforms(_alloc) + , twist(_alloc) + , wrench(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector, typename ContainerAllocator::template rebind::other > , typename ContainerAllocator::template rebind, typename ContainerAllocator::template rebind::other > >::other > _joint_names_type; + _joint_names_type joint_names; + + typedef std::vector< ::geometry_msgs::Transform_ , typename ContainerAllocator::template rebind< ::geometry_msgs::Transform_ >::other > _transforms_type; + _transforms_type transforms; + + typedef std::vector< ::geometry_msgs::Twist_ , typename ContainerAllocator::template rebind< ::geometry_msgs::Twist_ >::other > _twist_type; + _twist_type twist; + + typedef std::vector< ::geometry_msgs::Wrench_ , typename ContainerAllocator::template rebind< ::geometry_msgs::Wrench_ >::other > _wrench_type; + _wrench_type wrench; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::MultiDOFJointState_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::MultiDOFJointState_ const> ConstPtr; + +}; // struct MultiDOFJointState_ + +typedef ::sensor_msgs::MultiDOFJointState_ > MultiDOFJointState; + +typedef boost::shared_ptr< ::sensor_msgs::MultiDOFJointState > MultiDOFJointStatePtr; +typedef boost::shared_ptr< ::sensor_msgs::MultiDOFJointState const> MultiDOFJointStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::MultiDOFJointState_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::MultiDOFJointState_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::MultiDOFJointState_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::MultiDOFJointState_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::MultiDOFJointState_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::MultiDOFJointState_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::MultiDOFJointState_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::MultiDOFJointState_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::MultiDOFJointState_ > +{ + static const char* value() + { + return "690f272f0640d2631c305eeb8301e59d"; + } + + static const char* value(const ::sensor_msgs::MultiDOFJointState_&) { return value(); } + static const uint64_t static_value1 = 0x690f272f0640d263ULL; + static const uint64_t static_value2 = 0x1c305eeb8301e59dULL; +}; + +template +struct DataType< ::sensor_msgs::MultiDOFJointState_ > +{ + static const char* value() + { + return "sensor_msgs/MultiDOFJointState"; + } + + static const char* value(const ::sensor_msgs::MultiDOFJointState_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::MultiDOFJointState_ > +{ + static const char* value() + { + return "# Representation of state for joints with multiple degrees of freedom, \n\ +# following the structure of JointState.\n\ +#\n\ +# It is assumed that a joint in a system corresponds to a transform that gets applied \n\ +# along the kinematic chain. For example, a planar joint (as in URDF) is 3DOF (x, y, yaw)\n\ +# and those 3DOF can be expressed as a transformation matrix, and that transformation\n\ +# matrix can be converted back to (x, y, yaw)\n\ +#\n\ +# Each joint is uniquely identified by its name\n\ +# The header specifies the time at which the joint states were recorded. All the joint states\n\ +# in one message have to be recorded at the same time.\n\ +#\n\ +# This message consists of a multiple arrays, one for each part of the joint state. \n\ +# The goal is to make each of the fields optional. When e.g. your joints have no\n\ +# wrench associated with them, you can leave the wrench array empty. \n\ +#\n\ +# All arrays in this message should have the same size, or be empty.\n\ +# This is the only way to uniquely associate the joint name with the correct\n\ +# states.\n\ +\n\ +Header header\n\ +\n\ +string[] joint_names\n\ +geometry_msgs/Transform[] transforms\n\ +geometry_msgs/Twist[] twist\n\ +geometry_msgs/Wrench[] wrench\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Transform\n\ +# This represents the transform between two coordinate frames in free space.\n\ +\n\ +Vector3 translation\n\ +Quaternion rotation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Vector3\n\ +# This represents a vector in free space. \n\ +# It is only meant to represent a direction. Therefore, it does not\n\ +# make sense to apply a translation to it (e.g., when applying a \n\ +# generic rigid transformation to a Vector3, tf2 will only apply the\n\ +# rotation). If you want your data to be translatable too, use the\n\ +# geometry_msgs/Point message instead.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Twist\n\ +# This expresses velocity in free space broken into its linear and angular parts.\n\ +Vector3 linear\n\ +Vector3 angular\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Wrench\n\ +# This represents force in free space, separated into\n\ +# its linear and angular parts.\n\ +Vector3 force\n\ +Vector3 torque\n\ +"; + } + + static const char* value(const ::sensor_msgs::MultiDOFJointState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::MultiDOFJointState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.joint_names); + stream.next(m.transforms); + stream.next(m.twist); + stream.next(m.wrench); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MultiDOFJointState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::MultiDOFJointState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::MultiDOFJointState_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "joint_names[]" << std::endl; + for (size_t i = 0; i < v.joint_names.size(); ++i) + { + s << indent << " joint_names[" << i << "]: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.joint_names[i]); + } + s << indent << "transforms[]" << std::endl; + for (size_t i = 0; i < v.transforms.size(); ++i) + { + s << indent << " transforms[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::Transform_ >::stream(s, indent + " ", v.transforms[i]); + } + s << indent << "twist[]" << std::endl; + for (size_t i = 0; i < v.twist.size(); ++i) + { + s << indent << " twist[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::Twist_ >::stream(s, indent + " ", v.twist[i]); + } + s << indent << "wrench[]" << std::endl; + for (size_t i = 0; i < v.wrench.size(); ++i) + { + s << indent << " wrench[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::Wrench_ >::stream(s, indent + " ", v.wrench[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_MULTIDOFJOINTSTATE_H diff --git a/thirdparty/ros/include/sensor_msgs/MultiEchoLaserScan.h b/thirdparty/ros/include/sensor_msgs/MultiEchoLaserScan.h new file mode 100644 index 0000000..4e03934 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/MultiEchoLaserScan.h @@ -0,0 +1,330 @@ +// Generated by gencpp from file sensor_msgs/MultiEchoLaserScan.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_MULTIECHOLASERSCAN_H +#define SENSOR_MSGS_MESSAGE_MULTIECHOLASERSCAN_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace sensor_msgs +{ +template +struct MultiEchoLaserScan_ +{ + typedef MultiEchoLaserScan_ Type; + + MultiEchoLaserScan_() + : header() + , angle_min(0.0) + , angle_max(0.0) + , angle_increment(0.0) + , time_increment(0.0) + , scan_time(0.0) + , range_min(0.0) + , range_max(0.0) + , ranges() + , intensities() { + } + MultiEchoLaserScan_(const ContainerAllocator& _alloc) + : header(_alloc) + , angle_min(0.0) + , angle_max(0.0) + , angle_increment(0.0) + , time_increment(0.0) + , scan_time(0.0) + , range_min(0.0) + , range_max(0.0) + , ranges(_alloc) + , intensities(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef float _angle_min_type; + _angle_min_type angle_min; + + typedef float _angle_max_type; + _angle_max_type angle_max; + + typedef float _angle_increment_type; + _angle_increment_type angle_increment; + + typedef float _time_increment_type; + _time_increment_type time_increment; + + typedef float _scan_time_type; + _scan_time_type scan_time; + + typedef float _range_min_type; + _range_min_type range_min; + + typedef float _range_max_type; + _range_max_type range_max; + + typedef std::vector< ::sensor_msgs::LaserEcho_ , typename ContainerAllocator::template rebind< ::sensor_msgs::LaserEcho_ >::other > _ranges_type; + _ranges_type ranges; + + typedef std::vector< ::sensor_msgs::LaserEcho_ , typename ContainerAllocator::template rebind< ::sensor_msgs::LaserEcho_ >::other > _intensities_type; + _intensities_type intensities; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::MultiEchoLaserScan_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::MultiEchoLaserScan_ const> ConstPtr; + +}; // struct MultiEchoLaserScan_ + +typedef ::sensor_msgs::MultiEchoLaserScan_ > MultiEchoLaserScan; + +typedef boost::shared_ptr< ::sensor_msgs::MultiEchoLaserScan > MultiEchoLaserScanPtr; +typedef boost::shared_ptr< ::sensor_msgs::MultiEchoLaserScan const> MultiEchoLaserScanConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::MultiEchoLaserScan_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::MultiEchoLaserScan_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::MultiEchoLaserScan_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::MultiEchoLaserScan_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::MultiEchoLaserScan_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::MultiEchoLaserScan_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::MultiEchoLaserScan_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::MultiEchoLaserScan_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::MultiEchoLaserScan_ > +{ + static const char* value() + { + return "6fefb0c6da89d7c8abe4b339f5c2f8fb"; + } + + static const char* value(const ::sensor_msgs::MultiEchoLaserScan_&) { return value(); } + static const uint64_t static_value1 = 0x6fefb0c6da89d7c8ULL; + static const uint64_t static_value2 = 0xabe4b339f5c2f8fbULL; +}; + +template +struct DataType< ::sensor_msgs::MultiEchoLaserScan_ > +{ + static const char* value() + { + return "sensor_msgs/MultiEchoLaserScan"; + } + + static const char* value(const ::sensor_msgs::MultiEchoLaserScan_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::MultiEchoLaserScan_ > +{ + static const char* value() + { + return "# Single scan from a multi-echo planar laser range-finder\n\ +#\n\ +# If you have another ranging device with different behavior (e.g. a sonar\n\ +# array), please find or create a different message, since applications\n\ +# will make fairly laser-specific assumptions about this data\n\ +\n\ +Header header # timestamp in the header is the acquisition time of \n\ + # the first ray in the scan.\n\ + #\n\ + # in frame frame_id, angles are measured around \n\ + # the positive Z axis (counterclockwise, if Z is up)\n\ + # with zero angle being forward along the x axis\n\ + \n\ +float32 angle_min # start angle of the scan [rad]\n\ +float32 angle_max # end angle of the scan [rad]\n\ +float32 angle_increment # angular distance between measurements [rad]\n\ +\n\ +float32 time_increment # time between measurements [seconds] - if your scanner\n\ + # is moving, this will be used in interpolating position\n\ + # of 3d points\n\ +float32 scan_time # time between scans [seconds]\n\ +\n\ +float32 range_min # minimum range value [m]\n\ +float32 range_max # maximum range value [m]\n\ +\n\ +LaserEcho[] ranges # range data [m] (Note: NaNs, values < range_min or > range_max should be discarded)\n\ + # +Inf measurements are out of range\n\ + # -Inf measurements are too close to determine exact distance.\n\ +LaserEcho[] intensities # intensity data [device-specific units]. If your\n\ + # device does not provide intensities, please leave\n\ + # the array empty.\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/LaserEcho\n\ +# This message is a submessage of MultiEchoLaserScan and is not intended\n\ +# to be used separately.\n\ +\n\ +float32[] echoes # Multiple values of ranges or intensities.\n\ + # Each array represents data from the same angle increment.\n\ +"; + } + + static const char* value(const ::sensor_msgs::MultiEchoLaserScan_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::MultiEchoLaserScan_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.angle_min); + stream.next(m.angle_max); + stream.next(m.angle_increment); + stream.next(m.time_increment); + stream.next(m.scan_time); + stream.next(m.range_min); + stream.next(m.range_max); + stream.next(m.ranges); + stream.next(m.intensities); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MultiEchoLaserScan_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::MultiEchoLaserScan_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::MultiEchoLaserScan_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "angle_min: "; + Printer::stream(s, indent + " ", v.angle_min); + s << indent << "angle_max: "; + Printer::stream(s, indent + " ", v.angle_max); + s << indent << "angle_increment: "; + Printer::stream(s, indent + " ", v.angle_increment); + s << indent << "time_increment: "; + Printer::stream(s, indent + " ", v.time_increment); + s << indent << "scan_time: "; + Printer::stream(s, indent + " ", v.scan_time); + s << indent << "range_min: "; + Printer::stream(s, indent + " ", v.range_min); + s << indent << "range_max: "; + Printer::stream(s, indent + " ", v.range_max); + s << indent << "ranges[]" << std::endl; + for (size_t i = 0; i < v.ranges.size(); ++i) + { + s << indent << " ranges[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::sensor_msgs::LaserEcho_ >::stream(s, indent + " ", v.ranges[i]); + } + s << indent << "intensities[]" << std::endl; + for (size_t i = 0; i < v.intensities.size(); ++i) + { + s << indent << " intensities[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::sensor_msgs::LaserEcho_ >::stream(s, indent + " ", v.intensities[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_MULTIECHOLASERSCAN_H diff --git a/thirdparty/ros/include/sensor_msgs/NavSatFix.h b/thirdparty/ros/include/sensor_msgs/NavSatFix.h new file mode 100644 index 0000000..019c09f --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/NavSatFix.h @@ -0,0 +1,347 @@ +// Generated by gencpp from file sensor_msgs/NavSatFix.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_NAVSATFIX_H +#define SENSOR_MSGS_MESSAGE_NAVSATFIX_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace sensor_msgs +{ +template +struct NavSatFix_ +{ + typedef NavSatFix_ Type; + + NavSatFix_() + : header() + , status() + , latitude(0.0) + , longitude(0.0) + , altitude(0.0) + , position_covariance() + , position_covariance_type(0) { + position_covariance.assign(0.0); + } + NavSatFix_(const ContainerAllocator& _alloc) + : header(_alloc) + , status(_alloc) + , latitude(0.0) + , longitude(0.0) + , altitude(0.0) + , position_covariance() + , position_covariance_type(0) { + (void)_alloc; + position_covariance.assign(0.0); + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::sensor_msgs::NavSatStatus_ _status_type; + _status_type status; + + typedef double _latitude_type; + _latitude_type latitude; + + typedef double _longitude_type; + _longitude_type longitude; + + typedef double _altitude_type; + _altitude_type altitude; + + typedef boost::array _position_covariance_type; + _position_covariance_type position_covariance; + + typedef uint8_t _position_covariance_type_type; + _position_covariance_type_type position_covariance_type; + + + + enum { + COVARIANCE_TYPE_UNKNOWN = 0u, + COVARIANCE_TYPE_APPROXIMATED = 1u, + COVARIANCE_TYPE_DIAGONAL_KNOWN = 2u, + COVARIANCE_TYPE_KNOWN = 3u, + }; + + + typedef boost::shared_ptr< ::sensor_msgs::NavSatFix_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::NavSatFix_ const> ConstPtr; + +}; // struct NavSatFix_ + +typedef ::sensor_msgs::NavSatFix_ > NavSatFix; + +typedef boost::shared_ptr< ::sensor_msgs::NavSatFix > NavSatFixPtr; +typedef boost::shared_ptr< ::sensor_msgs::NavSatFix const> NavSatFixConstPtr; + +// constants requiring out of line definition + + + + + + + + + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::NavSatFix_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::NavSatFix_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::NavSatFix_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::NavSatFix_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::NavSatFix_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::NavSatFix_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::NavSatFix_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::NavSatFix_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::NavSatFix_ > +{ + static const char* value() + { + return "2d3a8cd499b9b4a0249fb98fd05cfa48"; + } + + static const char* value(const ::sensor_msgs::NavSatFix_&) { return value(); } + static const uint64_t static_value1 = 0x2d3a8cd499b9b4a0ULL; + static const uint64_t static_value2 = 0x249fb98fd05cfa48ULL; +}; + +template +struct DataType< ::sensor_msgs::NavSatFix_ > +{ + static const char* value() + { + return "sensor_msgs/NavSatFix"; + } + + static const char* value(const ::sensor_msgs::NavSatFix_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::NavSatFix_ > +{ + static const char* value() + { + return "# Navigation Satellite fix for any Global Navigation Satellite System\n\ +#\n\ +# Specified using the WGS 84 reference ellipsoid\n\ +\n\ +# header.stamp specifies the ROS time for this measurement (the\n\ +# corresponding satellite time may be reported using the\n\ +# sensor_msgs/TimeReference message).\n\ +#\n\ +# header.frame_id is the frame of reference reported by the satellite\n\ +# receiver, usually the location of the antenna. This is a\n\ +# Euclidean frame relative to the vehicle, not a reference\n\ +# ellipsoid.\n\ +Header header\n\ +\n\ +# satellite fix status information\n\ +NavSatStatus status\n\ +\n\ +# Latitude [degrees]. Positive is north of equator; negative is south.\n\ +float64 latitude\n\ +\n\ +# Longitude [degrees]. Positive is east of prime meridian; negative is west.\n\ +float64 longitude\n\ +\n\ +# Altitude [m]. Positive is above the WGS 84 ellipsoid\n\ +# (quiet NaN if no altitude is available).\n\ +float64 altitude\n\ +\n\ +# Position covariance [m^2] defined relative to a tangential plane\n\ +# through the reported position. The components are East, North, and\n\ +# Up (ENU), in row-major order.\n\ +#\n\ +# Beware: this coordinate system exhibits singularities at the poles.\n\ +\n\ +float64[9] position_covariance\n\ +\n\ +# If the covariance of the fix is known, fill it in completely. If the\n\ +# GPS receiver provides the variance of each measurement, put them\n\ +# along the diagonal. If only Dilution of Precision is available,\n\ +# estimate an approximate covariance from that.\n\ +\n\ +uint8 COVARIANCE_TYPE_UNKNOWN = 0\n\ +uint8 COVARIANCE_TYPE_APPROXIMATED = 1\n\ +uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2\n\ +uint8 COVARIANCE_TYPE_KNOWN = 3\n\ +\n\ +uint8 position_covariance_type\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/NavSatStatus\n\ +# Navigation Satellite fix status for any Global Navigation Satellite System\n\ +\n\ +# Whether to output an augmented fix is determined by both the fix\n\ +# type and the last time differential corrections were received. A\n\ +# fix is valid when status >= STATUS_FIX.\n\ +\n\ +int8 STATUS_NO_FIX = -1 # unable to fix position\n\ +int8 STATUS_FIX = 0 # unaugmented fix\n\ +int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation\n\ +int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation\n\ +\n\ +int8 status\n\ +\n\ +# Bits defining which Global Navigation Satellite System signals were\n\ +# used by the receiver.\n\ +\n\ +uint16 SERVICE_GPS = 1\n\ +uint16 SERVICE_GLONASS = 2\n\ +uint16 SERVICE_COMPASS = 4 # includes BeiDou.\n\ +uint16 SERVICE_GALILEO = 8\n\ +\n\ +uint16 service\n\ +"; + } + + static const char* value(const ::sensor_msgs::NavSatFix_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::NavSatFix_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.status); + stream.next(m.latitude); + stream.next(m.longitude); + stream.next(m.altitude); + stream.next(m.position_covariance); + stream.next(m.position_covariance_type); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct NavSatFix_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::NavSatFix_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::NavSatFix_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "status: "; + s << std::endl; + Printer< ::sensor_msgs::NavSatStatus_ >::stream(s, indent + " ", v.status); + s << indent << "latitude: "; + Printer::stream(s, indent + " ", v.latitude); + s << indent << "longitude: "; + Printer::stream(s, indent + " ", v.longitude); + s << indent << "altitude: "; + Printer::stream(s, indent + " ", v.altitude); + s << indent << "position_covariance[]" << std::endl; + for (size_t i = 0; i < v.position_covariance.size(); ++i) + { + s << indent << " position_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.position_covariance[i]); + } + s << indent << "position_covariance_type: "; + Printer::stream(s, indent + " ", v.position_covariance_type); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_NAVSATFIX_H diff --git a/thirdparty/ros/include/sensor_msgs/NavSatStatus.h b/thirdparty/ros/include/sensor_msgs/NavSatStatus.h new file mode 100644 index 0000000..4702295 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/NavSatStatus.h @@ -0,0 +1,242 @@ +// Generated by gencpp from file sensor_msgs/NavSatStatus.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_NAVSATSTATUS_H +#define SENSOR_MSGS_MESSAGE_NAVSATSTATUS_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct NavSatStatus_ +{ + typedef NavSatStatus_ Type; + + NavSatStatus_() + : status(0) + , service(0) { + } + NavSatStatus_(const ContainerAllocator& _alloc) + : status(0) + , service(0) { + (void)_alloc; + } + + + + typedef int8_t _status_type; + _status_type status; + + typedef uint16_t _service_type; + _service_type service; + + + + enum { + STATUS_NO_FIX = -1, + STATUS_FIX = 0, + STATUS_SBAS_FIX = 1, + STATUS_GBAS_FIX = 2, + SERVICE_GPS = 1u, + SERVICE_GLONASS = 2u, + SERVICE_COMPASS = 4u, + SERVICE_GALILEO = 8u, + }; + + + typedef boost::shared_ptr< ::sensor_msgs::NavSatStatus_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::NavSatStatus_ const> ConstPtr; + +}; // struct NavSatStatus_ + +typedef ::sensor_msgs::NavSatStatus_ > NavSatStatus; + +typedef boost::shared_ptr< ::sensor_msgs::NavSatStatus > NavSatStatusPtr; +typedef boost::shared_ptr< ::sensor_msgs::NavSatStatus const> NavSatStatusConstPtr; + +// constants requiring out of line definition + + + + + + + + + + + + + + + + + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::NavSatStatus_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::NavSatStatus_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::NavSatStatus_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sensor_msgs::NavSatStatus_ const> + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::NavSatStatus_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::NavSatStatus_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::NavSatStatus_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::NavSatStatus_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::NavSatStatus_ > +{ + static const char* value() + { + return "331cdbddfa4bc96ffc3b9ad98900a54c"; + } + + static const char* value(const ::sensor_msgs::NavSatStatus_&) { return value(); } + static const uint64_t static_value1 = 0x331cdbddfa4bc96fULL; + static const uint64_t static_value2 = 0xfc3b9ad98900a54cULL; +}; + +template +struct DataType< ::sensor_msgs::NavSatStatus_ > +{ + static const char* value() + { + return "sensor_msgs/NavSatStatus"; + } + + static const char* value(const ::sensor_msgs::NavSatStatus_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::NavSatStatus_ > +{ + static const char* value() + { + return "# Navigation Satellite fix status for any Global Navigation Satellite System\n\ +\n\ +# Whether to output an augmented fix is determined by both the fix\n\ +# type and the last time differential corrections were received. A\n\ +# fix is valid when status >= STATUS_FIX.\n\ +\n\ +int8 STATUS_NO_FIX = -1 # unable to fix position\n\ +int8 STATUS_FIX = 0 # unaugmented fix\n\ +int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation\n\ +int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation\n\ +\n\ +int8 status\n\ +\n\ +# Bits defining which Global Navigation Satellite System signals were\n\ +# used by the receiver.\n\ +\n\ +uint16 SERVICE_GPS = 1\n\ +uint16 SERVICE_GLONASS = 2\n\ +uint16 SERVICE_COMPASS = 4 # includes BeiDou.\n\ +uint16 SERVICE_GALILEO = 8\n\ +\n\ +uint16 service\n\ +"; + } + + static const char* value(const ::sensor_msgs::NavSatStatus_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::NavSatStatus_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.status); + stream.next(m.service); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct NavSatStatus_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::NavSatStatus_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::NavSatStatus_& v) + { + s << indent << "status: "; + Printer::stream(s, indent + " ", v.status); + s << indent << "service: "; + Printer::stream(s, indent + " ", v.service); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_NAVSATSTATUS_H diff --git a/thirdparty/ros/include/sensor_msgs/PointCloud.h b/thirdparty/ros/include/sensor_msgs/PointCloud.h new file mode 100644 index 0000000..654cdbd --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/PointCloud.h @@ -0,0 +1,290 @@ +// Generated by gencpp from file sensor_msgs/PointCloud.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_POINTCLOUD_H +#define SENSOR_MSGS_MESSAGE_POINTCLOUD_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace sensor_msgs +{ +template +struct PointCloud_ +{ + typedef PointCloud_ Type; + + PointCloud_() + : header() + , points() + , channels() { + } + PointCloud_(const ContainerAllocator& _alloc) + : header(_alloc) + , points(_alloc) + , channels(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector< ::geometry_msgs::Point32_ , typename ContainerAllocator::template rebind< ::geometry_msgs::Point32_ >::other > _points_type; + _points_type points; + + typedef std::vector< ::sensor_msgs::ChannelFloat32_ , typename ContainerAllocator::template rebind< ::sensor_msgs::ChannelFloat32_ >::other > _channels_type; + _channels_type channels; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::PointCloud_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::PointCloud_ const> ConstPtr; + +}; // struct PointCloud_ + +typedef ::sensor_msgs::PointCloud_ > PointCloud; + +typedef boost::shared_ptr< ::sensor_msgs::PointCloud > PointCloudPtr; +typedef boost::shared_ptr< ::sensor_msgs::PointCloud const> PointCloudConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::PointCloud_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::PointCloud_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::PointCloud_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::PointCloud_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::PointCloud_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::PointCloud_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::PointCloud_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::PointCloud_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::PointCloud_ > +{ + static const char* value() + { + return "d8e9c3f5afbdd8a130fd1d2763945fca"; + } + + static const char* value(const ::sensor_msgs::PointCloud_&) { return value(); } + static const uint64_t static_value1 = 0xd8e9c3f5afbdd8a1ULL; + static const uint64_t static_value2 = 0x30fd1d2763945fcaULL; +}; + +template +struct DataType< ::sensor_msgs::PointCloud_ > +{ + static const char* value() + { + return "sensor_msgs/PointCloud"; + } + + static const char* value(const ::sensor_msgs::PointCloud_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::PointCloud_ > +{ + static const char* value() + { + return "# This message holds a collection of 3d points, plus optional additional\n\ +# information about each point.\n\ +\n\ +# Time of sensor data acquisition, coordinate frame ID.\n\ +Header header\n\ +\n\ +# Array of 3d points. Each Point32 should be interpreted as a 3d point\n\ +# in the frame given in the header.\n\ +geometry_msgs/Point32[] points\n\ +\n\ +# Each channel should have the same number of elements as points array,\n\ +# and the data in each channel should correspond 1:1 with each point.\n\ +# Channel names in common practice are listed in ChannelFloat32.msg.\n\ +ChannelFloat32[] channels\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point32\n\ +# This contains the position of a point in free space(with 32 bits of precision).\n\ +# It is recommeded to use Point wherever possible instead of Point32. \n\ +# \n\ +# This recommendation is to promote interoperability. \n\ +#\n\ +# This message is designed to take up less space when sending\n\ +# lots of points at once, as in the case of a PointCloud. \n\ +\n\ +float32 x\n\ +float32 y\n\ +float32 z\n\ +================================================================================\n\ +MSG: sensor_msgs/ChannelFloat32\n\ +# This message is used by the PointCloud message to hold optional data\n\ +# associated with each point in the cloud. The length of the values\n\ +# array should be the same as the length of the points array in the\n\ +# PointCloud, and each value should be associated with the corresponding\n\ +# point.\n\ +\n\ +# Channel names in existing practice include:\n\ +# \"u\", \"v\" - row and column (respectively) in the left stereo image.\n\ +# This is opposite to usual conventions but remains for\n\ +# historical reasons. The newer PointCloud2 message has no\n\ +# such problem.\n\ +# \"rgb\" - For point clouds produced by color stereo cameras. uint8\n\ +# (R,G,B) values packed into the least significant 24 bits,\n\ +# in order.\n\ +# \"intensity\" - laser or pixel intensity.\n\ +# \"distance\"\n\ +\n\ +# The channel name should give semantics of the channel (e.g.\n\ +# \"intensity\" instead of \"value\").\n\ +string name\n\ +\n\ +# The values array should be 1-1 with the elements of the associated\n\ +# PointCloud.\n\ +float32[] values\n\ +"; + } + + static const char* value(const ::sensor_msgs::PointCloud_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::PointCloud_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.points); + stream.next(m.channels); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PointCloud_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::PointCloud_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::PointCloud_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "points[]" << std::endl; + for (size_t i = 0; i < v.points.size(); ++i) + { + s << indent << " points[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::Point32_ >::stream(s, indent + " ", v.points[i]); + } + s << indent << "channels[]" << std::endl; + for (size_t i = 0; i < v.channels.size(); ++i) + { + s << indent << " channels[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::sensor_msgs::ChannelFloat32_ >::stream(s, indent + " ", v.channels[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_POINTCLOUD_H diff --git a/thirdparty/ros/include/sensor_msgs/PointCloud2.h b/thirdparty/ros/include/sensor_msgs/PointCloud2.h new file mode 100644 index 0000000..cf95217 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/PointCloud2.h @@ -0,0 +1,326 @@ +// Generated by gencpp from file sensor_msgs/PointCloud2.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_POINTCLOUD2_H +#define SENSOR_MSGS_MESSAGE_POINTCLOUD2_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace sensor_msgs +{ +template +struct PointCloud2_ +{ + typedef PointCloud2_ Type; + + PointCloud2_() + : header() + , height(0) + , width(0) + , fields() + , is_bigendian(false) + , point_step(0) + , row_step(0) + , data() + , is_dense(false) { + } + PointCloud2_(const ContainerAllocator& _alloc) + : header(_alloc) + , height(0) + , width(0) + , fields(_alloc) + , is_bigendian(false) + , point_step(0) + , row_step(0) + , data(_alloc) + , is_dense(false) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint32_t _height_type; + _height_type height; + + typedef uint32_t _width_type; + _width_type width; + + typedef std::vector< ::sensor_msgs::PointField_ , typename ContainerAllocator::template rebind< ::sensor_msgs::PointField_ >::other > _fields_type; + _fields_type fields; + + typedef uint8_t _is_bigendian_type; + _is_bigendian_type is_bigendian; + + typedef uint32_t _point_step_type; + _point_step_type point_step; + + typedef uint32_t _row_step_type; + _row_step_type row_step; + + typedef std::vector::other > _data_type; + _data_type data; + + typedef uint8_t _is_dense_type; + _is_dense_type is_dense; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::PointCloud2_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::PointCloud2_ const> ConstPtr; + +}; // struct PointCloud2_ + +typedef ::sensor_msgs::PointCloud2_ > PointCloud2; + +typedef boost::shared_ptr< ::sensor_msgs::PointCloud2 > PointCloud2Ptr; +typedef boost::shared_ptr< ::sensor_msgs::PointCloud2 const> PointCloud2ConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::PointCloud2_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::PointCloud2_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::PointCloud2_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::PointCloud2_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::PointCloud2_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::PointCloud2_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::PointCloud2_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::PointCloud2_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::PointCloud2_ > +{ + static const char* value() + { + return "1158d486dd51d683ce2f1be655c3c181"; + } + + static const char* value(const ::sensor_msgs::PointCloud2_&) { return value(); } + static const uint64_t static_value1 = 0x1158d486dd51d683ULL; + static const uint64_t static_value2 = 0xce2f1be655c3c181ULL; +}; + +template +struct DataType< ::sensor_msgs::PointCloud2_ > +{ + static const char* value() + { + return "sensor_msgs/PointCloud2"; + } + + static const char* value(const ::sensor_msgs::PointCloud2_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::PointCloud2_ > +{ + static const char* value() + { + return "# This message holds a collection of N-dimensional points, which may\n\ +# contain additional information such as normals, intensity, etc. The\n\ +# point data is stored as a binary blob, its layout described by the\n\ +# contents of the \"fields\" array.\n\ +\n\ +# The point cloud data may be organized 2d (image-like) or 1d\n\ +# (unordered). Point clouds organized as 2d images may be produced by\n\ +# camera depth sensors such as stereo or time-of-flight.\n\ +\n\ +# Time of sensor data acquisition, and the coordinate frame ID (for 3d\n\ +# points).\n\ +Header header\n\ +\n\ +# 2D structure of the point cloud. If the cloud is unordered, height is\n\ +# 1 and width is the length of the point cloud.\n\ +uint32 height\n\ +uint32 width\n\ +\n\ +# Describes the channels and their layout in the binary data blob.\n\ +PointField[] fields\n\ +\n\ +bool is_bigendian # Is this data bigendian?\n\ +uint32 point_step # Length of a point in bytes\n\ +uint32 row_step # Length of a row in bytes\n\ +uint8[] data # Actual point data, size is (row_step*height)\n\ +\n\ +bool is_dense # True if there are no invalid points\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/PointField\n\ +# This message holds the description of one point entry in the\n\ +# PointCloud2 message format.\n\ +uint8 INT8 = 1\n\ +uint8 UINT8 = 2\n\ +uint8 INT16 = 3\n\ +uint8 UINT16 = 4\n\ +uint8 INT32 = 5\n\ +uint8 UINT32 = 6\n\ +uint8 FLOAT32 = 7\n\ +uint8 FLOAT64 = 8\n\ +\n\ +string name # Name of field\n\ +uint32 offset # Offset from start of point struct\n\ +uint8 datatype # Datatype enumeration, see above\n\ +uint32 count # How many elements in the field\n\ +"; + } + + static const char* value(const ::sensor_msgs::PointCloud2_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::PointCloud2_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.height); + stream.next(m.width); + stream.next(m.fields); + stream.next(m.is_bigendian); + stream.next(m.point_step); + stream.next(m.row_step); + stream.next(m.data); + stream.next(m.is_dense); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PointCloud2_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::PointCloud2_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::PointCloud2_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "height: "; + Printer::stream(s, indent + " ", v.height); + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "fields[]" << std::endl; + for (size_t i = 0; i < v.fields.size(); ++i) + { + s << indent << " fields[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::sensor_msgs::PointField_ >::stream(s, indent + " ", v.fields[i]); + } + s << indent << "is_bigendian: "; + Printer::stream(s, indent + " ", v.is_bigendian); + s << indent << "point_step: "; + Printer::stream(s, indent + " ", v.point_step); + s << indent << "row_step: "; + Printer::stream(s, indent + " ", v.row_step); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + s << indent << "is_dense: "; + Printer::stream(s, indent + " ", v.is_dense); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_POINTCLOUD2_H diff --git a/thirdparty/ros/include/sensor_msgs/PointField.h b/thirdparty/ros/include/sensor_msgs/PointField.h new file mode 100644 index 0000000..4d55f27 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/PointField.h @@ -0,0 +1,251 @@ +// Generated by gencpp from file sensor_msgs/PointField.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_POINTFIELD_H +#define SENSOR_MSGS_MESSAGE_POINTFIELD_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct PointField_ +{ + typedef PointField_ Type; + + PointField_() + : name() + , offset(0) + , datatype(0) + , count(0) { + } + PointField_(const ContainerAllocator& _alloc) + : name(_alloc) + , offset(0) + , datatype(0) + , count(0) { + (void)_alloc; + } + + + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _name_type; + _name_type name; + + typedef uint32_t _offset_type; + _offset_type offset; + + typedef uint8_t _datatype_type; + _datatype_type datatype; + + typedef uint32_t _count_type; + _count_type count; + + + + enum { + INT8 = 1u, + UINT8 = 2u, + INT16 = 3u, + UINT16 = 4u, + INT32 = 5u, + UINT32 = 6u, + FLOAT32 = 7u, + FLOAT64 = 8u, + }; + + + typedef boost::shared_ptr< ::sensor_msgs::PointField_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::PointField_ const> ConstPtr; + +}; // struct PointField_ + +typedef ::sensor_msgs::PointField_ > PointField; + +typedef boost::shared_ptr< ::sensor_msgs::PointField > PointFieldPtr; +typedef boost::shared_ptr< ::sensor_msgs::PointField const> PointFieldConstPtr; + +// constants requiring out of line definition + + + + + + + + + + + + + + + + + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::PointField_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::PointField_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::PointField_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::PointField_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::PointField_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::PointField_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::PointField_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::PointField_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::PointField_ > +{ + static const char* value() + { + return "268eacb2962780ceac86cbd17e328150"; + } + + static const char* value(const ::sensor_msgs::PointField_&) { return value(); } + static const uint64_t static_value1 = 0x268eacb2962780ceULL; + static const uint64_t static_value2 = 0xac86cbd17e328150ULL; +}; + +template +struct DataType< ::sensor_msgs::PointField_ > +{ + static const char* value() + { + return "sensor_msgs/PointField"; + } + + static const char* value(const ::sensor_msgs::PointField_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::PointField_ > +{ + static const char* value() + { + return "# This message holds the description of one point entry in the\n\ +# PointCloud2 message format.\n\ +uint8 INT8 = 1\n\ +uint8 UINT8 = 2\n\ +uint8 INT16 = 3\n\ +uint8 UINT16 = 4\n\ +uint8 INT32 = 5\n\ +uint8 UINT32 = 6\n\ +uint8 FLOAT32 = 7\n\ +uint8 FLOAT64 = 8\n\ +\n\ +string name # Name of field\n\ +uint32 offset # Offset from start of point struct\n\ +uint8 datatype # Datatype enumeration, see above\n\ +uint32 count # How many elements in the field\n\ +"; + } + + static const char* value(const ::sensor_msgs::PointField_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::PointField_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.name); + stream.next(m.offset); + stream.next(m.datatype); + stream.next(m.count); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PointField_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::PointField_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::PointField_& v) + { + s << indent << "name: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.name); + s << indent << "offset: "; + Printer::stream(s, indent + " ", v.offset); + s << indent << "datatype: "; + Printer::stream(s, indent + " ", v.datatype); + s << indent << "count: "; + Printer::stream(s, indent + " ", v.count); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_POINTFIELD_H diff --git a/thirdparty/ros/include/sensor_msgs/Range.h b/thirdparty/ros/include/sensor_msgs/Range.h new file mode 100644 index 0000000..d46e6ac --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/Range.h @@ -0,0 +1,293 @@ +// Generated by gencpp from file sensor_msgs/Range.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_RANGE_H +#define SENSOR_MSGS_MESSAGE_RANGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Range_ +{ + typedef Range_ Type; + + Range_() + : header() + , radiation_type(0) + , field_of_view(0.0) + , min_range(0.0) + , max_range(0.0) + , range(0.0) { + } + Range_(const ContainerAllocator& _alloc) + : header(_alloc) + , radiation_type(0) + , field_of_view(0.0) + , min_range(0.0) + , max_range(0.0) + , range(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint8_t _radiation_type_type; + _radiation_type_type radiation_type; + + typedef float _field_of_view_type; + _field_of_view_type field_of_view; + + typedef float _min_range_type; + _min_range_type min_range; + + typedef float _max_range_type; + _max_range_type max_range; + + typedef float _range_type; + _range_type range; + + + + enum { + ULTRASOUND = 0u, + INFRARED = 1u, + }; + + + typedef boost::shared_ptr< ::sensor_msgs::Range_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Range_ const> ConstPtr; + +}; // struct Range_ + +typedef ::sensor_msgs::Range_ > Range; + +typedef boost::shared_ptr< ::sensor_msgs::Range > RangePtr; +typedef boost::shared_ptr< ::sensor_msgs::Range const> RangeConstPtr; + +// constants requiring out of line definition + + + + + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Range_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Range_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::Range_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Range_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::Range_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Range_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Range_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Range_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Range_ > +{ + static const char* value() + { + return "c005c34273dc426c67a020a87bc24148"; + } + + static const char* value(const ::sensor_msgs::Range_&) { return value(); } + static const uint64_t static_value1 = 0xc005c34273dc426cULL; + static const uint64_t static_value2 = 0x67a020a87bc24148ULL; +}; + +template +struct DataType< ::sensor_msgs::Range_ > +{ + static const char* value() + { + return "sensor_msgs/Range"; + } + + static const char* value(const ::sensor_msgs::Range_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Range_ > +{ + static const char* value() + { + return "# Single range reading from an active ranger that emits energy and reports\n\ +# one range reading that is valid along an arc at the distance measured. \n\ +# This message is not appropriate for laser scanners. See the LaserScan\n\ +# message if you are working with a laser scanner.\n\ +\n\ +# This message also can represent a fixed-distance (binary) ranger. This\n\ +# sensor will have min_range===max_range===distance of detection.\n\ +# These sensors follow REP 117 and will output -Inf if the object is detected\n\ +# and +Inf if the object is outside of the detection range.\n\ +\n\ +Header header # timestamp in the header is the time the ranger\n\ + # returned the distance reading\n\ +\n\ +# Radiation type enums\n\ +# If you want a value added to this list, send an email to the ros-users list\n\ +uint8 ULTRASOUND=0\n\ +uint8 INFRARED=1\n\ +\n\ +uint8 radiation_type # the type of radiation used by the sensor\n\ + # (sound, IR, etc) [enum]\n\ +\n\ +float32 field_of_view # the size of the arc that the distance reading is\n\ + # valid for [rad]\n\ + # the object causing the range reading may have\n\ + # been anywhere within -field_of_view/2 and\n\ + # field_of_view/2 at the measured range. \n\ + # 0 angle corresponds to the x-axis of the sensor.\n\ +\n\ +float32 min_range # minimum range value [m]\n\ +float32 max_range # maximum range value [m]\n\ + # Fixed distance rangers require min_range==max_range\n\ +\n\ +float32 range # range data [m]\n\ + # (Note: values < range_min or > range_max\n\ + # should be discarded)\n\ + # Fixed distance rangers only output -Inf or +Inf.\n\ + # -Inf represents a detection within fixed distance.\n\ + # (Detection too close to the sensor to quantify)\n\ + # +Inf represents no detection within the fixed distance.\n\ + # (Object out of range)\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::Range_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Range_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.radiation_type); + stream.next(m.field_of_view); + stream.next(m.min_range); + stream.next(m.max_range); + stream.next(m.range); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Range_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Range_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Range_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "radiation_type: "; + Printer::stream(s, indent + " ", v.radiation_type); + s << indent << "field_of_view: "; + Printer::stream(s, indent + " ", v.field_of_view); + s << indent << "min_range: "; + Printer::stream(s, indent + " ", v.min_range); + s << indent << "max_range: "; + Printer::stream(s, indent + " ", v.max_range); + s << indent << "range: "; + Printer::stream(s, indent + " ", v.range); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_RANGE_H diff --git a/thirdparty/ros/include/sensor_msgs/RegionOfInterest.h b/thirdparty/ros/include/sensor_msgs/RegionOfInterest.h new file mode 100644 index 0000000..53facfa --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/RegionOfInterest.h @@ -0,0 +1,237 @@ +// Generated by gencpp from file sensor_msgs/RegionOfInterest.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_REGIONOFINTEREST_H +#define SENSOR_MSGS_MESSAGE_REGIONOFINTEREST_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct RegionOfInterest_ +{ + typedef RegionOfInterest_ Type; + + RegionOfInterest_() + : x_offset(0) + , y_offset(0) + , height(0) + , width(0) + , do_rectify(false) { + } + RegionOfInterest_(const ContainerAllocator& _alloc) + : x_offset(0) + , y_offset(0) + , height(0) + , width(0) + , do_rectify(false) { + (void)_alloc; + } + + + + typedef uint32_t _x_offset_type; + _x_offset_type x_offset; + + typedef uint32_t _y_offset_type; + _y_offset_type y_offset; + + typedef uint32_t _height_type; + _height_type height; + + typedef uint32_t _width_type; + _width_type width; + + typedef uint8_t _do_rectify_type; + _do_rectify_type do_rectify; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::RegionOfInterest_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::RegionOfInterest_ const> ConstPtr; + +}; // struct RegionOfInterest_ + +typedef ::sensor_msgs::RegionOfInterest_ > RegionOfInterest; + +typedef boost::shared_ptr< ::sensor_msgs::RegionOfInterest > RegionOfInterestPtr; +typedef boost::shared_ptr< ::sensor_msgs::RegionOfInterest const> RegionOfInterestConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::RegionOfInterest_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::RegionOfInterest_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::RegionOfInterest_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sensor_msgs::RegionOfInterest_ const> + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::RegionOfInterest_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::RegionOfInterest_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::RegionOfInterest_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::RegionOfInterest_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::RegionOfInterest_ > +{ + static const char* value() + { + return "bdb633039d588fcccb441a4d43ccfe09"; + } + + static const char* value(const ::sensor_msgs::RegionOfInterest_&) { return value(); } + static const uint64_t static_value1 = 0xbdb633039d588fccULL; + static const uint64_t static_value2 = 0xcb441a4d43ccfe09ULL; +}; + +template +struct DataType< ::sensor_msgs::RegionOfInterest_ > +{ + static const char* value() + { + return "sensor_msgs/RegionOfInterest"; + } + + static const char* value(const ::sensor_msgs::RegionOfInterest_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::RegionOfInterest_ > +{ + static const char* value() + { + return "# This message is used to specify a region of interest within an image.\n\ +#\n\ +# When used to specify the ROI setting of the camera when the image was\n\ +# taken, the height and width fields should either match the height and\n\ +# width fields for the associated image; or height = width = 0\n\ +# indicates that the full resolution image was captured.\n\ +\n\ +uint32 x_offset # Leftmost pixel of the ROI\n\ + # (0 if the ROI includes the left edge of the image)\n\ +uint32 y_offset # Topmost pixel of the ROI\n\ + # (0 if the ROI includes the top edge of the image)\n\ +uint32 height # Height of ROI\n\ +uint32 width # Width of ROI\n\ +\n\ +# True if a distinct rectified ROI should be calculated from the \"raw\"\n\ +# ROI in this message. Typically this should be False if the full image\n\ +# is captured (ROI not used), and True if a subwindow is captured (ROI\n\ +# used).\n\ +bool do_rectify\n\ +"; + } + + static const char* value(const ::sensor_msgs::RegionOfInterest_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::RegionOfInterest_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x_offset); + stream.next(m.y_offset); + stream.next(m.height); + stream.next(m.width); + stream.next(m.do_rectify); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct RegionOfInterest_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::RegionOfInterest_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::RegionOfInterest_& v) + { + s << indent << "x_offset: "; + Printer::stream(s, indent + " ", v.x_offset); + s << indent << "y_offset: "; + Printer::stream(s, indent + " ", v.y_offset); + s << indent << "height: "; + Printer::stream(s, indent + " ", v.height); + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "do_rectify: "; + Printer::stream(s, indent + " ", v.do_rectify); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_REGIONOFINTEREST_H diff --git a/thirdparty/ros/include/sensor_msgs/RelativeHumidity.h b/thirdparty/ros/include/sensor_msgs/RelativeHumidity.h new file mode 100644 index 0000000..ff5ec8e --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/RelativeHumidity.h @@ -0,0 +1,233 @@ +// Generated by gencpp from file sensor_msgs/RelativeHumidity.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_RELATIVEHUMIDITY_H +#define SENSOR_MSGS_MESSAGE_RELATIVEHUMIDITY_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct RelativeHumidity_ +{ + typedef RelativeHumidity_ Type; + + RelativeHumidity_() + : header() + , relative_humidity(0.0) + , variance(0.0) { + } + RelativeHumidity_(const ContainerAllocator& _alloc) + : header(_alloc) + , relative_humidity(0.0) + , variance(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef double _relative_humidity_type; + _relative_humidity_type relative_humidity; + + typedef double _variance_type; + _variance_type variance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::RelativeHumidity_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::RelativeHumidity_ const> ConstPtr; + +}; // struct RelativeHumidity_ + +typedef ::sensor_msgs::RelativeHumidity_ > RelativeHumidity; + +typedef boost::shared_ptr< ::sensor_msgs::RelativeHumidity > RelativeHumidityPtr; +typedef boost::shared_ptr< ::sensor_msgs::RelativeHumidity const> RelativeHumidityConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::RelativeHumidity_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::RelativeHumidity_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::RelativeHumidity_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::RelativeHumidity_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::RelativeHumidity_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::RelativeHumidity_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::RelativeHumidity_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::RelativeHumidity_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::RelativeHumidity_ > +{ + static const char* value() + { + return "8730015b05955b7e992ce29a2678d90f"; + } + + static const char* value(const ::sensor_msgs::RelativeHumidity_&) { return value(); } + static const uint64_t static_value1 = 0x8730015b05955b7eULL; + static const uint64_t static_value2 = 0x992ce29a2678d90fULL; +}; + +template +struct DataType< ::sensor_msgs::RelativeHumidity_ > +{ + static const char* value() + { + return "sensor_msgs/RelativeHumidity"; + } + + static const char* value(const ::sensor_msgs::RelativeHumidity_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::RelativeHumidity_ > +{ + static const char* value() + { + return " # Single reading from a relative humidity sensor. Defines the ratio of partial\n\ + # pressure of water vapor to the saturated vapor pressure at a temperature.\n\ +\n\ + Header header # timestamp of the measurement\n\ + # frame_id is the location of the humidity sensor\n\ +\n\ + float64 relative_humidity # Expression of the relative humidity\n\ + # from 0.0 to 1.0.\n\ + # 0.0 is no partial pressure of water vapor\n\ + # 1.0 represents partial pressure of saturation\n\ +\n\ + float64 variance # 0 is interpreted as variance unknown\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::RelativeHumidity_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::RelativeHumidity_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.relative_humidity); + stream.next(m.variance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct RelativeHumidity_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::RelativeHumidity_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::RelativeHumidity_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "relative_humidity: "; + Printer::stream(s, indent + " ", v.relative_humidity); + s << indent << "variance: "; + Printer::stream(s, indent + " ", v.variance); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_RELATIVEHUMIDITY_H diff --git a/thirdparty/ros/include/sensor_msgs/SetCameraInfo.h b/thirdparty/ros/include/sensor_msgs/SetCameraInfo.h new file mode 100644 index 0000000..2e4c167 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/SetCameraInfo.h @@ -0,0 +1,123 @@ +// Generated by gencpp from file sensor_msgs/SetCameraInfo.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_SETCAMERAINFO_H +#define SENSOR_MSGS_MESSAGE_SETCAMERAINFO_H + +#include + + +#include +#include + + +namespace sensor_msgs +{ + +struct SetCameraInfo +{ + +typedef SetCameraInfoRequest Request; +typedef SetCameraInfoResponse Response; +Request request; +Response response; + +typedef Request RequestType; +typedef Response ResponseType; + +}; // struct SetCameraInfo +} // namespace sensor_msgs + + +namespace ros +{ +namespace service_traits +{ + + +template<> +struct MD5Sum< ::sensor_msgs::SetCameraInfo > { + static const char* value() + { + return "bef1df590ed75ed1f393692395e15482"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfo&) { return value(); } +}; + +template<> +struct DataType< ::sensor_msgs::SetCameraInfo > { + static const char* value() + { + return "sensor_msgs/SetCameraInfo"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfo&) { return value(); } +}; + + +// service_traits::MD5Sum< ::sensor_msgs::SetCameraInfoRequest> should match +// service_traits::MD5Sum< ::sensor_msgs::SetCameraInfo > +template<> +struct MD5Sum< ::sensor_msgs::SetCameraInfoRequest> +{ + static const char* value() + { + return MD5Sum< ::sensor_msgs::SetCameraInfo >::value(); + } + static const char* value(const ::sensor_msgs::SetCameraInfoRequest&) + { + return value(); + } +}; + +// service_traits::DataType< ::sensor_msgs::SetCameraInfoRequest> should match +// service_traits::DataType< ::sensor_msgs::SetCameraInfo > +template<> +struct DataType< ::sensor_msgs::SetCameraInfoRequest> +{ + static const char* value() + { + return DataType< ::sensor_msgs::SetCameraInfo >::value(); + } + static const char* value(const ::sensor_msgs::SetCameraInfoRequest&) + { + return value(); + } +}; + +// service_traits::MD5Sum< ::sensor_msgs::SetCameraInfoResponse> should match +// service_traits::MD5Sum< ::sensor_msgs::SetCameraInfo > +template<> +struct MD5Sum< ::sensor_msgs::SetCameraInfoResponse> +{ + static const char* value() + { + return MD5Sum< ::sensor_msgs::SetCameraInfo >::value(); + } + static const char* value(const ::sensor_msgs::SetCameraInfoResponse&) + { + return value(); + } +}; + +// service_traits::DataType< ::sensor_msgs::SetCameraInfoResponse> should match +// service_traits::DataType< ::sensor_msgs::SetCameraInfo > +template<> +struct DataType< ::sensor_msgs::SetCameraInfoResponse> +{ + static const char* value() + { + return DataType< ::sensor_msgs::SetCameraInfo >::value(); + } + static const char* value(const ::sensor_msgs::SetCameraInfoResponse&) + { + return value(); + } +}; + +} // namespace service_traits +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_SETCAMERAINFO_H diff --git a/thirdparty/ros/include/sensor_msgs/SetCameraInfoRequest.h b/thirdparty/ros/include/sensor_msgs/SetCameraInfoRequest.h new file mode 100644 index 0000000..51db184 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/SetCameraInfoRequest.h @@ -0,0 +1,371 @@ +// Generated by gencpp from file sensor_msgs/SetCameraInfoRequest.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_SETCAMERAINFOREQUEST_H +#define SENSOR_MSGS_MESSAGE_SETCAMERAINFOREQUEST_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct SetCameraInfoRequest_ +{ + typedef SetCameraInfoRequest_ Type; + + SetCameraInfoRequest_() + : camera_info() { + } + SetCameraInfoRequest_(const ContainerAllocator& _alloc) + : camera_info(_alloc) { + (void)_alloc; + } + + + + typedef ::sensor_msgs::CameraInfo_ _camera_info_type; + _camera_info_type camera_info; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoRequest_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoRequest_ const> ConstPtr; + +}; // struct SetCameraInfoRequest_ + +typedef ::sensor_msgs::SetCameraInfoRequest_ > SetCameraInfoRequest; + +typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoRequest > SetCameraInfoRequestPtr; +typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoRequest const> SetCameraInfoRequestConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::SetCameraInfoRequest_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::SetCameraInfoRequest_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::SetCameraInfoRequest_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::SetCameraInfoRequest_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::SetCameraInfoRequest_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::SetCameraInfoRequest_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::SetCameraInfoRequest_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::SetCameraInfoRequest_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::SetCameraInfoRequest_ > +{ + static const char* value() + { + return "ee34be01fdeee563d0d99cd594d5581d"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfoRequest_&) { return value(); } + static const uint64_t static_value1 = 0xee34be01fdeee563ULL; + static const uint64_t static_value2 = 0xd0d99cd594d5581dULL; +}; + +template +struct DataType< ::sensor_msgs::SetCameraInfoRequest_ > +{ + static const char* value() + { + return "sensor_msgs/SetCameraInfoRequest"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfoRequest_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::SetCameraInfoRequest_ > +{ + static const char* value() + { + return "\n\ +\n\ +\n\ +\n\ +\n\ +\n\ +\n\ +\n\ +sensor_msgs/CameraInfo camera_info\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/CameraInfo\n\ +# This message defines meta information for a camera. It should be in a\n\ +# camera namespace on topic \"camera_info\" and accompanied by up to five\n\ +# image topics named:\n\ +#\n\ +# image_raw - raw data from the camera driver, possibly Bayer encoded\n\ +# image - monochrome, distorted\n\ +# image_color - color, distorted\n\ +# image_rect - monochrome, rectified\n\ +# image_rect_color - color, rectified\n\ +#\n\ +# The image_pipeline contains packages (image_proc, stereo_image_proc)\n\ +# for producing the four processed image topics from image_raw and\n\ +# camera_info. The meaning of the camera parameters are described in\n\ +# detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.\n\ +#\n\ +# The image_geometry package provides a user-friendly interface to\n\ +# common operations using this meta information. If you want to, e.g.,\n\ +# project a 3d point into image coordinates, we strongly recommend\n\ +# using image_geometry.\n\ +#\n\ +# If the camera is uncalibrated, the matrices D, K, R, P should be left\n\ +# zeroed out. In particular, clients may assume that K[0] == 0.0\n\ +# indicates an uncalibrated camera.\n\ +\n\ +#######################################################################\n\ +# Image acquisition info #\n\ +#######################################################################\n\ +\n\ +# Time of image acquisition, camera coordinate frame ID\n\ +Header header # Header timestamp should be acquisition time of image\n\ + # Header frame_id should be optical frame of camera\n\ + # origin of frame should be optical center of camera\n\ + # +x should point to the right in the image\n\ + # +y should point down in the image\n\ + # +z should point into the plane of the image\n\ +\n\ +\n\ +#######################################################################\n\ +# Calibration Parameters #\n\ +#######################################################################\n\ +# These are fixed during camera calibration. Their values will be the #\n\ +# same in all messages until the camera is recalibrated. Note that #\n\ +# self-calibrating systems may \"recalibrate\" frequently. #\n\ +# #\n\ +# The internal parameters can be used to warp a raw (distorted) image #\n\ +# to: #\n\ +# 1. An undistorted image (requires D and K) #\n\ +# 2. A rectified image (requires D, K, R) #\n\ +# The projection matrix P projects 3D points into the rectified image.#\n\ +#######################################################################\n\ +\n\ +# The image dimensions with which the camera was calibrated. Normally\n\ +# this will be the full camera resolution in pixels.\n\ +uint32 height\n\ +uint32 width\n\ +\n\ +# The distortion model used. Supported models are listed in\n\ +# sensor_msgs/distortion_models.h. For most cameras, \"plumb_bob\" - a\n\ +# simple model of radial and tangential distortion - is sufficient.\n\ +string distortion_model\n\ +\n\ +# The distortion parameters, size depending on the distortion model.\n\ +# For \"plumb_bob\", the 5 parameters are: (k1, k2, t1, t2, k3).\n\ +float64[] D\n\ +\n\ +# Intrinsic camera matrix for the raw (distorted) images.\n\ +# [fx 0 cx]\n\ +# K = [ 0 fy cy]\n\ +# [ 0 0 1]\n\ +# Projects 3D points in the camera coordinate frame to 2D pixel\n\ +# coordinates using the focal lengths (fx, fy) and principal point\n\ +# (cx, cy).\n\ +float64[9] K # 3x3 row-major matrix\n\ +\n\ +# Rectification matrix (stereo cameras only)\n\ +# A rotation matrix aligning the camera coordinate system to the ideal\n\ +# stereo image plane so that epipolar lines in both stereo images are\n\ +# parallel.\n\ +float64[9] R # 3x3 row-major matrix\n\ +\n\ +# Projection/camera matrix\n\ +# [fx' 0 cx' Tx]\n\ +# P = [ 0 fy' cy' Ty]\n\ +# [ 0 0 1 0]\n\ +# By convention, this matrix specifies the intrinsic (camera) matrix\n\ +# of the processed (rectified) image. That is, the left 3x3 portion\n\ +# is the normal camera intrinsic matrix for the rectified image.\n\ +# It projects 3D points in the camera coordinate frame to 2D pixel\n\ +# coordinates using the focal lengths (fx', fy') and principal point\n\ +# (cx', cy') - these may differ from the values in K.\n\ +# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will\n\ +# also have R = the identity and P[1:3,1:3] = K.\n\ +# For a stereo pair, the fourth column [Tx Ty 0]' is related to the\n\ +# position of the optical center of the second camera in the first\n\ +# camera's frame. We assume Tz = 0 so both cameras are in the same\n\ +# stereo image plane. The first camera always has Tx = Ty = 0. For\n\ +# the right (second) camera of a horizontal stereo pair, Ty = 0 and\n\ +# Tx = -fx' * B, where B is the baseline between the cameras.\n\ +# Given a 3D point [X Y Z]', the projection (x, y) of the point onto\n\ +# the rectified image is given by:\n\ +# [u v w]' = P * [X Y Z 1]'\n\ +# x = u / w\n\ +# y = v / w\n\ +# This holds for both images of a stereo pair.\n\ +float64[12] P # 3x4 row-major matrix\n\ +\n\ +\n\ +#######################################################################\n\ +# Operational Parameters #\n\ +#######################################################################\n\ +# These define the image region actually captured by the camera #\n\ +# driver. Although they affect the geometry of the output image, they #\n\ +# may be changed freely without recalibrating the camera. #\n\ +#######################################################################\n\ +\n\ +# Binning refers here to any camera setting which combines rectangular\n\ +# neighborhoods of pixels into larger \"super-pixels.\" It reduces the\n\ +# resolution of the output image to\n\ +# (width / binning_x) x (height / binning_y).\n\ +# The default values binning_x = binning_y = 0 is considered the same\n\ +# as binning_x = binning_y = 1 (no subsampling).\n\ +uint32 binning_x\n\ +uint32 binning_y\n\ +\n\ +# Region of interest (subwindow of full camera resolution), given in\n\ +# full resolution (unbinned) image coordinates. A particular ROI\n\ +# always denotes the same window of pixels on the camera sensor,\n\ +# regardless of binning settings.\n\ +# The default setting of roi (all values 0) is considered the same as\n\ +# full resolution (roi.width = width, roi.height = height).\n\ +RegionOfInterest roi\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/RegionOfInterest\n\ +# This message is used to specify a region of interest within an image.\n\ +#\n\ +# When used to specify the ROI setting of the camera when the image was\n\ +# taken, the height and width fields should either match the height and\n\ +# width fields for the associated image; or height = width = 0\n\ +# indicates that the full resolution image was captured.\n\ +\n\ +uint32 x_offset # Leftmost pixel of the ROI\n\ + # (0 if the ROI includes the left edge of the image)\n\ +uint32 y_offset # Topmost pixel of the ROI\n\ + # (0 if the ROI includes the top edge of the image)\n\ +uint32 height # Height of ROI\n\ +uint32 width # Width of ROI\n\ +\n\ +# True if a distinct rectified ROI should be calculated from the \"raw\"\n\ +# ROI in this message. Typically this should be False if the full image\n\ +# is captured (ROI not used), and True if a subwindow is captured (ROI\n\ +# used).\n\ +bool do_rectify\n\ +"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfoRequest_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::SetCameraInfoRequest_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.camera_info); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct SetCameraInfoRequest_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::SetCameraInfoRequest_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::SetCameraInfoRequest_& v) + { + s << indent << "camera_info: "; + s << std::endl; + Printer< ::sensor_msgs::CameraInfo_ >::stream(s, indent + " ", v.camera_info); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_SETCAMERAINFOREQUEST_H diff --git a/thirdparty/ros/include/sensor_msgs/SetCameraInfoResponse.h b/thirdparty/ros/include/sensor_msgs/SetCameraInfoResponse.h new file mode 100644 index 0000000..a1cc27c --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/SetCameraInfoResponse.h @@ -0,0 +1,197 @@ +// Generated by gencpp from file sensor_msgs/SetCameraInfoResponse.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_SETCAMERAINFORESPONSE_H +#define SENSOR_MSGS_MESSAGE_SETCAMERAINFORESPONSE_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sensor_msgs +{ +template +struct SetCameraInfoResponse_ +{ + typedef SetCameraInfoResponse_ Type; + + SetCameraInfoResponse_() + : success(false) + , status_message() { + } + SetCameraInfoResponse_(const ContainerAllocator& _alloc) + : success(false) + , status_message(_alloc) { + (void)_alloc; + } + + + + typedef uint8_t _success_type; + _success_type success; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _status_message_type; + _status_message_type status_message; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoResponse_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoResponse_ const> ConstPtr; + +}; // struct SetCameraInfoResponse_ + +typedef ::sensor_msgs::SetCameraInfoResponse_ > SetCameraInfoResponse; + +typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoResponse > SetCameraInfoResponsePtr; +typedef boost::shared_ptr< ::sensor_msgs::SetCameraInfoResponse const> SetCameraInfoResponseConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::SetCameraInfoResponse_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::SetCameraInfoResponse_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::SetCameraInfoResponse_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::SetCameraInfoResponse_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::SetCameraInfoResponse_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::SetCameraInfoResponse_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::SetCameraInfoResponse_ > + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::SetCameraInfoResponse_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sensor_msgs::SetCameraInfoResponse_ > +{ + static const char* value() + { + return "2ec6f3eff0161f4257b808b12bc830c2"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfoResponse_&) { return value(); } + static const uint64_t static_value1 = 0x2ec6f3eff0161f42ULL; + static const uint64_t static_value2 = 0x57b808b12bc830c2ULL; +}; + +template +struct DataType< ::sensor_msgs::SetCameraInfoResponse_ > +{ + static const char* value() + { + return "sensor_msgs/SetCameraInfoResponse"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfoResponse_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::SetCameraInfoResponse_ > +{ + static const char* value() + { + return "bool success\n\ +string status_message\n\ +\n\ +"; + } + + static const char* value(const ::sensor_msgs::SetCameraInfoResponse_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::SetCameraInfoResponse_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.success); + stream.next(m.status_message); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct SetCameraInfoResponse_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::SetCameraInfoResponse_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::SetCameraInfoResponse_& v) + { + s << indent << "success: "; + Printer::stream(s, indent + " ", v.success); + s << indent << "status_message: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.status_message); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_SETCAMERAINFORESPONSE_H diff --git a/thirdparty/ros/include/sensor_msgs/Temperature.h b/thirdparty/ros/include/sensor_msgs/Temperature.h new file mode 100644 index 0000000..21a336a --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/Temperature.h @@ -0,0 +1,229 @@ +// Generated by gencpp from file sensor_msgs/Temperature.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_TEMPERATURE_H +#define SENSOR_MSGS_MESSAGE_TEMPERATURE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Temperature_ +{ + typedef Temperature_ Type; + + Temperature_() + : header() + , temperature(0.0) + , variance(0.0) { + } + Temperature_(const ContainerAllocator& _alloc) + : header(_alloc) + , temperature(0.0) + , variance(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef double _temperature_type; + _temperature_type temperature; + + typedef double _variance_type; + _variance_type variance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Temperature_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Temperature_ const> ConstPtr; + +}; // struct Temperature_ + +typedef ::sensor_msgs::Temperature_ > Temperature; + +typedef boost::shared_ptr< ::sensor_msgs::Temperature > TemperaturePtr; +typedef boost::shared_ptr< ::sensor_msgs::Temperature const> TemperatureConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Temperature_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Temperature_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::Temperature_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Temperature_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::Temperature_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Temperature_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Temperature_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Temperature_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Temperature_ > +{ + static const char* value() + { + return "ff71b307acdbe7c871a5a6d7ed359100"; + } + + static const char* value(const ::sensor_msgs::Temperature_&) { return value(); } + static const uint64_t static_value1 = 0xff71b307acdbe7c8ULL; + static const uint64_t static_value2 = 0x71a5a6d7ed359100ULL; +}; + +template +struct DataType< ::sensor_msgs::Temperature_ > +{ + static const char* value() + { + return "sensor_msgs/Temperature"; + } + + static const char* value(const ::sensor_msgs::Temperature_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Temperature_ > +{ + static const char* value() + { + return " # Single temperature reading.\n\ +\n\ + Header header # timestamp is the time the temperature was measured\n\ + # frame_id is the location of the temperature reading\n\ +\n\ + float64 temperature # Measurement of the Temperature in Degrees Celsius\n\ +\n\ + float64 variance # 0 is interpreted as variance unknown\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::Temperature_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Temperature_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.temperature); + stream.next(m.variance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Temperature_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Temperature_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Temperature_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + s << indent << "variance: "; + Printer::stream(s, indent + " ", v.variance); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_TEMPERATURE_H diff --git a/thirdparty/ros/include/sensor_msgs/TimeReference.h b/thirdparty/ros/include/sensor_msgs/TimeReference.h new file mode 100644 index 0000000..cebc141 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/TimeReference.h @@ -0,0 +1,229 @@ +// Generated by gencpp from file sensor_msgs/TimeReference.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_TIMEREFERENCE_H +#define SENSOR_MSGS_MESSAGE_TIMEREFERENCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct TimeReference_ +{ + typedef TimeReference_ Type; + + TimeReference_() + : header() + , time_ref() + , source() { + } + TimeReference_(const ContainerAllocator& _alloc) + : header(_alloc) + , time_ref() + , source(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ros::Time _time_ref_type; + _time_ref_type time_ref; + + typedef std::basic_string, typename ContainerAllocator::template rebind::other > _source_type; + _source_type source; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::TimeReference_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::TimeReference_ const> ConstPtr; + +}; // struct TimeReference_ + +typedef ::sensor_msgs::TimeReference_ > TimeReference; + +typedef boost::shared_ptr< ::sensor_msgs::TimeReference > TimeReferencePtr; +typedef boost::shared_ptr< ::sensor_msgs::TimeReference const> TimeReferenceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::TimeReference_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::TimeReference_ >::stream(s, "", v); +return s; +} + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'sensor_msgs': ['/tmp/binarydeb/ros-kinetic-sensor-msgs-1.12.5/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::sensor_msgs::TimeReference_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::TimeReference_ const> + : FalseType + { }; + +template +struct IsMessage< ::sensor_msgs::TimeReference_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::TimeReference_ const> + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::TimeReference_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::TimeReference_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::TimeReference_ > +{ + static const char* value() + { + return "fded64a0265108ba86c3d38fb11c0c16"; + } + + static const char* value(const ::sensor_msgs::TimeReference_&) { return value(); } + static const uint64_t static_value1 = 0xfded64a0265108baULL; + static const uint64_t static_value2 = 0x86c3d38fb11c0c16ULL; +}; + +template +struct DataType< ::sensor_msgs::TimeReference_ > +{ + static const char* value() + { + return "sensor_msgs/TimeReference"; + } + + static const char* value(const ::sensor_msgs::TimeReference_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::TimeReference_ > +{ + static const char* value() + { + return "# Measurement from an external time source not actively synchronized with the system clock.\n\ +\n\ +Header header # stamp is system time for which measurement was valid\n\ + # frame_id is not used \n\ +\n\ +time time_ref # corresponding time from this external source\n\ +string source # (optional) name of time source\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +"; + } + + static const char* value(const ::sensor_msgs::TimeReference_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::TimeReference_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.time_ref); + stream.next(m.source); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TimeReference_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::TimeReference_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::TimeReference_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "time_ref: "; + Printer::stream(s, indent + " ", v.time_ref); + s << indent << "source: "; + Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + " ", v.source); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_TIMEREFERENCE_H diff --git a/thirdparty/ros/include/sensor_msgs/distortion_models.h b/thirdparty/ros/include/sensor_msgs/distortion_models.h new file mode 100644 index 0000000..c42492d --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/distortion_models.h @@ -0,0 +1,50 @@ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, Willow Garage, Inc. +* 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 Willow Garage 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 THE +* COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************/ + +#ifndef SENSOR_MSGS_DISTORTION_MODELS_H +#define SENSOR_MSGS_DISTORTION_MODELS_H + +#include + +namespace sensor_msgs +{ + namespace distortion_models + { + const std::string PLUMB_BOB = "plumb_bob"; + const std::string RATIONAL_POLYNOMIAL = "rational_polynomial"; + } +} + +#endif diff --git a/thirdparty/ros/include/sensor_msgs/fill_image.h b/thirdparty/ros/include/sensor_msgs/fill_image.h new file mode 100644 index 0000000..8795d2f --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/fill_image.h @@ -0,0 +1,70 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 Willow Garage 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 THE +* COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************/ + +#ifndef FILLIMAGE_HH +#define FILLIMAGE_HH + +#include "sensor_msgs/Image.h" +#include "sensor_msgs/image_encodings.h" + +namespace sensor_msgs +{ + + static inline bool fillImage(Image& image, + const std::string& encoding_arg, + uint32_t rows_arg, + uint32_t cols_arg, + uint32_t step_arg, + const void* data_arg) + { + image.encoding = encoding_arg; + image.height = rows_arg; + image.width = cols_arg; + image.step = step_arg; + size_t st0 = (step_arg * rows_arg); + image.data.resize(st0); + memcpy(&image.data[0], data_arg, st0); + + image.is_bigendian = 0; + return true; + } + + static inline void clearImage(Image& image) + { + image.data.resize(0); + } +} + + +#endif diff --git a/thirdparty/ros/include/sensor_msgs/image_encodings.h b/thirdparty/ros/include/sensor_msgs/image_encodings.h new file mode 100644 index 0000000..0180491 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/image_encodings.h @@ -0,0 +1,232 @@ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* 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 Willow Garage 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 THE +* COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************/ + +#ifndef SENSOR_MSGS_IMAGE_ENCODINGS_H +#define SENSOR_MSGS_IMAGE_ENCODINGS_H + +#include +#include + +namespace sensor_msgs +{ + namespace image_encodings + { + const std::string RGB8 = "rgb8"; + const std::string RGBA8 = "rgba8"; + const std::string RGB16 = "rgb16"; + const std::string RGBA16 = "rgba16"; + const std::string BGR8 = "bgr8"; + const std::string BGRA8 = "bgra8"; + const std::string BGR16 = "bgr16"; + const std::string BGRA16 = "bgra16"; + const std::string MONO8="mono8"; + const std::string MONO16="mono16"; + + // OpenCV CvMat types + const std::string TYPE_8UC1="8UC1"; + const std::string TYPE_8UC2="8UC2"; + const std::string TYPE_8UC3="8UC3"; + const std::string TYPE_8UC4="8UC4"; + const std::string TYPE_8SC1="8SC1"; + const std::string TYPE_8SC2="8SC2"; + const std::string TYPE_8SC3="8SC3"; + const std::string TYPE_8SC4="8SC4"; + const std::string TYPE_16UC1="16UC1"; + const std::string TYPE_16UC2="16UC2"; + const std::string TYPE_16UC3="16UC3"; + const std::string TYPE_16UC4="16UC4"; + const std::string TYPE_16SC1="16SC1"; + const std::string TYPE_16SC2="16SC2"; + const std::string TYPE_16SC3="16SC3"; + const std::string TYPE_16SC4="16SC4"; + const std::string TYPE_32SC1="32SC1"; + const std::string TYPE_32SC2="32SC2"; + const std::string TYPE_32SC3="32SC3"; + const std::string TYPE_32SC4="32SC4"; + const std::string TYPE_32FC1="32FC1"; + const std::string TYPE_32FC2="32FC2"; + const std::string TYPE_32FC3="32FC3"; + const std::string TYPE_32FC4="32FC4"; + const std::string TYPE_64FC1="64FC1"; + const std::string TYPE_64FC2="64FC2"; + const std::string TYPE_64FC3="64FC3"; + const std::string TYPE_64FC4="64FC4"; + + // Bayer encodings + const std::string BAYER_RGGB8="bayer_rggb8"; + const std::string BAYER_BGGR8="bayer_bggr8"; + const std::string BAYER_GBRG8="bayer_gbrg8"; + const std::string BAYER_GRBG8="bayer_grbg8"; + const std::string BAYER_RGGB16="bayer_rggb16"; + const std::string BAYER_BGGR16="bayer_bggr16"; + const std::string BAYER_GBRG16="bayer_gbrg16"; + const std::string BAYER_GRBG16="bayer_grbg16"; + + // Miscellaneous + // This is the UYVY version of YUV422 codec http://www.fourcc.org/yuv.php#UYVY + // with an 8-bit depth + const std::string YUV422="yuv422"; + + // Prefixes for abstract image encodings + const std::string ABSTRACT_ENCODING_PREFIXES[] = { + "8UC", "8SC", "16UC", "16SC", "32SC", "32FC", "64FC"}; + + // Utility functions for inspecting an encoding string + static inline bool isColor(const std::string& encoding) + { + return encoding == RGB8 || encoding == BGR8 || + encoding == RGBA8 || encoding == BGRA8 || + encoding == RGB16 || encoding == BGR16 || + encoding == RGBA16 || encoding == BGRA16; + } + + static inline bool isMono(const std::string& encoding) + { + return encoding == MONO8 || encoding == MONO16; + } + + static inline bool isBayer(const std::string& encoding) + { + return encoding == BAYER_RGGB8 || encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || encoding == BAYER_GRBG8 || + encoding == BAYER_RGGB16 || encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || encoding == BAYER_GRBG16; + } + + static inline bool hasAlpha(const std::string& encoding) + { + return encoding == RGBA8 || encoding == BGRA8 || + encoding == RGBA16 || encoding == BGRA16; + } + + static inline int numChannels(const std::string& encoding) + { + // First do the common-case encodings + if (encoding == MONO8 || + encoding == MONO16) + return 1; + if (encoding == BGR8 || + encoding == RGB8 || + encoding == BGR16 || + encoding == RGB16) + return 3; + if (encoding == BGRA8 || + encoding == RGBA8 || + encoding == BGRA16 || + encoding == RGBA16) + return 4; + if (encoding == BAYER_RGGB8 || + encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || + encoding == BAYER_GRBG8 || + encoding == BAYER_RGGB16 || + encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || + encoding == BAYER_GRBG16) + return 1; + + // Now all the generic content encodings + // TODO: Rewrite with regex when ROS supports C++11 + for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++) + { + std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; + if (encoding.substr(0, prefix.size()) != prefix) + continue; + if (encoding.size() == prefix.size()) + return 1; // ex. 8UC -> 1 + int n_channel = atoi(encoding.substr(prefix.size(), + encoding.size() - prefix.size()).c_str()); // ex. 8UC5 -> 5 + if (n_channel != 0) + return n_channel; // valid encoding string + } + + if (encoding == YUV422) + return 2; + + throw std::runtime_error("Unknown encoding " + encoding); + return -1; + } + + static inline int bitDepth(const std::string& encoding) + { + if (encoding == MONO16) + return 16; + if (encoding == MONO8 || + encoding == BGR8 || + encoding == RGB8 || + encoding == BGRA8 || + encoding == RGBA8 || + encoding == BAYER_RGGB8 || + encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || + encoding == BAYER_GRBG8) + return 8; + + if (encoding == MONO16 || + encoding == BGR16 || + encoding == RGB16 || + encoding == BGRA16 || + encoding == RGBA16 || + encoding == BAYER_RGGB16 || + encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || + encoding == BAYER_GRBG16) + return 16; + + // Now all the generic content encodings + // TODO: Rewrite with regex when ROS supports C++11 + for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++) + { + std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; + if (encoding.substr(0, prefix.size()) != prefix) + continue; + if (encoding.size() == prefix.size()) + return atoi(prefix.c_str()); // ex. 8UC -> 8 + int n_channel = atoi(encoding.substr(prefix.size(), + encoding.size() - prefix.size()).c_str()); // ex. 8UC10 -> 10 + if (n_channel != 0) + return atoi(prefix.c_str()); // valid encoding string + } + + if (encoding == YUV422) + return 8; + + throw std::runtime_error("Unknown encoding " + encoding); + return -1; + } + } +} + +#endif diff --git a/thirdparty/ros/include/sensor_msgs/point_cloud2_iterator.h b/thirdparty/ros/include/sensor_msgs/point_cloud2_iterator.h new file mode 100644 index 0000000..5b9c7a1 --- /dev/null +++ b/thirdparty/ros/include/sensor_msgs/point_cloud2_iterator.h @@ -0,0 +1,302 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * 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 Open Source Robotics Foundation 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 THE + * COPYRIGHT OWNER OR CONTRIBUTORS 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. + */ + +#ifndef SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H +#define SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H + +#include +#include +#include +#include + +/** + * \brief Tools for manipulating sensor_msgs + * + * This file provides two sets of utilities to modify and parse PointCloud2 + * The first set allows you to conveniently set the fields by hand: + * 
+ *   #include 
+ *   // Create a PointCloud2
+ *   sensor_msgs::PointCloud2 cloud_msg;
+ *   // Fill some internals of the PoinCloud2 like the header/width/height ...
+ *   cloud_msgs.height = 1;  cloud_msgs.width = 4;
+ *   // Set the point fields to xyzrgb and resize the vector with the following command
+ *   // 4 is for the number of added fields. Each come in triplet: the name of the PointField,
+ *   // the number of occurrences of the type in the PointField, the type of the PointField
+ *   sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
+ *   modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
+ *                                            "y", 1, sensor_msgs::PointField::FLOAT32,
+ *                                            "z", 1, sensor_msgs::PointField::FLOAT32,
+ *                                            "rgb", 1, sensor_msgs::PointField::FLOAT32);
+ *   // For convenience and the xyz, rgb, rgba fields, you can also use the following overloaded function.
+ *   // You have to be aware that the following function does add extra padding for backward compatibility though
+ *   // so it is definitely the solution of choice for PointXYZ and PointXYZRGB
+ *   // 2 is for the number of fields to add
+ *   modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+ *   // You can then reserve / resize as usual
+ *   modifier.resize(100);
+ *
+ * + * The second set allow you to traverse your PointCloud using an iterator: + * 
+ *   // Define some raw data we'll put in the PointCloud2
+ *   float point_data[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0};
+ *   uint8_t color_data[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
+ *   // Define the iterators. When doing so, you define the Field you would like to iterate upon and
+ *   // the type of you would like returned: it is not necessary the type of the PointField as sometimes
+ *   // you pack data in another type (e.g. 3 uchar + 1 uchar for RGB are packed in a float)
+ *   sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x");
+ *   sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y");
+ *   sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z");
+ *   // Even though the r,g,b,a fields do not exist (it's usually rgb, rgba), you can create iterators for
+ *   // those: they will handle data packing for you (in little endian RGB is packed as *,R,G,B in a float
+ *   // and RGBA as A,R,G,B)
+ *   sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r");
+ *   sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g");
+ *   sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b");
+ *   // Fill the PointCloud2
+ *   for(size_t i=0; i
+ */
+
+namespace sensor_msgs
+{
+/**
+ * @brief Enables modifying a sensor_msgs::PointCloud2 like a container
+ */
+class PointCloud2Modifier
+{
+public:
+  /**
+   * @brief Default constructor
+   * @param cloud_msg The sensor_msgs::PointCloud2 to modify
+   */
+  PointCloud2Modifier(PointCloud2& cloud_msg);
+
+  /**
+   * @return the number of T's in the original sensor_msgs::PointCloud2
+   */
+  size_t size() const;
+
+  /**
+   * @param size The number of T's to reserve in the original sensor_msgs::PointCloud2 for
+   */
+  void reserve(size_t size);
+
+  /**
+   * @param size The number of T's to change the size of the original sensor_msgs::PointCloud2 by
+   */
+  void resize(size_t size);
+
+  /**
+   * @brief remove all T's from the original sensor_msgs::PointCloud2
+   */
+  void clear();
+
+  /**
+   * @brief Function setting some fields in a PointCloud and adjusting the
+   *        internals of the PointCloud2
+   * @param n_fields the number of fields to add. The fields are given as
+   *        triplets: name of the field as char*, number of elements in the
+   *        field, the datatype of the elements in the field
+   *
+   * E.g, you create your PointCloud2 message with XYZ/RGB as follows:
+   * 
+   *   setPointCloud2Fields(cloud_msg, 4, "x", 1, sensor_msgs::PointField::FLOAT32,
+   *                                              "y", 1, sensor_msgs::PointField::FLOAT32,
+   *                                              "z", 1, sensor_msgs::PointField::FLOAT32,
+   *                                              "rgb", 1, sensor_msgs::PointField::FLOAT32);
+   * 

+   * WARNING: THIS DOES NOT TAKE INTO ACCOUNT ANY PADDING AS DONE UNTIL HYDRO
+   * For simple usual cases, the overloaded setPointCloud2FieldsByString is what you want.
+   */
+  void setPointCloud2Fields(int n_fields, ...);
+
+  /**
+   * @brief Function setting some fields in a PointCloud and adjusting the
+   *        internals of the PointCloud2
+   * @param n_fields the number of fields to add. The fields are given as
+   *        strings: "xyz" (3 floats), "rgb" (3 uchar stacked in a float),
+   *        "rgba" (4 uchar stacked in a float)
+   * @return void
+   *
+   * WARNING: THIS FUNCTION DOES ADD ANY NECESSARY PADDING TRANSPARENTLY
+   */
+  void setPointCloud2FieldsByString(int n_fields, ...);
+protected:
+  /** A reference to the original sensor_msgs::PointCloud2 that we read */
+  PointCloud2& cloud_msg_;
+};
+
+namespace impl
+{
+/** Private base class for PointCloud2Iterator and PointCloud2ConstIterator
+ * T is the type of the value on which the child class will be templated
+ * TT is the type of the value to be retrieved (same as T except for constness)
+ * U is the type of the raw data in PointCloud2 (only uchar and const uchar are supported)
+ * C is the type of the pointcloud to intialize from (const or not)
+ * V is the derived class (yop, curiously recurring template pattern)
+ */
+template class V>
+class PointCloud2IteratorBase
+{
+public:
+  /**
+   */
+  PointCloud2IteratorBase();
+
+  /**
+   * @param cloud_msg The PointCloud2 to iterate upon
+   * @param field_name The field to iterate upon
+   */
+  PointCloud2IteratorBase(C &cloud_msg, const std::string &field_name);
+
+  /** Assignment operator
+   * @param iter the iterator to copy data from
+   * @return a reference to *this
+   */
+  V& operator =(const V& iter);
+
+  /** Access the i th element starting at the current pointer (useful when a field has several elements of the same
+   * type)
+   * @param i
+   * @return a reference to the i^th value from the current position
+   */
+  TT& operator [](size_t i) const;
+
+  /** Dereference the iterator. Equivalent to accessing it through [0]
+   * @return the value to which the iterator is pointing
+   */
+  TT& operator *() const;
+
+  /** Increase the iterator to the next element
+   * @return a reference to the updated iterator
+   */
+  V& operator ++();
+
+  /** Basic pointer addition
+   * @param i the amount to increase the iterator by
+   * @return an iterator with an increased position
+   */
+  V operator +(int i);
+
+  /** Increase the iterator by a certain amount
+   * @return a reference to the updated iterator
+   */
+  V& operator +=(int i);
+
+  /** Compare to another iterator
+   * @return whether the current iterator points to a different address than the other one
+   */
+  bool operator !=(const V& iter) const;
+
+  /** Return the end iterator
+   * @return the end iterator (useful when performing normal iterator processing with ++)
+   */
+  V end() const;
+
+private:
+  /** Common code to set the field of the PointCloud2
+   * @param cloud_msg the PointCloud2 to modify
+   * @param field_name the name of the field to iterate upon
+   * @return the offset at which the field is found
+   */
+  int set_field(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name);
+
+  /** The "point_step" of the original cloud */
+  int point_step_;
+  /** The raw data  in uchar* where the iterator is */
+  U* data_char_;
+  /** The cast data where the iterator is */
+  TT* data_;
+  /** The end() pointer of the iterator */
+  TT* data_end_;
+  /** Whether the fields are stored as bigendian */
+  bool is_bigendian_;
+};
+}
+
+/**
+ * \brief Class that can iterate over a PointCloud2
+ *
+ * T type of the element being iterated upon
+ * E.g, you create your PointClou2 message as follows:
+ * 
+ *   setPointCloud2FieldsByString(cloud_msg, 2, "xyz", "rgb");
+ * 

+ *
+ * For iterating over XYZ, you do :
+ * 
+ *   sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x");
+ * 

+ * and then access X through iter_x[0] or *iter_x
+ * You could create an iterator for Y and Z too but as they are consecutive,
+ * you can just use iter_x[1] and iter_x[2]
+ *
+ * For iterating over RGB, you do:
+ * 
+ * sensor_msgs::PointCloud2Iterator iter_rgb(cloud_msg, "rgb");
+ * 

+ * and then access R,G,B through  iter_rgb[0], iter_rgb[1], iter_rgb[2]
+ */
+template
+class PointCloud2Iterator : public impl::PointCloud2IteratorBase
+{
+public:
+  PointCloud2Iterator(sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) :
+    impl::PointCloud2IteratorBase::PointCloud2IteratorBase(cloud_msg, field_name) {}
+};
+
+/**
+ * \brief Same as a PointCloud2Iterator but for const data
+ */
+template
+class PointCloud2ConstIterator : public impl::PointCloud2IteratorBase
+{
+public:
+  PointCloud2ConstIterator(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) :
+    impl::PointCloud2IteratorBase::PointCloud2IteratorBase(cloud_msg, field_name) {}
+};
+}
+
+#include 
+
+#endif// SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H
diff --git a/thirdparty/ros/include/sensor_msgs/point_cloud_conversion.h b/thirdparty/ros/include/sensor_msgs/point_cloud_conversion.h
new file mode 100644
index 0000000..439145d
--- /dev/null
+++ b/thirdparty/ros/include/sensor_msgs/point_cloud_conversion.h
@@ -0,0 +1,169 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
+#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
+
+#include 
+#include 
+#include 
+
+/** 
+  * \brief Convert between the old (sensor_msgs::PointCloud) and the new (sensor_msgs::PointCloud2) format.
+  * \author Radu Bogdan Rusu
+  */
+namespace sensor_msgs
+{
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Get the index of a specified field (i.e., dimension/channel)
+    * \param points the the point cloud message
+    * \param field_name the string defining the field name
+    */
+static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
+{
+  // Get the index we need
+  for (size_t d = 0; d < cloud.fields.size (); ++d)
+    if (cloud.fields[d].name == field_name)
+      return (d);
+  return (-1);
+}
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Convert a sensor_msgs::PointCloud message to a sensor_msgs::PointCloud2 message.
+    * \param input the message in the sensor_msgs::PointCloud format
+    * \param output the resultant message in the sensor_msgs::PointCloud2 format
+    */ 
+static inline bool convertPointCloudToPointCloud2 (const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
+{
+  output.header = input.header;
+  output.width  = input.points.size ();
+  output.height = 1;
+  output.fields.resize (3 + input.channels.size ());
+  // Convert x/y/z to fields
+  output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
+  int offset = 0;
+  // All offsets are *4, as all field data types are float32
+  for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
+  {
+    output.fields[d].offset = offset;
+    output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
+    output.fields[d].count  = 1;
+  }
+  output.point_step = offset;
+  output.row_step   = output.point_step * output.width;
+  // Convert the remaining of the channels to fields
+  for (size_t d = 0; d < input.channels.size (); ++d)
+    output.fields[3 + d].name = input.channels[d].name;
+  output.data.resize (input.points.size () * output.point_step);
+  output.is_bigendian = false;  // @todo ?
+  output.is_dense     = false;
+
+  // Copy the data points
+  for (size_t cp = 0; cp < input.points.size (); ++cp)
+  {
+    memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
+    memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
+    memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
+    for (size_t d = 0; d < input.channels.size (); ++d)
+    {
+      if (input.channels[d].values.size() == input.points.size())
+      {
+        memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
+      }
+    }
+  }
+  return (true);
+}
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Convert a sensor_msgs::PointCloud2 message to a sensor_msgs::PointCloud message.
+    * \param input the message in the sensor_msgs::PointCloud2 format
+    * \param output the resultant message in the sensor_msgs::PointCloud format
+    */ 
+static inline bool convertPointCloud2ToPointCloud (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
+{
+
+  output.header = input.header;
+  output.points.resize (input.width * input.height);
+  output.channels.resize (input.fields.size () - 3);
+  // Get the x/y/z field offsets
+  int x_idx = getPointCloud2FieldIndex (input, "x");
+  int y_idx = getPointCloud2FieldIndex (input, "y");
+  int z_idx = getPointCloud2FieldIndex (input, "z");
+  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
+  {
+    std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
+    return (false);
+  }
+  int x_offset = input.fields[x_idx].offset;
+  int y_offset = input.fields[y_idx].offset;
+  int z_offset = input.fields[z_idx].offset;
+  uint8_t x_datatype = input.fields[x_idx].datatype;
+  uint8_t y_datatype = input.fields[y_idx].datatype;
+  uint8_t z_datatype = input.fields[z_idx].datatype;
+   
+  // Convert the fields to channels
+  int cur_c = 0;
+  for (size_t d = 0; d < input.fields.size (); ++d)
+  {
+    if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
+      continue;
+    output.channels[cur_c].name = input.fields[d].name;
+    output.channels[cur_c].values.resize (output.points.size ());
+    cur_c++;
+  }
+
+  // Copy the data points
+  for (size_t cp = 0; cp < output.points.size (); ++cp)
+  {
+    // Copy x/y/z
+    output.points[cp].x = sensor_msgs::readPointCloud2BufferValue(&input.data[cp * input.point_step + x_offset], x_datatype);
+    output.points[cp].y = sensor_msgs::readPointCloud2BufferValue(&input.data[cp * input.point_step + y_offset], y_datatype);
+    output.points[cp].z = sensor_msgs::readPointCloud2BufferValue(&input.data[cp * input.point_step + z_offset], z_datatype);
+    // Copy the rest of the data
+    int cur_c = 0;
+    for (size_t d = 0; d < input.fields.size (); ++d)
+    {
+      if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
+        continue;
+      output.channels[cur_c++].values[cp] = sensor_msgs::readPointCloud2BufferValue(&input.data[cp * input.point_step + input.fields[d].offset], input.fields[d].datatype);
+    }
+  }
+  return (true);
+}
+}
+#endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
diff --git a/thirdparty/ros/include/sensor_msgs/point_field_conversion.h b/thirdparty/ros/include/sensor_msgs/point_field_conversion.h
new file mode 100644
index 0000000..473b853
--- /dev/null
+++ b/thirdparty/ros/include/sensor_msgs/point_field_conversion.h
@@ -0,0 +1,178 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Robot Operating System code by the University of Osnabrück
+ * Copyright (c) 2015, University of Osnabrück
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   1. Redistributions of source code must retain the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. 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.
+ *
+ *   3. Neither the name of the copyright holder 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 THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS 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.
+ *
+ *
+ *
+ * point_field_conversion.h
+ *
+ * Created on: 16.07.2015
+ *  Authors: Sebastian Pütz 
+ */
+
+#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H
+#define SENSOR_MSGS_POINT_FIELD_CONVERSION_H
+
+/** 
+  * \brief  This file provides a type to enum mapping for the different
+  *         PointField types and methods to read and write in 
+  *         a PointCloud2 buffer for the different PointField types.
+  * \author Sebastian Pütz
+  */
+namespace sensor_msgs{
+  /*!
+   * \Enum to type mapping.
+   */
+  template struct pointFieldTypeAsType {};
+  template<> struct pointFieldTypeAsType    { typedef int8_t   type; };
+  template<> struct pointFieldTypeAsType   { typedef uint8_t  type; };
+  template<> struct pointFieldTypeAsType   { typedef int16_t  type; };
+  template<> struct pointFieldTypeAsType  { typedef uint16_t type; };
+  template<> struct pointFieldTypeAsType   { typedef int32_t  type; };
+  template<> struct pointFieldTypeAsType  { typedef uint32_t type; };
+  template<> struct pointFieldTypeAsType { typedef float    type; };
+  template<> struct pointFieldTypeAsType { typedef double   type; };
+  
+  /*!
+   * \Type to enum mapping.
+   */
+  template struct typeAsPointFieldType {};
+  template<> struct typeAsPointFieldType   { static const uint8_t value = sensor_msgs::PointField::INT8;    };
+  template<> struct typeAsPointFieldType  { static const uint8_t value = sensor_msgs::PointField::UINT8;   };
+  template<> struct typeAsPointFieldType  { static const uint8_t value = sensor_msgs::PointField::INT16;   };
+  template<> struct typeAsPointFieldType { static const uint8_t value = sensor_msgs::PointField::UINT16;  };
+  template<> struct typeAsPointFieldType  { static const uint8_t value = sensor_msgs::PointField::INT32;   };
+  template<> struct typeAsPointFieldType { static const uint8_t value = sensor_msgs::PointField::UINT32;  };
+  template<> struct typeAsPointFieldType    { static const uint8_t value = sensor_msgs::PointField::FLOAT32; };
+  template<> struct typeAsPointFieldType   { static const uint8_t value = sensor_msgs::PointField::FLOAT64; };
+ 
+  /*!
+   * \Converts a value at the given pointer position, interpreted as the datatype
+   *  specified by the given template argument point_field_type, to the given
+   *  template type T and returns it.
+   * \param data_ptr            pointer into the point cloud 2 buffer
+   * \tparam point_field_type   sensor_msgs::PointField datatype value
+   * \tparam T                  return type
+   */
+  template
+    inline T readPointCloud2BufferValue(const unsigned char* data_ptr){
+      typedef typename pointFieldTypeAsType::type type;
+      return static_cast(*(reinterpret_cast(data_ptr)));
+    }
+  
+  /*!
+   * \Converts a value at the given pointer position interpreted as the datatype
+   *  specified by the given datatype parameter to the given template type and returns it.
+   * \param data_ptr    pointer into the point cloud 2 buffer
+   * \param datatype    sensor_msgs::PointField datatype value
+   * \tparam T          return type
+   */
+  template
+    inline T readPointCloud2BufferValue(const unsigned char* data_ptr, const unsigned char datatype){
+      switch(datatype){
+        case sensor_msgs::PointField::INT8:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::UINT8:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::INT16:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::UINT16:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::INT32:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::UINT32:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::FLOAT32:
+          return readPointCloud2BufferValue(data_ptr);
+        case sensor_msgs::PointField::FLOAT64:
+          return readPointCloud2BufferValue(data_ptr);
+      }
+    }
+
+  /*!
+   * \Inserts a given value at the given point position interpreted as the datatype
+   *  specified by the template argument point_field_type.
+   * \param data_ptr            pointer into the point cloud 2 buffer
+   * \param value               the value to insert
+   * \tparam point_field_type   sensor_msgs::PointField datatype value
+   * \tparam T                  type of the value to insert
+   */
+  template
+    inline void writePointCloud2BufferValue(unsigned char* data_ptr, T value){
+      typedef typename pointFieldTypeAsType::type type;
+      *(reinterpret_cast(data_ptr)) = static_cast(value);
+    }
+
+  /*!
+   * \Inserts a given value at the given point position interpreted as the datatype
+   *  specified by the given datatype parameter.
+   * \param data_ptr    pointer into the point cloud 2 buffer
+   * \param datatype    sensor_msgs::PointField datatype value
+   * \param value       the value to insert
+   * \tparam T          type of the value to insert
+   */
+  template
+    inline void writePointCloud2BufferValue(unsigned char* data_ptr, const unsigned char datatype, T value){
+      switch(datatype){
+        case sensor_msgs::PointField::INT8:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::UINT8:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::INT16:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::UINT16:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::INT32:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::UINT32:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::FLOAT32:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::FLOAT64:
+          writePointCloud2BufferValue(data_ptr, value);
+          break;
+      }
+    }
+}
+
+#endif /* point_field_conversion.h */
diff --git a/thirdparty/ros/include/std_msgs/Bool.h b/thirdparty/ros/include/std_msgs/Bool.h
new file mode 100644
index 0000000..951197f
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Bool.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Bool.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_BOOL_H
+#define STD_MSGS_MESSAGE_BOOL_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Bool_
+{
+  typedef Bool_ Type;
+
+  Bool_()
+    : data(false)  {
+    }
+  Bool_(const ContainerAllocator& _alloc)
+    : data(false)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Bool_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Bool_ const> ConstPtr;
+
+}; // struct Bool_
+
+typedef ::std_msgs::Bool_ > Bool;
+
+typedef boost::shared_ptr< ::std_msgs::Bool > BoolPtr;
+typedef boost::shared_ptr< ::std_msgs::Bool const> BoolConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Bool_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Bool_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Bool_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Bool_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Bool_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Bool_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Bool_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Bool_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Bool_ >
+{
+  static const char* value()
+  {
+    return "8b94c1b53db61fb6aed406028ad6332a";
+  }
+
+  static const char* value(const ::std_msgs::Bool_&) { return value(); }
+  static const uint64_t static_value1 = 0x8b94c1b53db61fb6ULL;
+  static const uint64_t static_value2 = 0xaed406028ad6332aULL;
+};
+
+template
+struct DataType< ::std_msgs::Bool_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Bool";
+  }
+
+  static const char* value(const ::std_msgs::Bool_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Bool_ >
+{
+  static const char* value()
+  {
+    return "bool data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Bool_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Bool_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Bool_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Bool_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Bool_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_BOOL_H
diff --git a/thirdparty/ros/include/std_msgs/Byte.h b/thirdparty/ros/include/std_msgs/Byte.h
new file mode 100644
index 0000000..2f131e8
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Byte.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Byte.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_BYTE_H
+#define STD_MSGS_MESSAGE_BYTE_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Byte_
+{
+  typedef Byte_ Type;
+
+  Byte_()
+    : data(0)  {
+    }
+  Byte_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef int8_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Byte_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Byte_ const> ConstPtr;
+
+}; // struct Byte_
+
+typedef ::std_msgs::Byte_ > Byte;
+
+typedef boost::shared_ptr< ::std_msgs::Byte > BytePtr;
+typedef boost::shared_ptr< ::std_msgs::Byte const> ByteConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Byte_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Byte_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Byte_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Byte_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Byte_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Byte_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Byte_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Byte_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Byte_ >
+{
+  static const char* value()
+  {
+    return "ad736a2e8818154c487bb80fe42ce43b";
+  }
+
+  static const char* value(const ::std_msgs::Byte_&) { return value(); }
+  static const uint64_t static_value1 = 0xad736a2e8818154cULL;
+  static const uint64_t static_value2 = 0x487bb80fe42ce43bULL;
+};
+
+template
+struct DataType< ::std_msgs::Byte_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Byte";
+  }
+
+  static const char* value(const ::std_msgs::Byte_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Byte_ >
+{
+  static const char* value()
+  {
+    return "byte data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Byte_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Byte_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Byte_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Byte_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Byte_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_BYTE_H
diff --git a/thirdparty/ros/include/std_msgs/ByteMultiArray.h b/thirdparty/ros/include/std_msgs/ByteMultiArray.h
new file mode 100644
index 0000000..0594ad2
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/ByteMultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/ByteMultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_BYTEMULTIARRAY_H
+#define STD_MSGS_MESSAGE_BYTEMULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct ByteMultiArray_
+{
+  typedef ByteMultiArray_ Type;
+
+  ByteMultiArray_()
+    : layout()
+    , data()  {
+    }
+  ByteMultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::ByteMultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::ByteMultiArray_ const> ConstPtr;
+
+}; // struct ByteMultiArray_
+
+typedef ::std_msgs::ByteMultiArray_ > ByteMultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::ByteMultiArray > ByteMultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::ByteMultiArray const> ByteMultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::ByteMultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::ByteMultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::ByteMultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::ByteMultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::ByteMultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::ByteMultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::ByteMultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::ByteMultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::ByteMultiArray_ >
+{
+  static const char* value()
+  {
+    return "70ea476cbcfd65ac2f68f3cda1e891fe";
+  }
+
+  static const char* value(const ::std_msgs::ByteMultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x70ea476cbcfd65acULL;
+  static const uint64_t static_value2 = 0x2f68f3cda1e891feULL;
+};
+
+template
+struct DataType< ::std_msgs::ByteMultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/ByteMultiArray";
+  }
+
+  static const char* value(const ::std_msgs::ByteMultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::ByteMultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+byte[]            data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::ByteMultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::ByteMultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct ByteMultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::ByteMultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::ByteMultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_BYTEMULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/Char.h b/thirdparty/ros/include/std_msgs/Char.h
new file mode 100644
index 0000000..c743fcc
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Char.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Char.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_CHAR_H
+#define STD_MSGS_MESSAGE_CHAR_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Char_
+{
+  typedef Char_ Type;
+
+  Char_()
+    : data(0)  {
+    }
+  Char_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Char_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Char_ const> ConstPtr;
+
+}; // struct Char_
+
+typedef ::std_msgs::Char_ > Char;
+
+typedef boost::shared_ptr< ::std_msgs::Char > CharPtr;
+typedef boost::shared_ptr< ::std_msgs::Char const> CharConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Char_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Char_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Char_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Char_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Char_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Char_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Char_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Char_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Char_ >
+{
+  static const char* value()
+  {
+    return "1bf77f25acecdedba0e224b162199717";
+  }
+
+  static const char* value(const ::std_msgs::Char_&) { return value(); }
+  static const uint64_t static_value1 = 0x1bf77f25acecdedbULL;
+  static const uint64_t static_value2 = 0xa0e224b162199717ULL;
+};
+
+template
+struct DataType< ::std_msgs::Char_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Char";
+  }
+
+  static const char* value(const ::std_msgs::Char_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Char_ >
+{
+  static const char* value()
+  {
+    return "char data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Char_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Char_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Char_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Char_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Char_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_CHAR_H
diff --git a/thirdparty/ros/include/std_msgs/ColorRGBA.h b/thirdparty/ros/include/std_msgs/ColorRGBA.h
new file mode 100644
index 0000000..8dcef5b
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/ColorRGBA.h
@@ -0,0 +1,214 @@
+// Generated by gencpp from file std_msgs/ColorRGBA.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_COLORRGBA_H
+#define STD_MSGS_MESSAGE_COLORRGBA_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct ColorRGBA_
+{
+  typedef ColorRGBA_ Type;
+
+  ColorRGBA_()
+    : r(0.0)
+    , g(0.0)
+    , b(0.0)
+    , a(0.0)  {
+    }
+  ColorRGBA_(const ContainerAllocator& _alloc)
+    : r(0.0)
+    , g(0.0)
+    , b(0.0)
+    , a(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef float _r_type;
+  _r_type r;
+
+   typedef float _g_type;
+  _g_type g;
+
+   typedef float _b_type;
+  _b_type b;
+
+   typedef float _a_type;
+  _a_type a;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::ColorRGBA_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::ColorRGBA_ const> ConstPtr;
+
+}; // struct ColorRGBA_
+
+typedef ::std_msgs::ColorRGBA_ > ColorRGBA;
+
+typedef boost::shared_ptr< ::std_msgs::ColorRGBA > ColorRGBAPtr;
+typedef boost::shared_ptr< ::std_msgs::ColorRGBA const> ColorRGBAConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::ColorRGBA_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::ColorRGBA_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::ColorRGBA_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::ColorRGBA_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::ColorRGBA_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::ColorRGBA_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::ColorRGBA_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::ColorRGBA_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::ColorRGBA_ >
+{
+  static const char* value()
+  {
+    return "a29a96539573343b1310c73607334b00";
+  }
+
+  static const char* value(const ::std_msgs::ColorRGBA_&) { return value(); }
+  static const uint64_t static_value1 = 0xa29a96539573343bULL;
+  static const uint64_t static_value2 = 0x1310c73607334b00ULL;
+};
+
+template
+struct DataType< ::std_msgs::ColorRGBA_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/ColorRGBA";
+  }
+
+  static const char* value(const ::std_msgs::ColorRGBA_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::ColorRGBA_ >
+{
+  static const char* value()
+  {
+    return "float32 r\n\
+float32 g\n\
+float32 b\n\
+float32 a\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::ColorRGBA_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::ColorRGBA_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.r);
+      stream.next(m.g);
+      stream.next(m.b);
+      stream.next(m.a);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct ColorRGBA_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::ColorRGBA_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::ColorRGBA_& v)
+  {
+    s << indent << "r: ";
+    Printer::stream(s, indent + "  ", v.r);
+    s << indent << "g: ";
+    Printer::stream(s, indent + "  ", v.g);
+    s << indent << "b: ";
+    Printer::stream(s, indent + "  ", v.b);
+    s << indent << "a: ";
+    Printer::stream(s, indent + "  ", v.a);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_COLORRGBA_H
diff --git a/thirdparty/ros/include/std_msgs/Duration.h b/thirdparty/ros/include/std_msgs/Duration.h
new file mode 100644
index 0000000..40319b5
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Duration.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Duration.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_DURATION_H
+#define STD_MSGS_MESSAGE_DURATION_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Duration_
+{
+  typedef Duration_ Type;
+
+  Duration_()
+    : data()  {
+    }
+  Duration_(const ContainerAllocator& _alloc)
+    : data()  {
+  (void)_alloc;
+    }
+
+
+
+   typedef ros::Duration _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Duration_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Duration_ const> ConstPtr;
+
+}; // struct Duration_
+
+typedef ::std_msgs::Duration_ > Duration;
+
+typedef boost::shared_ptr< ::std_msgs::Duration > DurationPtr;
+typedef boost::shared_ptr< ::std_msgs::Duration const> DurationConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Duration_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Duration_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Duration_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Duration_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Duration_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Duration_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Duration_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Duration_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Duration_ >
+{
+  static const char* value()
+  {
+    return "3e286caf4241d664e55f3ad380e2ae46";
+  }
+
+  static const char* value(const ::std_msgs::Duration_&) { return value(); }
+  static const uint64_t static_value1 = 0x3e286caf4241d664ULL;
+  static const uint64_t static_value2 = 0xe55f3ad380e2ae46ULL;
+};
+
+template
+struct DataType< ::std_msgs::Duration_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Duration";
+  }
+
+  static const char* value(const ::std_msgs::Duration_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Duration_ >
+{
+  static const char* value()
+  {
+    return "duration data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Duration_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Duration_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Duration_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Duration_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Duration_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_DURATION_H
diff --git a/thirdparty/ros/include/std_msgs/Empty.h b/thirdparty/ros/include/std_msgs/Empty.h
new file mode 100644
index 0000000..cb0bacc
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Empty.h
@@ -0,0 +1,179 @@
+// Generated by gencpp from file std_msgs/Empty.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_EMPTY_H
+#define STD_MSGS_MESSAGE_EMPTY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Empty_
+{
+  typedef Empty_ Type;
+
+  Empty_()
+    {
+    }
+  Empty_(const ContainerAllocator& _alloc)
+    {
+  (void)_alloc;
+    }
+
+
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Empty_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Empty_ const> ConstPtr;
+
+}; // struct Empty_
+
+typedef ::std_msgs::Empty_ > Empty;
+
+typedef boost::shared_ptr< ::std_msgs::Empty > EmptyPtr;
+typedef boost::shared_ptr< ::std_msgs::Empty const> EmptyConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Empty_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Empty_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Empty_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Empty_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Empty_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Empty_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Empty_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Empty_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Empty_ >
+{
+  static const char* value()
+  {
+    return "d41d8cd98f00b204e9800998ecf8427e";
+  }
+
+  static const char* value(const ::std_msgs::Empty_&) { return value(); }
+  static const uint64_t static_value1 = 0xd41d8cd98f00b204ULL;
+  static const uint64_t static_value2 = 0xe9800998ecf8427eULL;
+};
+
+template
+struct DataType< ::std_msgs::Empty_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Empty";
+  }
+
+  static const char* value(const ::std_msgs::Empty_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Empty_ >
+{
+  static const char* value()
+  {
+    return "\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Empty_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Empty_ >
+  {
+    template inline static void allInOne(Stream&, T)
+    {}
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Empty_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Empty_ >
+{
+  template static void stream(Stream&, const std::string&, const ::std_msgs::Empty_&)
+  {}
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_EMPTY_H
diff --git a/thirdparty/ros/include/std_msgs/Float32.h b/thirdparty/ros/include/std_msgs/Float32.h
new file mode 100644
index 0000000..0ac3fa1
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Float32.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Float32.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_FLOAT32_H
+#define STD_MSGS_MESSAGE_FLOAT32_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Float32_
+{
+  typedef Float32_ Type;
+
+  Float32_()
+    : data(0.0)  {
+    }
+  Float32_(const ContainerAllocator& _alloc)
+    : data(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef float _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Float32_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Float32_ const> ConstPtr;
+
+}; // struct Float32_
+
+typedef ::std_msgs::Float32_ > Float32;
+
+typedef boost::shared_ptr< ::std_msgs::Float32 > Float32Ptr;
+typedef boost::shared_ptr< ::std_msgs::Float32 const> Float32ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Float32_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Float32_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Float32_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Float32_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float32_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float32_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float32_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float32_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Float32_ >
+{
+  static const char* value()
+  {
+    return "73fcbf46b49191e672908e50842a83d4";
+  }
+
+  static const char* value(const ::std_msgs::Float32_&) { return value(); }
+  static const uint64_t static_value1 = 0x73fcbf46b49191e6ULL;
+  static const uint64_t static_value2 = 0x72908e50842a83d4ULL;
+};
+
+template
+struct DataType< ::std_msgs::Float32_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Float32";
+  }
+
+  static const char* value(const ::std_msgs::Float32_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Float32_ >
+{
+  static const char* value()
+  {
+    return "float32 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Float32_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Float32_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Float32_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Float32_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Float32_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_FLOAT32_H
diff --git a/thirdparty/ros/include/std_msgs/Float32MultiArray.h b/thirdparty/ros/include/std_msgs/Float32MultiArray.h
new file mode 100644
index 0000000..dfd1fa6
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Float32MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/Float32MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_FLOAT32MULTIARRAY_H
+#define STD_MSGS_MESSAGE_FLOAT32MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct Float32MultiArray_
+{
+  typedef Float32MultiArray_ Type;
+
+  Float32MultiArray_()
+    : layout()
+    , data()  {
+    }
+  Float32MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Float32MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Float32MultiArray_ const> ConstPtr;
+
+}; // struct Float32MultiArray_
+
+typedef ::std_msgs::Float32MultiArray_ > Float32MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::Float32MultiArray > Float32MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::Float32MultiArray const> Float32MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Float32MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Float32MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Float32MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Float32MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float32MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float32MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float32MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float32MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Float32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "6a40e0ffa6a17a503ac3f8616991b1f6";
+  }
+
+  static const char* value(const ::std_msgs::Float32MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x6a40e0ffa6a17a50ULL;
+  static const uint64_t static_value2 = 0x3ac3f8616991b1f6ULL;
+};
+
+template
+struct DataType< ::std_msgs::Float32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Float32MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::Float32MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Float32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+float32[]         data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Float32MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Float32MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Float32MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Float32MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Float32MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_FLOAT32MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/Float64.h b/thirdparty/ros/include/std_msgs/Float64.h
new file mode 100644
index 0000000..d038016
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Float64.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Float64.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_FLOAT64_H
+#define STD_MSGS_MESSAGE_FLOAT64_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Float64_
+{
+  typedef Float64_ Type;
+
+  Float64_()
+    : data(0.0)  {
+    }
+  Float64_(const ContainerAllocator& _alloc)
+    : data(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef double _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Float64_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Float64_ const> ConstPtr;
+
+}; // struct Float64_
+
+typedef ::std_msgs::Float64_ > Float64;
+
+typedef boost::shared_ptr< ::std_msgs::Float64 > Float64Ptr;
+typedef boost::shared_ptr< ::std_msgs::Float64 const> Float64ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Float64_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Float64_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Float64_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Float64_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float64_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float64_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float64_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float64_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Float64_ >
+{
+  static const char* value()
+  {
+    return "fdb28210bfa9d7c91146260178d9a584";
+  }
+
+  static const char* value(const ::std_msgs::Float64_&) { return value(); }
+  static const uint64_t static_value1 = 0xfdb28210bfa9d7c9ULL;
+  static const uint64_t static_value2 = 0x1146260178d9a584ULL;
+};
+
+template
+struct DataType< ::std_msgs::Float64_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Float64";
+  }
+
+  static const char* value(const ::std_msgs::Float64_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Float64_ >
+{
+  static const char* value()
+  {
+    return "float64 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Float64_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Float64_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Float64_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Float64_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Float64_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_FLOAT64_H
diff --git a/thirdparty/ros/include/std_msgs/Float64MultiArray.h b/thirdparty/ros/include/std_msgs/Float64MultiArray.h
new file mode 100644
index 0000000..62683b3
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Float64MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/Float64MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_FLOAT64MULTIARRAY_H
+#define STD_MSGS_MESSAGE_FLOAT64MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct Float64MultiArray_
+{
+  typedef Float64MultiArray_ Type;
+
+  Float64MultiArray_()
+    : layout()
+    , data()  {
+    }
+  Float64MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Float64MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Float64MultiArray_ const> ConstPtr;
+
+}; // struct Float64MultiArray_
+
+typedef ::std_msgs::Float64MultiArray_ > Float64MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::Float64MultiArray > Float64MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::Float64MultiArray const> Float64MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Float64MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Float64MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Float64MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Float64MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float64MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Float64MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float64MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Float64MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Float64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "4b7d974086d4060e7db4613a7e6c3ba4";
+  }
+
+  static const char* value(const ::std_msgs::Float64MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x4b7d974086d4060eULL;
+  static const uint64_t static_value2 = 0x7db4613a7e6c3ba4ULL;
+};
+
+template
+struct DataType< ::std_msgs::Float64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Float64MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::Float64MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Float64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+float64[]         data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Float64MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Float64MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Float64MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Float64MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Float64MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_FLOAT64MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/Header.h b/thirdparty/ros/include/std_msgs/Header.h
new file mode 100644
index 0000000..f8daebe
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Header.h
@@ -0,0 +1,217 @@
+// Generated by gencpp from file std_msgs/Header.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_HEADER_H
+#define STD_MSGS_MESSAGE_HEADER_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Header_
+{
+  typedef Header_ Type;
+
+  Header_()
+    : seq(0)
+    , stamp()
+    , frame_id()  {
+    }
+  Header_(const ContainerAllocator& _alloc)
+    : seq(0)
+    , stamp()
+    , frame_id(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint32_t _seq_type;
+  _seq_type seq;
+
+   typedef ros::Time _stamp_type;
+  _stamp_type stamp;
+
+   typedef std::basic_string, typename ContainerAllocator::template rebind::other >  _frame_id_type;
+  _frame_id_type frame_id;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Header_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Header_ const> ConstPtr;
+
+}; // struct Header_
+
+typedef ::std_msgs::Header_ > Header;
+
+typedef boost::shared_ptr< ::std_msgs::Header > HeaderPtr;
+typedef boost::shared_ptr< ::std_msgs::Header const> HeaderConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Header_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Header_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Header_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Header_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Header_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Header_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Header_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Header_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Header_ >
+{
+  static const char* value()
+  {
+    return "2176decaecbce78abc3b96ef049fabed";
+  }
+
+  static const char* value(const ::std_msgs::Header_&) { return value(); }
+  static const uint64_t static_value1 = 0x2176decaecbce78aULL;
+  static const uint64_t static_value2 = 0xbc3b96ef049fabedULL;
+};
+
+template
+struct DataType< ::std_msgs::Header_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Header";
+  }
+
+  static const char* value(const ::std_msgs::Header_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Header_ >
+{
+  static const char* value()
+  {
+    return "# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Header_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Header_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.seq);
+      stream.next(m.stamp);
+      stream.next(m.frame_id);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Header_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Header_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Header_& v)
+  {
+    s << indent << "seq: ";
+    Printer::stream(s, indent + "  ", v.seq);
+    s << indent << "stamp: ";
+    Printer::stream(s, indent + "  ", v.stamp);
+    s << indent << "frame_id: ";
+    Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + "  ", v.frame_id);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_HEADER_H
diff --git a/thirdparty/ros/include/std_msgs/Int16.h b/thirdparty/ros/include/std_msgs/Int16.h
new file mode 100644
index 0000000..2fae55c
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int16.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Int16.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT16_H
+#define STD_MSGS_MESSAGE_INT16_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Int16_
+{
+  typedef Int16_ Type;
+
+  Int16_()
+    : data(0)  {
+    }
+  Int16_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef int16_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int16_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int16_ const> ConstPtr;
+
+}; // struct Int16_
+
+typedef ::std_msgs::Int16_ > Int16;
+
+typedef boost::shared_ptr< ::std_msgs::Int16 > Int16Ptr;
+typedef boost::shared_ptr< ::std_msgs::Int16 const> Int16ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int16_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int16_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int16_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int16_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int16_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int16_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int16_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int16_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int16_ >
+{
+  static const char* value()
+  {
+    return "8524586e34fbd7cb1c08c5f5f1ca0e57";
+  }
+
+  static const char* value(const ::std_msgs::Int16_&) { return value(); }
+  static const uint64_t static_value1 = 0x8524586e34fbd7cbULL;
+  static const uint64_t static_value2 = 0x1c08c5f5f1ca0e57ULL;
+};
+
+template
+struct DataType< ::std_msgs::Int16_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int16";
+  }
+
+  static const char* value(const ::std_msgs::Int16_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int16_ >
+{
+  static const char* value()
+  {
+    return "int16 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int16_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int16_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int16_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int16_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int16_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT16_H
diff --git a/thirdparty/ros/include/std_msgs/Int16MultiArray.h b/thirdparty/ros/include/std_msgs/Int16MultiArray.h
new file mode 100644
index 0000000..cc57fed
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int16MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/Int16MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT16MULTIARRAY_H
+#define STD_MSGS_MESSAGE_INT16MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct Int16MultiArray_
+{
+  typedef Int16MultiArray_ Type;
+
+  Int16MultiArray_()
+    : layout()
+    , data()  {
+    }
+  Int16MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int16MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int16MultiArray_ const> ConstPtr;
+
+}; // struct Int16MultiArray_
+
+typedef ::std_msgs::Int16MultiArray_ > Int16MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::Int16MultiArray > Int16MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::Int16MultiArray const> Int16MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int16MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int16MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int16MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int16MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int16MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int16MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int16MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int16MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int16MultiArray_ >
+{
+  static const char* value()
+  {
+    return "d9338d7f523fcb692fae9d0a0e9f067c";
+  }
+
+  static const char* value(const ::std_msgs::Int16MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0xd9338d7f523fcb69ULL;
+  static const uint64_t static_value2 = 0x2fae9d0a0e9f067cULL;
+};
+
+template
+struct DataType< ::std_msgs::Int16MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int16MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::Int16MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int16MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+int16[]           data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int16MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int16MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int16MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int16MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int16MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT16MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/Int32.h b/thirdparty/ros/include/std_msgs/Int32.h
new file mode 100644
index 0000000..2c03215
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int32.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Int32.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT32_H
+#define STD_MSGS_MESSAGE_INT32_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Int32_
+{
+  typedef Int32_ Type;
+
+  Int32_()
+    : data(0)  {
+    }
+  Int32_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef int32_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int32_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int32_ const> ConstPtr;
+
+}; // struct Int32_
+
+typedef ::std_msgs::Int32_ > Int32;
+
+typedef boost::shared_ptr< ::std_msgs::Int32 > Int32Ptr;
+typedef boost::shared_ptr< ::std_msgs::Int32 const> Int32ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int32_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int32_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int32_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int32_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int32_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int32_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int32_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int32_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int32_ >
+{
+  static const char* value()
+  {
+    return "da5909fbe378aeaf85e547e830cc1bb7";
+  }
+
+  static const char* value(const ::std_msgs::Int32_&) { return value(); }
+  static const uint64_t static_value1 = 0xda5909fbe378aeafULL;
+  static const uint64_t static_value2 = 0x85e547e830cc1bb7ULL;
+};
+
+template
+struct DataType< ::std_msgs::Int32_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int32";
+  }
+
+  static const char* value(const ::std_msgs::Int32_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int32_ >
+{
+  static const char* value()
+  {
+    return "int32 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int32_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int32_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int32_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int32_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int32_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT32_H
diff --git a/thirdparty/ros/include/std_msgs/Int32MultiArray.h b/thirdparty/ros/include/std_msgs/Int32MultiArray.h
new file mode 100644
index 0000000..47deadb
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int32MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/Int32MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT32MULTIARRAY_H
+#define STD_MSGS_MESSAGE_INT32MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct Int32MultiArray_
+{
+  typedef Int32MultiArray_ Type;
+
+  Int32MultiArray_()
+    : layout()
+    , data()  {
+    }
+  Int32MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int32MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int32MultiArray_ const> ConstPtr;
+
+}; // struct Int32MultiArray_
+
+typedef ::std_msgs::Int32MultiArray_ > Int32MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::Int32MultiArray > Int32MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::Int32MultiArray const> Int32MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int32MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int32MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int32MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int32MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int32MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int32MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int32MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int32MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "1d99f79f8b325b44fee908053e9c945b";
+  }
+
+  static const char* value(const ::std_msgs::Int32MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x1d99f79f8b325b44ULL;
+  static const uint64_t static_value2 = 0xfee908053e9c945bULL;
+};
+
+template
+struct DataType< ::std_msgs::Int32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int32MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::Int32MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+int32[]           data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int32MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int32MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int32MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int32MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int32MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT32MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/Int64.h b/thirdparty/ros/include/std_msgs/Int64.h
new file mode 100644
index 0000000..d53ed02
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int64.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Int64.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT64_H
+#define STD_MSGS_MESSAGE_INT64_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Int64_
+{
+  typedef Int64_ Type;
+
+  Int64_()
+    : data(0)  {
+    }
+  Int64_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef int64_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int64_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int64_ const> ConstPtr;
+
+}; // struct Int64_
+
+typedef ::std_msgs::Int64_ > Int64;
+
+typedef boost::shared_ptr< ::std_msgs::Int64 > Int64Ptr;
+typedef boost::shared_ptr< ::std_msgs::Int64 const> Int64ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int64_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int64_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int64_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int64_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int64_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int64_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int64_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int64_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int64_ >
+{
+  static const char* value()
+  {
+    return "34add168574510e6e17f5d23ecc077ef";
+  }
+
+  static const char* value(const ::std_msgs::Int64_&) { return value(); }
+  static const uint64_t static_value1 = 0x34add168574510e6ULL;
+  static const uint64_t static_value2 = 0xe17f5d23ecc077efULL;
+};
+
+template
+struct DataType< ::std_msgs::Int64_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int64";
+  }
+
+  static const char* value(const ::std_msgs::Int64_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int64_ >
+{
+  static const char* value()
+  {
+    return "int64 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int64_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int64_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int64_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int64_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int64_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT64_H
diff --git a/thirdparty/ros/include/std_msgs/Int64MultiArray.h b/thirdparty/ros/include/std_msgs/Int64MultiArray.h
new file mode 100644
index 0000000..0c209a1
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int64MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/Int64MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT64MULTIARRAY_H
+#define STD_MSGS_MESSAGE_INT64MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct Int64MultiArray_
+{
+  typedef Int64MultiArray_ Type;
+
+  Int64MultiArray_()
+    : layout()
+    , data()  {
+    }
+  Int64MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int64MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int64MultiArray_ const> ConstPtr;
+
+}; // struct Int64MultiArray_
+
+typedef ::std_msgs::Int64MultiArray_ > Int64MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::Int64MultiArray > Int64MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::Int64MultiArray const> Int64MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int64MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int64MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int64MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int64MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int64MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int64MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int64MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int64MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "54865aa6c65be0448113a2afc6a49270";
+  }
+
+  static const char* value(const ::std_msgs::Int64MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x54865aa6c65be044ULL;
+  static const uint64_t static_value2 = 0x8113a2afc6a49270ULL;
+};
+
+template
+struct DataType< ::std_msgs::Int64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int64MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::Int64MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+int64[]           data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int64MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int64MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int64MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int64MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int64MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT64MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/Int8.h b/thirdparty/ros/include/std_msgs/Int8.h
new file mode 100644
index 0000000..4c1f164
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int8.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Int8.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT8_H
+#define STD_MSGS_MESSAGE_INT8_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Int8_
+{
+  typedef Int8_ Type;
+
+  Int8_()
+    : data(0)  {
+    }
+  Int8_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef int8_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int8_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int8_ const> ConstPtr;
+
+}; // struct Int8_
+
+typedef ::std_msgs::Int8_ > Int8;
+
+typedef boost::shared_ptr< ::std_msgs::Int8 > Int8Ptr;
+typedef boost::shared_ptr< ::std_msgs::Int8 const> Int8ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int8_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int8_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int8_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int8_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int8_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int8_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int8_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int8_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int8_ >
+{
+  static const char* value()
+  {
+    return "27ffa0c9c4b8fb8492252bcad9e5c57b";
+  }
+
+  static const char* value(const ::std_msgs::Int8_&) { return value(); }
+  static const uint64_t static_value1 = 0x27ffa0c9c4b8fb84ULL;
+  static const uint64_t static_value2 = 0x92252bcad9e5c57bULL;
+};
+
+template
+struct DataType< ::std_msgs::Int8_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int8";
+  }
+
+  static const char* value(const ::std_msgs::Int8_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int8_ >
+{
+  static const char* value()
+  {
+    return "int8 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int8_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int8_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int8_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int8_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int8_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT8_H
diff --git a/thirdparty/ros/include/std_msgs/Int8MultiArray.h b/thirdparty/ros/include/std_msgs/Int8MultiArray.h
new file mode 100644
index 0000000..3784d3b
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Int8MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/Int8MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_INT8MULTIARRAY_H
+#define STD_MSGS_MESSAGE_INT8MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct Int8MultiArray_
+{
+  typedef Int8MultiArray_ Type;
+
+  Int8MultiArray_()
+    : layout()
+    , data()  {
+    }
+  Int8MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Int8MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Int8MultiArray_ const> ConstPtr;
+
+}; // struct Int8MultiArray_
+
+typedef ::std_msgs::Int8MultiArray_ > Int8MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::Int8MultiArray > Int8MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::Int8MultiArray const> Int8MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Int8MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Int8MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Int8MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Int8MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int8MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Int8MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int8MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Int8MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Int8MultiArray_ >
+{
+  static const char* value()
+  {
+    return "d7c1af35a1b4781bbe79e03dd94b7c13";
+  }
+
+  static const char* value(const ::std_msgs::Int8MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0xd7c1af35a1b4781bULL;
+  static const uint64_t static_value2 = 0xbe79e03dd94b7c13ULL;
+};
+
+template
+struct DataType< ::std_msgs::Int8MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Int8MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::Int8MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Int8MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+int8[]            data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Int8MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Int8MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Int8MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Int8MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Int8MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_INT8MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/MultiArrayDimension.h b/thirdparty/ros/include/std_msgs/MultiArrayDimension.h
new file mode 100644
index 0000000..94bfdc6
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/MultiArrayDimension.h
@@ -0,0 +1,205 @@
+// Generated by gencpp from file std_msgs/MultiArrayDimension.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_MULTIARRAYDIMENSION_H
+#define STD_MSGS_MESSAGE_MULTIARRAYDIMENSION_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct MultiArrayDimension_
+{
+  typedef MultiArrayDimension_ Type;
+
+  MultiArrayDimension_()
+    : label()
+    , size(0)
+    , stride(0)  {
+    }
+  MultiArrayDimension_(const ContainerAllocator& _alloc)
+    : label(_alloc)
+    , size(0)
+    , stride(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef std::basic_string, typename ContainerAllocator::template rebind::other >  _label_type;
+  _label_type label;
+
+   typedef uint32_t _size_type;
+  _size_type size;
+
+   typedef uint32_t _stride_type;
+  _stride_type stride;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension_ const> ConstPtr;
+
+}; // struct MultiArrayDimension_
+
+typedef ::std_msgs::MultiArrayDimension_ > MultiArrayDimension;
+
+typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension > MultiArrayDimensionPtr;
+typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension const> MultiArrayDimensionConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::MultiArrayDimension_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::MultiArrayDimension_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::MultiArrayDimension_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::MultiArrayDimension_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::MultiArrayDimension_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::MultiArrayDimension_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::MultiArrayDimension_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::MultiArrayDimension_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::MultiArrayDimension_ >
+{
+  static const char* value()
+  {
+    return "4cd0c83a8683deae40ecdac60e53bfa8";
+  }
+
+  static const char* value(const ::std_msgs::MultiArrayDimension_&) { return value(); }
+  static const uint64_t static_value1 = 0x4cd0c83a8683deaeULL;
+  static const uint64_t static_value2 = 0x40ecdac60e53bfa8ULL;
+};
+
+template
+struct DataType< ::std_msgs::MultiArrayDimension_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/MultiArrayDimension";
+  }
+
+  static const char* value(const ::std_msgs::MultiArrayDimension_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::MultiArrayDimension_ >
+{
+  static const char* value()
+  {
+    return "string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::MultiArrayDimension_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::MultiArrayDimension_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.label);
+      stream.next(m.size);
+      stream.next(m.stride);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct MultiArrayDimension_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::MultiArrayDimension_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::MultiArrayDimension_& v)
+  {
+    s << indent << "label: ";
+    Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + "  ", v.label);
+    s << indent << "size: ";
+    Printer::stream(s, indent + "  ", v.size);
+    s << indent << "stride: ";
+    Printer::stream(s, indent + "  ", v.stride);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_MULTIARRAYDIMENSION_H
diff --git a/thirdparty/ros/include/std_msgs/MultiArrayLayout.h b/thirdparty/ros/include/std_msgs/MultiArrayLayout.h
new file mode 100644
index 0000000..3b73764
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/MultiArrayLayout.h
@@ -0,0 +1,233 @@
+// Generated by gencpp from file std_msgs/MultiArrayLayout.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_MULTIARRAYLAYOUT_H
+#define STD_MSGS_MESSAGE_MULTIARRAYLAYOUT_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct MultiArrayLayout_
+{
+  typedef MultiArrayLayout_ Type;
+
+  MultiArrayLayout_()
+    : dim()
+    , data_offset(0)  {
+    }
+  MultiArrayLayout_(const ContainerAllocator& _alloc)
+    : dim(_alloc)
+    , data_offset(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef std::vector< ::std_msgs::MultiArrayDimension_ , typename ContainerAllocator::template rebind< ::std_msgs::MultiArrayDimension_ >::other >  _dim_type;
+  _dim_type dim;
+
+   typedef uint32_t _data_offset_type;
+  _data_offset_type data_offset;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout_ const> ConstPtr;
+
+}; // struct MultiArrayLayout_
+
+typedef ::std_msgs::MultiArrayLayout_ > MultiArrayLayout;
+
+typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout > MultiArrayLayoutPtr;
+typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout const> MultiArrayLayoutConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::MultiArrayLayout_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::MultiArrayLayout_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::MultiArrayLayout_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::MultiArrayLayout_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::MultiArrayLayout_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::MultiArrayLayout_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::MultiArrayLayout_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::MultiArrayLayout_ >
+{
+  static const char* value()
+  {
+    return "0fed2a11c13e11c5571b4e2a995a91a3";
+  }
+
+  static const char* value(const ::std_msgs::MultiArrayLayout_&) { return value(); }
+  static const uint64_t static_value1 = 0x0fed2a11c13e11c5ULL;
+  static const uint64_t static_value2 = 0x571b4e2a995a91a3ULL;
+};
+
+template
+struct DataType< ::std_msgs::MultiArrayLayout_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/MultiArrayLayout";
+  }
+
+  static const char* value(const ::std_msgs::MultiArrayLayout_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::MultiArrayLayout_ >
+{
+  static const char* value()
+  {
+    return "# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::MultiArrayLayout_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::MultiArrayLayout_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.dim);
+      stream.next(m.data_offset);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct MultiArrayLayout_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::MultiArrayLayout_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::MultiArrayLayout_& v)
+  {
+    s << indent << "dim[]" << std::endl;
+    for (size_t i = 0; i < v.dim.size(); ++i)
+    {
+      s << indent << "  dim[" << i << "]: ";
+      s << std::endl;
+      s << indent;
+      Printer< ::std_msgs::MultiArrayDimension_ >::stream(s, indent + "    ", v.dim[i]);
+    }
+    s << indent << "data_offset: ";
+    Printer::stream(s, indent + "  ", v.data_offset);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_MULTIARRAYLAYOUT_H
diff --git a/thirdparty/ros/include/std_msgs/String.h b/thirdparty/ros/include/std_msgs/String.h
new file mode 100644
index 0000000..d73c8e1
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/String.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/String.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_STRING_H
+#define STD_MSGS_MESSAGE_STRING_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct String_
+{
+  typedef String_ Type;
+
+  String_()
+    : data()  {
+    }
+  String_(const ContainerAllocator& _alloc)
+    : data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef std::basic_string, typename ContainerAllocator::template rebind::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::String_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::String_ const> ConstPtr;
+
+}; // struct String_
+
+typedef ::std_msgs::String_ > String;
+
+typedef boost::shared_ptr< ::std_msgs::String > StringPtr;
+typedef boost::shared_ptr< ::std_msgs::String const> StringConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::String_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::String_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::String_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::String_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::String_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::String_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::String_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::String_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::String_ >
+{
+  static const char* value()
+  {
+    return "992ce8a1687cec8c8bd883ec73ca41d1";
+  }
+
+  static const char* value(const ::std_msgs::String_&) { return value(); }
+  static const uint64_t static_value1 = 0x992ce8a1687cec8cULL;
+  static const uint64_t static_value2 = 0x8bd883ec73ca41d1ULL;
+};
+
+template
+struct DataType< ::std_msgs::String_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/String";
+  }
+
+  static const char* value(const ::std_msgs::String_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::String_ >
+{
+  static const char* value()
+  {
+    return "string data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::String_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::String_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct String_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::String_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::String_& v)
+  {
+    s << indent << "data: ";
+    Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_STRING_H
diff --git a/thirdparty/ros/include/std_msgs/Time.h b/thirdparty/ros/include/std_msgs/Time.h
new file mode 100644
index 0000000..e47fec4
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/Time.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/Time.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_TIME_H
+#define STD_MSGS_MESSAGE_TIME_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct Time_
+{
+  typedef Time_ Type;
+
+  Time_()
+    : data()  {
+    }
+  Time_(const ContainerAllocator& _alloc)
+    : data()  {
+  (void)_alloc;
+    }
+
+
+
+   typedef ros::Time _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::Time_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::Time_ const> ConstPtr;
+
+}; // struct Time_
+
+typedef ::std_msgs::Time_ > Time;
+
+typedef boost::shared_ptr< ::std_msgs::Time > TimePtr;
+typedef boost::shared_ptr< ::std_msgs::Time const> TimeConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::Time_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::Time_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::Time_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::Time_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Time_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::Time_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Time_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::Time_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::Time_ >
+{
+  static const char* value()
+  {
+    return "cd7166c74c552c311fbcc2fe5a7bc289";
+  }
+
+  static const char* value(const ::std_msgs::Time_&) { return value(); }
+  static const uint64_t static_value1 = 0xcd7166c74c552c31ULL;
+  static const uint64_t static_value2 = 0x1fbcc2fe5a7bc289ULL;
+};
+
+template
+struct DataType< ::std_msgs::Time_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/Time";
+  }
+
+  static const char* value(const ::std_msgs::Time_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::Time_ >
+{
+  static const char* value()
+  {
+    return "time data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::Time_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::Time_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Time_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::Time_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Time_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_TIME_H
diff --git a/thirdparty/ros/include/std_msgs/UInt16.h b/thirdparty/ros/include/std_msgs/UInt16.h
new file mode 100644
index 0000000..377ae13
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt16.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/UInt16.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT16_H
+#define STD_MSGS_MESSAGE_UINT16_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct UInt16_
+{
+  typedef UInt16_ Type;
+
+  UInt16_()
+    : data(0)  {
+    }
+  UInt16_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint16_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt16_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt16_ const> ConstPtr;
+
+}; // struct UInt16_
+
+typedef ::std_msgs::UInt16_ > UInt16;
+
+typedef boost::shared_ptr< ::std_msgs::UInt16 > UInt16Ptr;
+typedef boost::shared_ptr< ::std_msgs::UInt16 const> UInt16ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt16_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt16_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt16_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt16_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt16_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt16_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt16_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt16_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt16_ >
+{
+  static const char* value()
+  {
+    return "1df79edf208b629fe6b81923a544552d";
+  }
+
+  static const char* value(const ::std_msgs::UInt16_&) { return value(); }
+  static const uint64_t static_value1 = 0x1df79edf208b629fULL;
+  static const uint64_t static_value2 = 0xe6b81923a544552dULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt16_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt16";
+  }
+
+  static const char* value(const ::std_msgs::UInt16_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt16_ >
+{
+  static const char* value()
+  {
+    return "uint16 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt16_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt16_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt16_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt16_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt16_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT16_H
diff --git a/thirdparty/ros/include/std_msgs/UInt16MultiArray.h b/thirdparty/ros/include/std_msgs/UInt16MultiArray.h
new file mode 100644
index 0000000..a8deaeb
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt16MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/UInt16MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT16MULTIARRAY_H
+#define STD_MSGS_MESSAGE_UINT16MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct UInt16MultiArray_
+{
+  typedef UInt16MultiArray_ Type;
+
+  UInt16MultiArray_()
+    : layout()
+    , data()  {
+    }
+  UInt16MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt16MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt16MultiArray_ const> ConstPtr;
+
+}; // struct UInt16MultiArray_
+
+typedef ::std_msgs::UInt16MultiArray_ > UInt16MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::UInt16MultiArray > UInt16MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::UInt16MultiArray const> UInt16MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt16MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt16MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt16MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt16MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt16MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt16MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt16MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt16MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt16MultiArray_ >
+{
+  static const char* value()
+  {
+    return "52f264f1c973c4b73790d384c6cb4484";
+  }
+
+  static const char* value(const ::std_msgs::UInt16MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x52f264f1c973c4b7ULL;
+  static const uint64_t static_value2 = 0x3790d384c6cb4484ULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt16MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt16MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::UInt16MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt16MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+uint16[]            data        # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt16MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt16MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt16MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt16MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt16MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT16MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/UInt32.h b/thirdparty/ros/include/std_msgs/UInt32.h
new file mode 100644
index 0000000..2d9f745
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt32.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/UInt32.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT32_H
+#define STD_MSGS_MESSAGE_UINT32_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct UInt32_
+{
+  typedef UInt32_ Type;
+
+  UInt32_()
+    : data(0)  {
+    }
+  UInt32_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint32_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt32_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt32_ const> ConstPtr;
+
+}; // struct UInt32_
+
+typedef ::std_msgs::UInt32_ > UInt32;
+
+typedef boost::shared_ptr< ::std_msgs::UInt32 > UInt32Ptr;
+typedef boost::shared_ptr< ::std_msgs::UInt32 const> UInt32ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt32_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt32_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt32_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt32_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt32_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt32_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt32_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt32_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt32_ >
+{
+  static const char* value()
+  {
+    return "304a39449588c7f8ce2df6e8001c5fce";
+  }
+
+  static const char* value(const ::std_msgs::UInt32_&) { return value(); }
+  static const uint64_t static_value1 = 0x304a39449588c7f8ULL;
+  static const uint64_t static_value2 = 0xce2df6e8001c5fceULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt32_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt32";
+  }
+
+  static const char* value(const ::std_msgs::UInt32_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt32_ >
+{
+  static const char* value()
+  {
+    return "uint32 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt32_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt32_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt32_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt32_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt32_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT32_H
diff --git a/thirdparty/ros/include/std_msgs/UInt32MultiArray.h b/thirdparty/ros/include/std_msgs/UInt32MultiArray.h
new file mode 100644
index 0000000..2fd8f3a
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt32MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/UInt32MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT32MULTIARRAY_H
+#define STD_MSGS_MESSAGE_UINT32MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct UInt32MultiArray_
+{
+  typedef UInt32MultiArray_ Type;
+
+  UInt32MultiArray_()
+    : layout()
+    , data()  {
+    }
+  UInt32MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt32MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt32MultiArray_ const> ConstPtr;
+
+}; // struct UInt32MultiArray_
+
+typedef ::std_msgs::UInt32MultiArray_ > UInt32MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::UInt32MultiArray > UInt32MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::UInt32MultiArray const> UInt32MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt32MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt32MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt32MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt32MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt32MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt32MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt32MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt32MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "4d6a180abc9be191b96a7eda6c8a233d";
+  }
+
+  static const char* value(const ::std_msgs::UInt32MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x4d6a180abc9be191ULL;
+  static const uint64_t static_value2 = 0xb96a7eda6c8a233dULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt32MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::UInt32MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt32MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+uint32[]          data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt32MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt32MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt32MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt32MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt32MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT32MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/UInt64.h b/thirdparty/ros/include/std_msgs/UInt64.h
new file mode 100644
index 0000000..448393d
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt64.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/UInt64.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT64_H
+#define STD_MSGS_MESSAGE_UINT64_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct UInt64_
+{
+  typedef UInt64_ Type;
+
+  UInt64_()
+    : data(0)  {
+    }
+  UInt64_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint64_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt64_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt64_ const> ConstPtr;
+
+}; // struct UInt64_
+
+typedef ::std_msgs::UInt64_ > UInt64;
+
+typedef boost::shared_ptr< ::std_msgs::UInt64 > UInt64Ptr;
+typedef boost::shared_ptr< ::std_msgs::UInt64 const> UInt64ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt64_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt64_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt64_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt64_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt64_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt64_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt64_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt64_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt64_ >
+{
+  static const char* value()
+  {
+    return "1b2a79973e8bf53d7b53acb71299cb57";
+  }
+
+  static const char* value(const ::std_msgs::UInt64_&) { return value(); }
+  static const uint64_t static_value1 = 0x1b2a79973e8bf53dULL;
+  static const uint64_t static_value2 = 0x7b53acb71299cb57ULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt64_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt64";
+  }
+
+  static const char* value(const ::std_msgs::UInt64_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt64_ >
+{
+  static const char* value()
+  {
+    return "uint64 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt64_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt64_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt64_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt64_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt64_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT64_H
diff --git a/thirdparty/ros/include/std_msgs/UInt64MultiArray.h b/thirdparty/ros/include/std_msgs/UInt64MultiArray.h
new file mode 100644
index 0000000..28ac539
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt64MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/UInt64MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT64MULTIARRAY_H
+#define STD_MSGS_MESSAGE_UINT64MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct UInt64MultiArray_
+{
+  typedef UInt64MultiArray_ Type;
+
+  UInt64MultiArray_()
+    : layout()
+    , data()  {
+    }
+  UInt64MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt64MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt64MultiArray_ const> ConstPtr;
+
+}; // struct UInt64MultiArray_
+
+typedef ::std_msgs::UInt64MultiArray_ > UInt64MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::UInt64MultiArray > UInt64MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::UInt64MultiArray const> UInt64MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt64MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt64MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt64MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt64MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt64MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt64MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt64MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt64MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "6088f127afb1d6c72927aa1247e945af";
+  }
+
+  static const char* value(const ::std_msgs::UInt64MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x6088f127afb1d6c7ULL;
+  static const uint64_t static_value2 = 0x2927aa1247e945afULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt64MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::UInt64MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt64MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+uint64[]          data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt64MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt64MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt64MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt64MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt64MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT64MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/UInt8.h b/thirdparty/ros/include/std_msgs/UInt8.h
new file mode 100644
index 0000000..e698e5f
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt8.h
@@ -0,0 +1,187 @@
+// Generated by gencpp from file std_msgs/UInt8.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT8_H
+#define STD_MSGS_MESSAGE_UINT8_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+
+namespace std_msgs
+{
+template 
+struct UInt8_
+{
+  typedef UInt8_ Type;
+
+  UInt8_()
+    : data(0)  {
+    }
+  UInt8_(const ContainerAllocator& _alloc)
+    : data(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt8_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt8_ const> ConstPtr;
+
+}; // struct UInt8_
+
+typedef ::std_msgs::UInt8_ > UInt8;
+
+typedef boost::shared_ptr< ::std_msgs::UInt8 > UInt8Ptr;
+typedef boost::shared_ptr< ::std_msgs::UInt8 const> UInt8ConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt8_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt8_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt8_ >
+  : TrueType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt8_ const>
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt8_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt8_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt8_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt8_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt8_ >
+{
+  static const char* value()
+  {
+    return "7c8164229e7d2c17eb95e9231617fdee";
+  }
+
+  static const char* value(const ::std_msgs::UInt8_&) { return value(); }
+  static const uint64_t static_value1 = 0x7c8164229e7d2c17ULL;
+  static const uint64_t static_value2 = 0xeb95e9231617fdeeULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt8_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt8";
+  }
+
+  static const char* value(const ::std_msgs::UInt8_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt8_ >
+{
+  static const char* value()
+  {
+    return "uint8 data\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt8_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt8_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt8_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt8_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt8_& v)
+  {
+    s << indent << "data: ";
+    Printer::stream(s, indent + "  ", v.data);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT8_H
diff --git a/thirdparty/ros/include/std_msgs/UInt8MultiArray.h b/thirdparty/ros/include/std_msgs/UInt8MultiArray.h
new file mode 100644
index 0000000..a185aa0
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/UInt8MultiArray.h
@@ -0,0 +1,241 @@
+// Generated by gencpp from file std_msgs/UInt8MultiArray.msg
+// DO NOT EDIT!
+
+
+#ifndef STD_MSGS_MESSAGE_UINT8MULTIARRAY_H
+#define STD_MSGS_MESSAGE_UINT8MULTIARRAY_H
+
+
+#include 
+#include 
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include 
+
+namespace std_msgs
+{
+template 
+struct UInt8MultiArray_
+{
+  typedef UInt8MultiArray_ Type;
+
+  UInt8MultiArray_()
+    : layout()
+    , data()  {
+    }
+  UInt8MultiArray_(const ContainerAllocator& _alloc)
+    : layout(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::MultiArrayLayout_  _layout_type;
+  _layout_type layout;
+
+   typedef std::vector::other >  _data_type;
+  _data_type data;
+
+
+
+
+
+  typedef boost::shared_ptr< ::std_msgs::UInt8MultiArray_ > Ptr;
+  typedef boost::shared_ptr< ::std_msgs::UInt8MultiArray_ const> ConstPtr;
+
+}; // struct UInt8MultiArray_
+
+typedef ::std_msgs::UInt8MultiArray_ > UInt8MultiArray;
+
+typedef boost::shared_ptr< ::std_msgs::UInt8MultiArray > UInt8MultiArrayPtr;
+typedef boost::shared_ptr< ::std_msgs::UInt8MultiArray const> UInt8MultiArrayConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template
+std::ostream& operator<<(std::ostream& s, const ::std_msgs::UInt8MultiArray_ & v)
+{
+ros::message_operations::Printer< ::std_msgs::UInt8MultiArray_ >::stream(s, "", v);
+return s;
+}
+
+} // namespace std_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/tmp/binarydeb/ros-kinetic-std-msgs-0.5.11/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template 
+struct IsFixedSize< ::std_msgs::UInt8MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct IsFixedSize< ::std_msgs::UInt8MultiArray_ const>
+  : FalseType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt8MultiArray_ >
+  : TrueType
+  { };
+
+template 
+struct IsMessage< ::std_msgs::UInt8MultiArray_ const>
+  : TrueType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt8MultiArray_ >
+  : FalseType
+  { };
+
+template 
+struct HasHeader< ::std_msgs::UInt8MultiArray_ const>
+  : FalseType
+  { };
+
+
+template
+struct MD5Sum< ::std_msgs::UInt8MultiArray_ >
+{
+  static const char* value()
+  {
+    return "82373f1612381bb6ee473b5cd6f5d89c";
+  }
+
+  static const char* value(const ::std_msgs::UInt8MultiArray_&) { return value(); }
+  static const uint64_t static_value1 = 0x82373f1612381bb6ULL;
+  static const uint64_t static_value2 = 0xee473b5cd6f5d89cULL;
+};
+
+template
+struct DataType< ::std_msgs::UInt8MultiArray_ >
+{
+  static const char* value()
+  {
+    return "std_msgs/UInt8MultiArray";
+  }
+
+  static const char* value(const ::std_msgs::UInt8MultiArray_&) { return value(); }
+};
+
+template
+struct Definition< ::std_msgs::UInt8MultiArray_ >
+{
+  static const char* value()
+  {
+    return "# Please look at the MultiArrayLayout message definition for\n\
+# documentation on all multiarrays.\n\
+\n\
+MultiArrayLayout  layout        # specification of data layout\n\
+uint8[]           data          # array of data\n\
+\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayLayout\n\
+# The multiarray declares a generic multi-dimensional array of a\n\
+# particular data type.  Dimensions are ordered from outer most\n\
+# to inner most.\n\
+\n\
+MultiArrayDimension[] dim # Array of dimension properties\n\
+uint32 data_offset        # padding elements at front of data\n\
+\n\
+# Accessors should ALWAYS be written in terms of dimension stride\n\
+# and specified outer-most dimension first.\n\
+# \n\
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n\
+#\n\
+# A standard, 3-channel 640x480 image with interleaved color channels\n\
+# would be specified as:\n\
+#\n\
+# dim[0].label  = \"height\"\n\
+# dim[0].size   = 480\n\
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)\n\
+# dim[1].label  = \"width\"\n\
+# dim[1].size   = 640\n\
+# dim[1].stride = 3*640 = 1920\n\
+# dim[2].label  = \"channel\"\n\
+# dim[2].size   = 3\n\
+# dim[2].stride = 3\n\
+#\n\
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/MultiArrayDimension\n\
+string label   # label of given dimension\n\
+uint32 size    # size of given dimension (in type units)\n\
+uint32 stride  # stride of given dimension\n\
+";
+  }
+
+  static const char* value(const ::std_msgs::UInt8MultiArray_&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template struct Serializer< ::std_msgs::UInt8MultiArray_ >
+  {
+    template inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.layout);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct UInt8MultiArray_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::std_msgs::UInt8MultiArray_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const ::std_msgs::UInt8MultiArray_& v)
+  {
+    s << indent << "layout: ";
+    s << std::endl;
+    Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + "  ", v.layout);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // STD_MSGS_MESSAGE_UINT8MULTIARRAY_H
diff --git a/thirdparty/ros/include/std_msgs/builtin_bool.h b/thirdparty/ros/include/std_msgs/builtin_bool.h
new file mode 100644
index 0000000..e2f0f3d
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_bool.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_BOOL_H
+#define STD_MSGS_BUILTIN_BOOL_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(bool, Bool, 0x8b94c1b53db61fb6ULL, 0xaed406028ad6332aULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_double.h b/thirdparty/ros/include/std_msgs/builtin_double.h
new file mode 100644
index 0000000..22fd85b
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_double.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_DOUBLE_H
+#define STD_MSGS_BUILTIN_DOUBLE_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(double, Float64, 0xfdb28210bfa9d7c9ULL, 0x1146260178d9a584ULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_float.h b/thirdparty/ros/include/std_msgs/builtin_float.h
new file mode 100644
index 0000000..1766492
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_float.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_FLOAT_H
+#define STD_MSGS_BUILTIN_FLOAT_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(float, Float32, 0x73fcbf46b49191e6ULL, 0x72908e50842a83d4ULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_int16.h b/thirdparty/ros/include/std_msgs/builtin_int16.h
new file mode 100644
index 0000000..8f1edf4
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_int16.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_INT16_H
+#define STD_MSGS_BUILTIN_INT16_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(int16_t, Int16, 0x8524586e34fbd7cbULL, 0x1c08c5f5f1ca0e57ULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_int32.h b/thirdparty/ros/include/std_msgs/builtin_int32.h
new file mode 100644
index 0000000..126c995
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_int32.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_INT32_H
+#define STD_MSGS_BUILTIN_INT32_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(int32_t, Int32, 0xda5909fbe378aeafULL, 0x85e547e830cc1bb7ULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_int64.h b/thirdparty/ros/include/std_msgs/builtin_int64.h
new file mode 100644
index 0000000..a009542
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_int64.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_INT64_H
+#define STD_MSGS_BUILTIN_INT64_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(int64_t, Int64, 0x34add168574510e6ULL, 0xe17f5d23ecc077efULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_int8.h b/thirdparty/ros/include/std_msgs/builtin_int8.h
new file mode 100644
index 0000000..b4896ec
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_int8.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_INT8_H
+#define STD_MSGS_BUILTIN_INT8_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(int8_t, Int8, 0x27ffa0c9c4b8fb84ULL, 0x92252bcad9e5c57bULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_string.h b/thirdparty/ros/include/std_msgs/builtin_string.h
new file mode 100644
index 0000000..1118849
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_string.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_STRING_H
+#define STD_MSGS_BUILTIN_STRING_H
+
+#include "trait_macros.h"
+#include 
+
+namespace ros
+{
+namespace message_traits
+{
+
+template
+struct MD5Sum, ContainerAllocator> >
+{
+  static const char* value()
+  {
+    ROS_STATIC_ASSERT(MD5Sum::static_value1 == 0x992ce8a1687cec8cULL);
+    ROS_STATIC_ASSERT(MD5Sum::static_value2 == 0x8bd883ec73ca41d1ULL);
+    return MD5Sum >::value();
+  }
+
+  static const char* value(const std::basic_string, ContainerAllocator>&)
+  {
+    return value();
+  }
+};
+
+template
+struct DataType, ContainerAllocator > >
+{
+  static const char* value()
+  {
+    return DataType >::value();
+  }
+
+  static const char* value(const std::basic_string, ContainerAllocator >&)
+  {
+    return value();
+  }
+};
+
+template
+struct Definition, ContainerAllocator > >
+{
+  static const char* value()
+  {
+    return Definition >::value();
+  }
+
+  static const char* value(const std::basic_string, ContainerAllocator >&)
+  {
+    return value();
+  }
+};
+
+}
+}
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_uint16.h b/thirdparty/ros/include/std_msgs/builtin_uint16.h
new file mode 100644
index 0000000..6845f28
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_uint16.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_UINT16_H
+#define STD_MSGS_BUILTIN_UINT16_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(uint16_t, UInt16, 0x1df79edf208b629fULL, 0xe6b81923a544552dULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_uint32.h b/thirdparty/ros/include/std_msgs/builtin_uint32.h
new file mode 100644
index 0000000..c13436f
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_uint32.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_UINT32_H
+#define STD_MSGS_BUILTIN_UINT32_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(uint32_t, UInt32, 0x304a39449588c7f8ULL, 0xce2df6e8001c5fceULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_uint64.h b/thirdparty/ros/include/std_msgs/builtin_uint64.h
new file mode 100644
index 0000000..d0fba01
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_uint64.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_UINT64_H
+#define STD_MSGS_BUILTIN_UINT64_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(uint64_t, UInt64, 0x1b2a79973e8bf53dULL, 0x7b53acb71299cb57ULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/builtin_uint8.h b/thirdparty/ros/include/std_msgs/builtin_uint8.h
new file mode 100644
index 0000000..1de482d
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/builtin_uint8.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_BUILTIN_UINT8_H
+#define STD_MSGS_BUILTIN_UINT8_H
+
+#include "trait_macros.h"
+#include 
+
+STD_MSGS_DEFINE_BUILTIN_TRAITS(uint8_t, UInt8, 0x7c8164229e7d2c17ULL, 0xeb95e9231617fdeeULL)
+
+#endif
diff --git a/thirdparty/ros/include/std_msgs/header_deprecated_def.h b/thirdparty/ros/include/std_msgs/header_deprecated_def.h
new file mode 100644
index 0000000..6dbdd09
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/header_deprecated_def.h
@@ -0,0 +1,249 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+ 
+#ifndef STD_MSGS_INCLUDING_HEADER_DEPRECATED_DEF
+#error("Do not include this file directly.  Instead, include std_msgs/Header.h")
+#endif
+
+namespace roslib
+{
+template 
+struct Header_ : public std_msgs::Header_
+{
+  typedef Header_ Type;
+
+  ROS_DEPRECATED Header_()
+  {
+  }
+
+  ROS_DEPRECATED Header_(const ContainerAllocator& _alloc)
+  : std_msgs::Header_(_alloc)
+  {
+  }
+
+  ROS_DEPRECATED Header_(const std_msgs::Header_& rhs)
+  {
+    *this = rhs;
+  }
+
+  ROS_DEPRECATED Type& operator=(const std_msgs::Header_& rhs)
+  {
+    if (this == &rhs)
+      return *this;
+    this->seq = rhs.seq;
+    this->stamp = rhs.stamp;
+    this->frame_id = rhs.frame_id;
+    return *this;
+  }
+
+  ROS_DEPRECATED operator std_msgs::Header_()
+  {
+    std_msgs::Header_ h;
+    h.seq = this->seq;
+    h.stamp = this->stamp;
+    h.frame_id = this->frame_id;
+    return h;
+  }
+
+private:
+  static const char* __s_getDataType_() { return "roslib/Header"; }
+public:
+   static const std::string __s_getDataType() { return __s_getDataType_(); }
+
+   const std::string __getDataType() const { return __s_getDataType_(); }
+
+private:
+  static const char* __s_getMD5Sum_() { return "2176decaecbce78abc3b96ef049fabed"; }
+public:
+   static const std::string __s_getMD5Sum() { return __s_getMD5Sum_(); }
+
+   const std::string __getMD5Sum() const { return __s_getMD5Sum_(); }
+
+private:
+  static const char* __s_getMessageDefinition_() { return "# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.secs: seconds (stamp_secs) since epoch\n\
+# * stamp.nsecs: nanoseconds since stamp_secs\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+"; }
+public:
+   static const std::string __s_getMessageDefinition() { return __s_getMessageDefinition_(); }
+
+   const std::string __getMessageDefinition() const { return __s_getMessageDefinition_(); }
+
+   virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const
+  {
+    ros::serialization::OStream stream(write_ptr, 1000000000);
+    ros::serialization::serialize(stream, this->seq);
+    ros::serialization::serialize(stream, this->stamp);
+    ros::serialization::serialize(stream, this->frame_id);
+    return stream.getData();
+  }
+
+   virtual uint8_t *deserialize(uint8_t *read_ptr)
+  {
+    ros::serialization::IStream stream(read_ptr, 1000000000);
+    ros::serialization::deserialize(stream, this->seq);
+    ros::serialization::deserialize(stream, this->stamp);
+    ros::serialization::deserialize(stream, this->frame_id);
+    return stream.getData();
+  }
+
+   virtual uint32_t serializationLength() const
+  {
+    uint32_t size = 0;
+    size += ros::serialization::serializationLength(this->seq);
+    size += ros::serialization::serializationLength(this->stamp);
+    size += ros::serialization::serializationLength(this->frame_id);
+    return size;
+  }
+
+  typedef boost::shared_ptr< ::roslib::Header_ > Ptr;
+  typedef boost::shared_ptr< ::roslib::Header_  const> ConstPtr;
+}; // struct Header
+typedef  ::roslib::Header_ > Header;
+
+typedef boost::shared_ptr< ::roslib::Header> HeaderPtr;
+typedef boost::shared_ptr< ::roslib::Header const> HeaderConstPtr;
+
+
+template
+std::ostream& operator<<(std::ostream& s, const  ::roslib::Header_ & v)
+{
+  ros::message_operations::Printer< ::roslib::Header_ >::stream(s, "", v);
+  return s;}
+
+} // namespace roslib
+
+namespace ros
+{
+namespace message_traits
+{
+template
+struct MD5Sum< ::roslib::Header_ > {
+  static const char* value() 
+  {
+    return "2176decaecbce78abc3b96ef049fabed";
+  }
+
+  static const char* value(const  ::roslib::Header_ &) { return value(); } 
+  static const uint64_t static_value1 = 0x2176decaecbce78aULL;
+  static const uint64_t static_value2 = 0xbc3b96ef049fabedULL;
+};
+
+template
+struct DataType< ::roslib::Header_ > {
+  static const char* value() 
+  {
+    return "roslib/Header";
+  }
+
+  static const char* value(const  ::roslib::Header_ &) { return value(); } 
+};
+
+template
+struct Definition< ::roslib::Header_ > {
+  static const char* value() 
+  {
+    return "# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.secs: seconds (stamp_secs) since epoch\n\
+# * stamp.nsecs: nanoseconds since stamp_secs\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+";
+  }
+
+  static const char* value(const  ::roslib::Header_ &) { return value(); } 
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+template struct Serializer< ::roslib::Header_ >
+{
+  template inline static void allInOne(Stream& stream, T m)
+  {
+    stream.next(m.seq);
+    stream.next(m.stamp);
+    stream.next(m.frame_id);
+  }
+
+  ROS_DECLARE_ALLINONE_SERIALIZER;
+}; // struct Header_
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template
+struct Printer< ::roslib::Header_ >
+{
+  template static void stream(Stream& s, const std::string& indent, const  ::roslib::Header_ & v) 
+  {
+    s << indent << "seq: ";
+    Printer::stream(s, indent + "  ", v.seq);
+    s << indent << "stamp: ";
+    Printer::stream(s, indent + "  ", v.stamp);
+    s << indent << "frame_id: ";
+    Printer, typename ContainerAllocator::template rebind::other > >::stream(s, indent + "  ", v.frame_id);
+  }
+};
+
+
+} // namespace message_operations
+} // namespace ros
+
diff --git a/thirdparty/ros/include/std_msgs/trait_macros.h b/thirdparty/ros/include/std_msgs/trait_macros.h
new file mode 100644
index 0000000..3efa852
--- /dev/null
+++ b/thirdparty/ros/include/std_msgs/trait_macros.h
@@ -0,0 +1,80 @@
+
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Willow Garage, Inc. 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ */
+
+#ifndef STD_MSGS_TRAIT_MACROS_H
+#define STD_MSGS_TRAIT_MACROS_H
+
+#define STD_MSGS_DEFINE_BUILTIN_TRAITS(builtin, msg, static_md5sum1, static_md5sum2) \
+  namespace ros \
+  { \
+  namespace message_traits \
+  { \
+    \
+    template<> struct MD5Sum \
+    { \
+      static const char* value() \
+      { \
+        return MD5Sum::value(); \
+      } \
+      \
+      static const char* value(const builtin&) \
+      { \
+        return value(); \
+      } \
+    }; \
+    \
+    template<> struct DataType \
+    { \
+      static const char* value() \
+      { \
+        return DataType::value(); \
+      } \
+     \
+      static const char* value(const builtin&) \
+      { \
+        return value(); \
+      } \
+    }; \
+    \
+    template<> struct Definition \
+    { \
+      static const char* value() \
+      { \
+        return Definition::value(); \
+      } \
+      \
+      static const char* value(const builtin&) \
+      { \
+        return value(); \
+      } \
+    }; \
+    \
+  } \
+  }
+
+#endif // STD_MSGS_TRAIT_MACROS_H
diff --git a/thirdparty/ros/ros_comm b/thirdparty/ros/ros_comm
new file mode 160000
index 0000000..2c8a3f6
--- /dev/null
+++ b/thirdparty/ros/ros_comm
@@ -0,0 +1 @@
+Subproject commit 2c8a3f65628d0362d97c29d333d5669b2c7803a6
diff --git a/thirdparty/ros/roscpp_core b/thirdparty/ros/roscpp_core
new file mode 160000
index 0000000..d2bb8cb
--- /dev/null
+++ b/thirdparty/ros/roscpp_core
@@ -0,0 +1 @@
+Subproject commit d2bb8cbcfd4944e48d16cdb20edfab5d225db265