ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit

This commit is contained in:
Vladyslav Usenko 2019-04-14 21:07:42 +02:00
commit 0cef943b16
293 changed files with 55391 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.clang-format Normal file
View File

@ -0,0 +1,6 @@
---
Language: Cpp
BasedOnStyle: Google
IndentWidth: 2
...

7
.gitignore vendored Normal file
View File

@ -0,0 +1,7 @@
cmake-build*
.idea
CMakeLists.txt.user
build
thirdparty/build-*
scripts/eval/eval_*
scripts/eval_full/eval_*

76
.gitlab-ci.yml Normal file
View File

@ -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/

21
.gitmodules vendored Normal file
View File

@ -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

210
CMakeLists.txt Normal file
View File

@ -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 <vlad.usenko@tum.de>")
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()

29
LICENSE Normal file
View File

@ -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.

62
README.md Normal file
View File

@ -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.

83
cmake_modules/FindEigen3.cmake Normal file
View File

@ -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, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# 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)

85
cmake_modules/FindOpenGV.cmake Normal file
View File

@ -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)

283
cmake_modules/FindTBB.cmake Normal file
View File

@ -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 <hannes.hofmann _at_ informatik.uni-erlangen.de>
# Improvements by Gino van den Bergen <gino _at_ dtecta.com>,
# Florian Uhlig <F.Uhlig _at_ gsi.de>,
# Jiri Marsik <jiri.marsik89 _at_ gmail.com>
# 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)

30
data/euroc_config.json Normal file
View File

@ -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
}
}

213
data/euroc_ds_calib.json Normal file
View File

@ -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
}
}

213
data/euroc_eucm_calib.json Normal file
View File

@ -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
}
}

30
data/tumvi_512_config.json Normal file
View File

@ -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
}
}

381
data/tumvi_512_ds_calib.json Normal file
View File

@ -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
]
]
}
]
}
}

381
data/tumvi_512_eucm_calib.json Normal file
View File

@ -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
]
]
}
]
}
}

117
doc/Calibration.md Normal file
View File

@ -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)

50
doc/Simulation.md Normal file
View File

@ -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.

86
doc/VioMapping.md Normal file
View File

@ -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)

BIN
doc/img/MH_05_MAPPING.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 407 KiB

BIN
doc/img/MH_05_OPT_FLOW.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 543 KiB

BIN
doc/img/MH_05_VIO.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 334 KiB

BIN
doc/img/SIM_MAPPER.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 46 KiB

BIN
doc/img/SIM_VIO.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 140 KiB

BIN
doc/img/euroc_cam_calib.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 413 KiB

BIN
doc/img/euroc_imu_calib.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 601 KiB

BIN
doc/img/teaser.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 269 KiB

BIN
doc/img/tumvi_cam_calib.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 365 KiB

BIN
doc/img/tumvi_imu_calib.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 401 KiB

3
docker/b_image/Dockerfile Normal file
View File

@ -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

17
docker/b_image_xenial/Dockerfile Normal file
View File

@ -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

137
include/basalt/calibration/aprilgrid.h Normal file
View File

@ -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 <basalt/utils/sophus_utils.hpp>
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<Eigen::Vector4d> aprilgrid_corner_pos_3d;
Eigen::vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
};
} // namespace basalt

116
include/basalt/calibration/calibration_helper.h Normal file
View File

@ -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 <basalt/io/dataset_io.h>
#include <basalt/calibration/calibration.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
namespace basalt {
struct CalibCornerData {
Eigen::vector<Eigen::Vector2d> corners;
std::vector<int> corner_ids;
std::vector<double> 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<Eigen::Vector2d> reprojected_corners;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class CalibHelper {
public:
static void detectCorners(
const VioDatasetPtr& vio_data,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>& calib_corners,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>&
calib_corners_rejected);
static void initCamPoses(
const Calibration<double>::Ptr& calib, const VioDatasetPtr& vio_data,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>& calib_corners,
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData>&
calib_init_poses);
static bool initializeIntrinsics(
const Eigen::vector<Eigen::Vector2d>& corners,
const std::vector<int>& corner_ids,
const Eigen::vector<Eigen::Vector4d>& 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<double>::Ptr& calib, size_t cam_id,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp);
static size_t computeReprojectionError(
const UnifiedCamera<double>& cam_calib,
const Eigen::vector<Eigen::Vector2d>& corners,
const std::vector<int>& corner_ids,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
const Sophus::SE3d& T_target_camera, double& error);
};
} // namespace basalt
namespace cereal {
template <class Archive>
void serialize(Archive& ar, basalt::CalibCornerData& c) {
ar(c.corners, c.corner_ids, c.radii);
}
template <class Archive>
void serialize(Archive& ar, basalt::CalibInitPoseData& c) {
ar(c.T_a_c, c.num_inliers, c.reprojected_corners);
}
} // namespace cereal

165
include/basalt/calibration/cam_calib.h Normal file
View File

@ -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 <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include <thread>
#include <basalt/calibration/aprilgrid.h>
#include <basalt/calibration/calibration_helper.h>
#include <basalt/utils/image.h>
#include <basalt/utils/test_utils.h>
#include <basalt/utils/sophus_utils.hpp>
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<std::string> &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<std::string, double> *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<TimeCamId, CalibCornerData> calib_corners;
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>
calib_corners_rejected;
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData> calib_init_poses;
std::shared_ptr<std::thread> processing_thread;
std::shared_ptr<PosesOptimization> calib_opt;
std::map<TimeCamId, Eigen::vector<Eigen::Vector2d>> reprojected_corners;
std::map<TimeCamId, Eigen::vector<Eigen::Vector2d>> reprojected_vignette;
std::map<TimeCamId, std::vector<double>> 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<std::string> cam_types;
bool show_gui;
const size_t MIN_CORNERS = 15;
//////////////////////
pangolin::Var<int> show_frame;
pangolin::Var<bool> show_corners;
pangolin::Var<bool> show_corners_rejected;
pangolin::Var<bool> show_init_reproj;
pangolin::Var<bool> show_opt;
pangolin::Var<bool> show_vign;
pangolin::Var<bool> show_ids;
pangolin::Var<double> huber_thresh;
pangolin::Var<bool> opt_intr;
std::shared_ptr<pangolin::Plotter> vign_plotter;
pangolin::View *img_view_display;
std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
pangolin::DataLog vign_data_log;
};
} // namespace basalt

180
include/basalt/calibration/cam_imu_calib.h Normal file
View File

@ -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 <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include <thread>
#include <basalt/calibration/aprilgrid.h>
#include <basalt/calibration/calibration_helper.h>
#include <basalt/utils/image.h>
#include <basalt/utils/test_utils.h>
#include <basalt/utils/sophus_utils.hpp>
namespace basalt {
template <int N, typename Scalar>
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<double> &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<std::string, double> *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<TimeCamId, CalibCornerData> calib_corners;
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>
calib_corners_rejected;
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData> calib_init_poses;
std::shared_ptr<std::thread> processing_thread;
std::shared_ptr<SplineOptimization<5, double>> calib_opt;
std::map<TimeCamId, Eigen::vector<Eigen::Vector2d>> 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<double> imu_noise;
//////////////////////
pangolin::Var<int> show_frame;
pangolin::Var<bool> show_corners;
pangolin::Var<bool> show_corners_rejected;
pangolin::Var<bool> show_init_reproj;
pangolin::Var<bool> show_opt;
pangolin::Var<bool> show_ids;
pangolin::Var<bool> show_accel;
pangolin::Var<bool> show_gyro;
pangolin::Var<bool> show_pos;
pangolin::Var<bool> show_rot_error;
pangolin::Var<bool> show_mocap;
pangolin::Var<bool> show_mocap_rot_error;
pangolin::Var<bool> show_mocap_rot_vel;
pangolin::Var<bool> show_spline;
pangolin::Var<bool> show_data;
pangolin::Var<bool> opt_intr;
pangolin::Var<bool> opt_poses;
pangolin::Var<bool> opt_corners;
pangolin::Var<bool> opt_cam_time_offset;
pangolin::Var<bool> opt_imu_scale;
pangolin::Var<bool> opt_mocap;
pangolin::Var<double> huber_thresh;
pangolin::Plotter *plotter;
pangolin::View *img_view_display;
std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
pangolin::DataLog imu_data_log, pose_data_log, mocap_data_log, vign_data_log;
};
} // namespace basalt

87
include/basalt/calibration/vignette.h Normal file
View File

@ -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 <basalt/calibration/aprilgrid.h>
#include <basalt/calibration/calibration_helper.h>
#include <basalt/spline/rd_spline.h>
#include <pangolin/plot/datalog.h>
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<Eigen::Vector2d> &optical_centers,
const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>>
&reprojected_vignette,
const AprilGrid &april_grid);
void compute_error(std::map<TimeCamId, std::vector<double>>
*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<basalt::RdSpline<1, SPLINE_N>> &get_vign_param() {
return vign_param;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
const VioDatasetPtr vio_dataset;
Eigen::vector<Eigen::Vector2d> optical_centers;
std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> reprojected_vignette;
const AprilGrid &april_grid;
size_t vign_size;
std::vector<double> irradiance;
std::vector<basalt::RdSpline<1, SPLINE_N>> vign_param;
};
}

247
include/basalt/io/dataset_io.h Normal file
View File

@ -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 <array>
#include <fstream>
#include <iomanip>
#include <memory>
#include <string>
#include <vector>
#include <cereal/archives/binary.hpp>
#include <cereal/archives/json.hpp>
#include <cereal/types/bitset.hpp>
#include <cereal/types/deque.hpp>
#include <cereal/types/map.hpp>
#include <cereal/types/memory.hpp>
#include <cereal/types/polymorphic.hpp>
#include <cereal/types/set.hpp>
#include <cereal/types/string.hpp>
#include <cereal/types/unordered_map.hpp>
#include <cereal/types/utility.hpp>
#include <cereal/types/vector.hpp>
#include <Eigen/Dense>
#include <basalt/utils/sophus_utils.hpp>
#include <basalt/utils/assert.h>
#include <basalt/utils/image.h>
#include <basalt/camera/generic_camera.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <pangolin/image/image_io.h>
namespace basalt {
inline bool file_exists(const std::string &name) {
std::ifstream f(name.c_str());
return f.good();
}
typedef std::pair<int64_t, size_t> TimeCamId;
struct ImageData {
ImageData() : exposure(0) {}
ManagedImage<uint16_t>::Ptr img;
double exposure;
};
struct Observations {
Eigen::vector<Eigen::Vector2d> pos;
std::vector<int> 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<Eigen::Vector2d> corner_pos;
std::vector<int> corner_id;
};
class VioDataset {
public:
virtual ~VioDataset(){};
virtual size_t get_num_cams() const = 0;
virtual std::vector<int64_t> &get_image_timestamps() = 0;
virtual const Eigen::vector<AccelData> &get_accel_data() const = 0;
virtual const Eigen::vector<GyroData> &get_gyro_data() const = 0;
virtual const std::vector<int64_t> &get_gt_timestamps() const = 0;
virtual const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const = 0;
virtual int64_t get_mocap_to_imu_offset_ns() const = 0;
virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
typedef std::shared_ptr<VioDataset> 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<DatasetIoInterface> DatasetIoInterfacePtr;
class DatasetIoFactory {
public:
static DatasetIoInterfacePtr getDatasetIo(const std::string &dataset_type,
bool with_images = true);
};
} // namespace basalt
namespace cereal {
template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
int _MaxRows, int _MaxCols>
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<std::int32_t>(matrix.rows());
const std::int32_t cols = static_cast<std::int32_t>(matrix.cols());
ar(rows);
ar(cols);
ar(binary_data(matrix.data(), rows * cols * sizeof(_Scalar)));
};
template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
int _MaxRows, int _MaxCols>
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<std::size_t>(rows * cols * sizeof(_Scalar))));
};
template <class Archive>
void serialize(Archive &archive, basalt::ManagedImage<uint8_t> &m) {
archive(m.w);
archive(m.h);
m.Reinitialise(m.w, m.h);
archive(binary_data(m.ptr, m.size()));
}
template <class Archive>
void serialize(Archive &ar, basalt::GyroData &c) {
ar(c.timestamp_ns, c.data);
}
template <class Archive>
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<basalt::TimeCamId> {
size_t operator()(const basalt::TimeCamId &x) const {
size_t seed = 0;
hash_combine(seed, std::hash<int>()(x.first));
hash_combine(seed, std::hash<int>()(x.second));
return seed;
}
};
template <>
struct hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>> {
size_t operator()(
const std::pair<basalt::TimeCamId, basalt::TimeCamId> &x) const {
size_t seed = 0;
hash_combine(seed, std::hash<int>()(x.first.first));
hash_combine(seed, std::hash<int>()(x.first.second));
hash_combine(seed, std::hash<int>()(x.second.first));
hash_combine(seed, std::hash<int>()(x.second.second));
return seed;
}
};
} // namespace std
#endif // DATASET_IO_H

248
include/basalt/io/dataset_io_euroc.h Normal file
View File

@ -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 <basalt/io/dataset_io.h>
namespace basalt {
class EurocVioDataset : public VioDataset {
size_t num_cams;
std::string path;
std::vector<int64_t> image_timestamps;
std::unordered_map<int64_t, std::string> 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<int64_t, std::vector<ImageData>> image_data;
Eigen::vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> 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<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; }
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; }
const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps;
}
const Eigen::vector<Sophus::SE3d> &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<ImageData> get_image_data(int64_t t_ns) {
std::vector<ImageData> res(num_cams);
const std::vector<std::string> 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<uint16_t>(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<uint16_t>(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<EurocVioDataset> data;
};
} // namespace basalt
#endif // DATASET_IO_H

375
include/basalt/io/dataset_io_rosbag.h Normal file
View File

@ -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 <mutex>
#include <optional>
#include <basalt/io/dataset_io.h>
// Hack to access private functions
#define private public
#include <rosbag/bag.h>
#include <rosbag/view.h>
#undef private
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
namespace basalt {
class RosbagVioDataset : public VioDataset {
std::shared_ptr<rosbag::Bag> bag;
std::mutex m;
size_t num_cams;
std::vector<int64_t> 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<int64_t, std::vector<std::optional<rosbag::IndexEntry>>>
image_data_idx;
Eigen::vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> 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<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; }
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; }
const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps;
}
const Eigen::vector<Sophus::SE3d> &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<ImageData> get_image_data(int64_t t_ns) {
std::vector<ImageData> 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<sensor_msgs::Image>(*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<uint16_t>(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<const rosbag::ConnectionInfo *> connection_infos =
view.getConnections();
std::set<std::string> 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<std::string, int> 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<int64_t>::max();
int64_t max_time = std::numeric_limits<int64_t>::min();
std::vector<geometry_msgs::TransformStampedConstPtr> mocap_msgs;
std::vector<geometry_msgs::PointStampedConstPtr> point_msgs;
std::vector<int64_t>
system_to_imu_offset_vec; // t_imu = t_system + system_to_imu_offset
std::vector<int64_t> system_to_mocap_offset_vec; // t_mocap = t_system +
// system_to_mocap_offset
std::set<int64_t> 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<sensor_msgs::Image>();
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<sensor_msgs::Imu>();
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<geometry_msgs::TransformStamped>();
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<geometry_msgs::PointStamped>();
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<VioDataset>(data);
return data;
}
private:
std::shared_ptr<RosbagVioDataset> data;
bool with_images;
};
} // namespace basalt
#endif // DATASET_IO_ROSBAG_H

76
include/basalt/io/marg_data_io.h Normal file
View File

@ -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 <memory>
#include <thread>
#include <basalt/utils/imu_types.h>
namespace basalt {
class MargDataSaver {
public:
using Ptr = std::shared_ptr<MargDataSaver>;
MargDataSaver(const std::string& path);
~MargDataSaver() {
saving_thread->join();
saving_img_thread->join();
}
tbb::concurrent_bounded_queue<MargData::Ptr> in_marg_queue;
private:
std::shared_ptr<std::thread> saving_thread;
std::shared_ptr<std::thread> saving_img_thread;
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr> save_image_queue;
};
class MargDataLoader {
public:
using Ptr = std::shared_ptr<MargDataLoader>;
MargDataLoader();
void start(const std::string& path);
~MargDataLoader() { processing_thread->join(); }
tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue;
private:
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

382
include/basalt/optical_flow/frame_to_frame_optical_flow.h Normal file
View File

@ -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 <thread>
#include <tbb/blocked_range.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/patch.h>
#include <basalt/utils/keypoints.h>
#include <tbb/parallel_for.h>
namespace basalt {
template <typename Scalar, template <typename> typename Pattern>
class FrameToFrameOpticalFlow : public OpticalFlowBase {
public:
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Sophus::SE2<Scalar> SE2;
FrameToFrameOpticalFlow(const VioConfig& config,
const basalt::Calibration<double>& calib)
: t_ns(-1), last_keypoint_id(0), config(config) {
input_queue.set_capacity(10);
this->calib = calib.cast<Scalar>();
patch_coord = PatchT::pattern2.template cast<float>();
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<Scalar>();
}
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<basalt::ManagedImagePyr<u_int16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& 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<basalt::ManagedImagePyr<u_int16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& 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<u_int16_t>& pyr_1,
const basalt::ManagedImagePyr<u_int16_t>& pyr_2,
const Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_1,
Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_2) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
Eigen::vector<Eigen::AffineCompact2f> 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<KeypointId, Eigen::AffineCompact2f> result;
auto compute_func = [&](const tbb::blocked_range<size_t>& 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<size_t> 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<uint16_t>& old_pyr,
const basalt::ManagedImagePyr<uint16_t>& 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<const u_int16_t>& 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<Eigen::Vector2d> pts0;
for (const auto& kv : transforms->observations.at(0)) {
pts0.emplace_back(kv.second.translation().cast<double>());
}
KeypointsData kd;
detectKeypoints(pyramid->at(0).lvl(0), kd,
config.optical_flow_detection_grid_size, 1, pts0);
Eigen::map<KeypointId, Eigen::AffineCompact2f> 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<Scalar>();
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<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid;
Eigen::vector<Eigen::Vector2f> 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<Eigen::Vector4f> p3d0, p3d1;
std::vector<bool> 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<Scalar> calib;
OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<u_int16_t>>> old_pyramid,
pyramid;
Matrix4 E;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

85
include/basalt/optical_flow/optical_flow.h Normal file
View File

@ -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 <memory>
#include <Eigen/Geometry>
#include <basalt/utils/vio_config.h>
#include <basalt/io/dataset_io.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <basalt/utils/sophus_utils.hpp>
#include <tbb/concurrent_queue.h>
namespace basalt {
using KeypointId = uint32_t;
struct OpticalFlowInput {
using Ptr = std::shared_ptr<OpticalFlowInput>;
int64_t t_ns;
std::vector<ImageData> img_data;
};
struct OpticalFlowResult {
using Ptr = std::shared_ptr<OpticalFlowResult>;
int64_t t_ns;
std::vector<Eigen::map<KeypointId, Eigen::AffineCompact2f>> observations;
OpticalFlowInput::Ptr input_images;
};
class OpticalFlowBase {
public:
using Ptr = std::shared_ptr<OpticalFlowBase>;
tbb::concurrent_bounded_queue<OpticalFlowInput::Ptr> input_queue;
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr>* output_queue = nullptr;
Eigen::MatrixXf patch_coord;
};
class OpticalFlowFactory {
public:
static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config,
const Calibration<double>& cam);
};
} // namespace basalt

177
include/basalt/optical_flow/patch.h Normal file
View File

@ -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 <Eigen/Dense>
#include <basalt/optical_flow/patterns.h>
#include <basalt/utils/image.h>
namespace basalt {
template <typename Scalar, typename Pattern>
struct OpticalFlowPatch {
static constexpr int PATTERN_SIZE = Pattern::PATTERN_SIZE;
typedef Eigen::Matrix<int, 2, 1> Vector2i;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 1, 2> Vector2T;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 1> VectorP;
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 2> MatrixP2;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 3> MatrixP3;
typedef Eigen::Matrix<Scalar, 3, PATTERN_SIZE> Matrix3P;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 4> MatrixP4;
typedef Eigen::Matrix<int, 2, PATTERN_SIZE> Matrix2Pi;
static const Matrix2P pattern2;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
OpticalFlowPatch() { mean = 0; }
OpticalFlowPatch(const Image<const uint16_t> &img, const Vector2 &pos) {
setFromImage(img, pos);
}
void setFromImage(const Image<const uint16_t> &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<Scalar>(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<Scalar, 2, 3> 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<const uint16_t> &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<Scalar>(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 <typename Scalar, typename Pattern>
const typename OpticalFlowPatch<Scalar, Pattern>::Matrix2P
OpticalFlowPatch<Scalar, Pattern>::pattern2 = Pattern::pattern2;
} // namespace basalt

376
include/basalt/optical_flow/patch_optical_flow.h Normal file
View File

@ -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 <thread>
#include <tbb/blocked_range.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/patch.h>
#include <basalt/utils/keypoints.h>
namespace basalt {
template <typename Scalar, template <typename> typename Pattern>
class PatchOpticalFlow : public OpticalFlowBase {
public:
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Sophus::SE2<Scalar> SE2;
PatchOpticalFlow(const VioConfig& config,
const basalt::Calibration<double>& 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<float>();
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<basalt::ManagedImagePyr<u_int16_t>>);
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<basalt::ManagedImagePyr<u_int16_t>>);
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<u_int16_t>& pyr_1,
const basalt::ManagedImagePyr<u_int16_t>& pyr_2,
const Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_1,
Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_2) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
Eigen::vector<Eigen::AffineCompact2f> 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<KeypointId, Eigen::AffineCompact2f> result;
auto compute_func = [&](const tbb::blocked_range<size_t>& 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<PatchT>& 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<size_t> 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<uint16_t>& pyr,
const Eigen::vector<PatchT>& 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<const u_int16_t>& 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<Eigen::Vector2d> pts0;
for (const auto& kv : transforms->observations.at(0)) {
pts0.emplace_back(kv.second.translation().cast<double>());
}
KeypointsData kd;
detectKeypoints(pyramid->at(0).lvl(0), kd,
config.optical_flow_detection_grid_size, 1, pts0);
Eigen::map<KeypointId, Eigen::AffineCompact2f> new_poses0, new_poses1;
for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::vector<PatchT>& p = patches[last_keypoint_id];
Vector2 pos = kd.corners[i].cast<Scalar>();
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<Scalar>();
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<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid;
Eigen::vector<Eigen::Vector2d> 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<double>());
proj1.emplace_back(kv.second.translation().cast<double>());
kpid.emplace_back(kv.first);
}
}
Eigen::vector<Eigen::Vector4d> p3d0, p3d1;
std::vector<bool> 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<double> calib;
Eigen::unordered_map<KeypointId, Eigen::vector<PatchT>> patches;
OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<u_int16_t>>> old_pyramid,
pyramid;
Eigen::Matrix4d E;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

171
include/basalt/optical_flow/patterns.h Normal file
View File

@ -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 <Eigen/Dense>
namespace basalt {
template <class Scalar>
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<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern24<Scalar>::Matrix2P Pattern24<Scalar>::pattern2 =
Eigen::Map<Pattern24<Scalar>::Matrix2P>((Scalar *)
Pattern24<Scalar>::pattern_raw);
template <class Scalar>
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<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern52<Scalar>::Matrix2P Pattern52<Scalar>::pattern2 =
Eigen::Map<Pattern52<Scalar>::Matrix2P>((Scalar *)
Pattern52<Scalar>::pattern_raw);
// Same as Pattern52 but twice smaller
template <class Scalar>
struct Pattern51 {
static constexpr int PATTERN_SIZE = Pattern52<Scalar>::PATTERN_SIZE;
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern51<Scalar>::Matrix2P Pattern51<Scalar>::pattern2 =
0.5 * Pattern52<Scalar>::pattern2;
// Same as Pattern52 but 0.75 smaller
template <class Scalar>
struct Pattern50 {
static constexpr int PATTERN_SIZE = Pattern52<Scalar>::PATTERN_SIZE;
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern50<Scalar>::Matrix2P Pattern50<Scalar>::pattern2 =
0.75 * Pattern52<Scalar>::pattern2;
} // namespace basalt

336
include/basalt/optimization/accumulator.h Normal file
View File

@ -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 <Eigen/Dense>
#include <Eigen/Sparse>
#include <array>
#include <chrono>
#include <unordered_map>
#include <basalt/utils/assert.h>
namespace basalt {
template <typename Scalar = double>
class DenseAccumulator {
public:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
template <int ROWS, int COLS, typename Derived>
inline void addH(int i, int j, const Eigen::MatrixBase<Derived>& 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<ROWS, COLS>(i, j) += data;
}
template <int ROWS, typename Derived>
inline void addB(int i, const Eigen::MatrixBase<Derived>& 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<ROWS>(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<Scalar>& 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 <typename Scalar = double>
class SparseAccumulator {
public:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Triplet<Scalar> T;
typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
template <int ROWS, int COLS, typename Derived>
inline void addH(int si, int sj, const Eigen::MatrixBase<Derived>& 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 <int ROWS, typename Derived>
inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
b.template segment<ROWS>(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<SparseMatrix> 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<Scalar>& 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<T> triplets;
VectorX b;
};
template <typename Scalar = double>
class SparseHashAccumulator {
public:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Eigen::Triplet<Scalar> T;
typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
template <int ROWS, int COLS, typename Derived>
inline void addH(int si, int sj, const Eigen::MatrixBase<Derived>& 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 <int ROWS, typename Derived>
inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
b.template segment<ROWS>(i) += data;
}
inline VectorX solve() const {
SparseMatrix sm(b.rows(), b.rows());
auto t1 = std::chrono::high_resolution_clock::now();
std::vector<T> 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::SparseMatrix<double>,
Eigen::Lower | Eigen::Upper>
cg;
cg.setTolerance(tolerance);
cg.compute(sm);
res = cg.solve(b);
} else {
Eigen::SimplicialLDLT<SparseMatrix> chol(sm);
res = chol.solve(b);
}
auto t3 = std::chrono::high_resolution_clock::now();
auto elapsed1 =
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
auto elapsed2 =
std::chrono::duration_cast<std::chrono::microseconds>(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<Scalar>& 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<int, 4>;
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<int>()(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<KeyT, MatrixX, KeyHash> hash_map;
VectorX b;
};
} // namespace basalt
#endif

187
include/basalt/optimization/linearize.h Normal file
View File

@ -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 <basalt/io/dataset_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <pangolin/image/managed_image.h>
#include <tbb/tbb.h>
namespace basalt {
template <typename Scalar>
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<Scalar> SE3;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef typename Eigen::vector<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::vector<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::vector<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::vector<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
template <int INTRINSICS_SIZE>
struct PoseCalibH {
Sophus::Matrix6d H_pose_accum;
Sophus::Vector6d b_pose_accum;
Eigen::Matrix<double, INTRINSICS_SIZE, 6> H_intr_pose_accum;
Eigen::Matrix<double, INTRINSICS_SIZE, INTRINSICS_SIZE> H_intr_accum;
Eigen::Matrix<double, INTRINSICS_SIZE, 1> 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<Scalar>* calibration = nullptr;
const MocapCalibration<Scalar>* mocap_calibration = nullptr;
const Eigen::vector<Eigen::Vector4d>* aprilgrid_corner_pos_3d = nullptr;
// Calib data
const std::vector<size_t>* offset_T_i_c = nullptr;
const std::vector<size_t>* offset_intrinsics = nullptr;
// Cam data
size_t mocap_block_offset;
size_t bias_block_offset;
const std::unordered_map<int64_t, size_t>* 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 <class CamT>
inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id,
const Eigen::Matrix4d& T_c_w, const CamT& cam,
PoseCalibH<CamT::N>& cph, double& error,
int& num_points, double& reproj_err) const {
Eigen::Matrix<double, 2, 4> d_r_d_p;
Eigen::Matrix<double, 2, CamT::N> d_r_d_param;
Eigen::Matrix<double, 4, 6> 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<double, 2, 6> 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

180
include/basalt/optimization/poses_linearize.h Normal file
View File

@ -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 <basalt/io/dataset_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/optimization/linearize.h>
#include <basalt/optimization/accumulator.h>
#include <tbb/tbb.h>
namespace basalt {
template <typename Scalar, typename AccumT>
struct LinearizePosesOpt : public LinearizeBase<Scalar> {
static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
typedef Sophus::SE3<Scalar> SE3;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef typename Eigen::vector<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
AccumT accum;
Scalar error;
Scalar reprojection_error;
int num_points;
size_t opt_size;
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose;
LinearizePosesOpt(size_t opt_size,
const Eigen::unordered_map<int64_t, SE3>& 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<AprilgridCornersDataIter>& r) {
for (const AprilgridCornersData& acd : r) {
std::visit(
[&](const auto& cam) {
constexpr int INTRINSICS_SIZE =
std::remove_reference<decltype(cam)>::type::N;
typename LinearizeBase<Scalar>::template PoseCalibH<INTRINSICS_SIZE>
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<POSE_SIZE, POSE_SIZE>(
po, po, Adj.transpose() * cph.H_pose_accum * Adj);
accum.template addB<POSE_SIZE>(po,
Adj.transpose() * cph.b_pose_accum);
if (acd.cam_id > 0) {
accum.template addH<POSE_SIZE, POSE_SIZE>(
co, po, -cph.H_pose_accum * Adj);
accum.template addH<POSE_SIZE, POSE_SIZE>(co, co,
cph.H_pose_accum);
accum.template addB<POSE_SIZE>(co, -cph.b_pose_accum);
}
if (this->common_data.opt_intrinsics) {
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
io, po, cph.H_intr_pose_accum * Adj);
if (acd.cam_id > 0)
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
io, co, -cph.H_intr_pose_accum);
accum.template addH<INTRINSICS_SIZE, INTRINSICS_SIZE>(
io, io, cph.H_intr_accum);
accum.template addB<INTRINSICS_SIZE>(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

259
include/basalt/optimization/poses_optimize.h Normal file
View File

@ -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 <basalt/calibration/calibration_helper.h>
#include <basalt/optimization/poses_linearize.h>
#include <basalt/camera/unified_camera.hpp>
#include <basalt/utils/image.h>
namespace basalt {
class PosesOptimization {
static constexpr size_t POSE_SIZE = 6;
using Scalar = double;
typedef LinearizePosesOpt<Scalar, SparseHashAccumulator<Scalar>> 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<AprilgridCornersData>::const_iterator;
public:
PosesOptimization() {}
bool initializeIntrinsics(
size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &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<std::string> &cam_types) {
BASALT_ASSERT(cam_types.size() == num_cams);
calib.reset(new Calibration<Scalar>);
for (size_t i = 0; i < cam_types.size(); i++) {
calib->intrinsics.emplace_back(
GenericCamera<Scalar>::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<Scalar>);
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<double, SparseHashAccumulator<double>> lopt(
problem_size, timestam_to_pose, ccd);
tbb::blocked_range<AprilgridCornersDataIter> 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<POSE_SIZE>(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<POSE_SIZE>(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<Eigen::Vector4d> &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<Eigen::Vector2d> &corners_pos,
const std::vector<int> &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<basalt::RdSpline<1, 4>> &vign) {
calib->vignette = vign;
}
void setResolution(const Eigen::vector<Eigen::Vector2i> &resolution) {
calib->resolution = resolution;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
std::shared_ptr<Calibration<Scalar>> calib;
private:
typename LinearizePosesOpt<
Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
size_t problem_size;
std::unordered_map<int64_t, size_t> offset_poses;
std::vector<size_t> offset_cam_intrinsics;
std::vector<size_t> offset_T_i_c;
// frame poses
Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose;
Eigen::vector<AprilgridCornersData> aprilgrid_corners_measurements;
Eigen::vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
}; // namespace basalt
} // namespace basalt

635
include/basalt/optimization/spline_linearize.h Normal file
View File

@ -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 <basalt/io/dataset_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/camera/generic_camera.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <basalt/optimization/linearize.h>
#include <basalt/utils/test_utils.h>
#include <tbb/tbb.h>
namespace basalt {
template <int N, typename Scalar, typename AccumT>
struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
static const int POS_SIZE = LinearizeBase<Scalar>::POS_SIZE;
static const int POS_OFFSET = LinearizeBase<Scalar>::POS_OFFSET;
static const int ROT_SIZE = LinearizeBase<Scalar>::ROT_SIZE;
static const int ROT_OFFSET = LinearizeBase<Scalar>::ROT_OFFSET;
static const int ACCEL_BIAS_SIZE = LinearizeBase<Scalar>::ACCEL_BIAS_SIZE;
static const int GYRO_BIAS_SIZE = LinearizeBase<Scalar>::GYRO_BIAS_SIZE;
static const int G_SIZE = LinearizeBase<Scalar>::G_SIZE;
static const int TIME_SIZE = LinearizeBase<Scalar>::TIME_SIZE;
static const int ACCEL_BIAS_OFFSET = LinearizeBase<Scalar>::ACCEL_BIAS_OFFSET;
static const int GYRO_BIAS_OFFSET = LinearizeBase<Scalar>::GYRO_BIAS_OFFSET;
static const int G_OFFSET = LinearizeBase<Scalar>::G_OFFSET;
typedef Sophus::SE3<Scalar> SE3;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
typedef Eigen::Matrix<Scalar, 2, 4> Matrix24;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Se3Spline<N, Scalar> SplineT;
typedef typename Eigen::deque<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::deque<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef
typename Eigen::deque<MocapPoseData>::const_iterator MocapPoseDataIter;
// typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
typedef typename LinearizeBase<Scalar>::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<PoseDataIter>& 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<POS_SIZE, POS_SIZE>(
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<ROT_SIZE, ROT_SIZE>(
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<POS_SIZE>(
start_i + POS_OFFSET,
pose_var_inv * J_pos.d_val_d_knot[i] * residual_pos);
accum.template addB<ROT_SIZE>(
start_i + ROT_OFFSET,
pose_var_inv * J_rot.d_val_d_knot[i].transpose() * residual_rot);
}
}
}
void operator()(const tbb::blocked_range<AccelDataIter>& 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<POSE_SIZE, POSE_SIZE>(
start_i, start_j, accel_var_inv * J.d_val_d_knot[i].transpose() *
J.d_val_d_knot[j]);
}
accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
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<G_SIZE, POSE_SIZE>(
start_g, start_i,
accel_var_inv * J_g.transpose() * J.d_val_d_knot[i]);
}
accum.template addB<POSE_SIZE>(
start_i, accel_var_inv * J.d_val_d_knot[i].transpose() * residual);
}
accum.template addH<ACCEL_BIAS_SIZE, ACCEL_BIAS_SIZE>(
start_bias, start_bias, accel_var_inv * J_bias.transpose() * J_bias);
if (this->common_data.opt_g) {
accum.template addH<G_SIZE, ACCEL_BIAS_SIZE>(
start_g, start_bias, accel_var_inv * J_g.transpose() * J_bias);
accum.template addH<G_SIZE, G_SIZE>(
start_g, start_g, accel_var_inv * J_g.transpose() * J_g);
}
accum.template addB<ACCEL_BIAS_SIZE>(
start_bias, accel_var_inv * J_bias.transpose() * residual);
if (this->common_data.opt_g) {
accum.template addB<G_SIZE>(start_g,
accel_var_inv * J_g.transpose() * residual);
}
}
}
void operator()(const tbb::blocked_range<GyroDataIter>& 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<ROT_SIZE, ROT_SIZE>(
start_i, start_j,
gyro_var_inv * J.d_val_d_knot[i].transpose() * J.d_val_d_knot[j]);
}
accum.template addH<GYRO_BIAS_SIZE, ROT_SIZE>(
start_bias, start_i,
gyro_var_inv * J_bias.transpose() * J.d_val_d_knot[i]);
accum.template addB<ROT_SIZE>(
start_i, gyro_var_inv * J.d_val_d_knot[i].transpose() * residual);
}
accum.template addH<GYRO_BIAS_SIZE, GYRO_BIAS_SIZE>(
start_bias, start_bias, gyro_var_inv * J_bias.transpose() * J_bias);
accum.template addB<GYRO_BIAS_SIZE>(
start_bias, gyro_var_inv * J_bias.transpose() * residual);
}
}
void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
for (const AprilgridCornersData& acd : r) {
std::visit(
[&](const auto& cam) {
constexpr int INTRINSICS_SIZE =
std::remove_reference<decltype(cam)>::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<Scalar>::template PoseCalibH<INTRINSICS_SIZE>
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<POSE_SIZE, POSE_SIZE>(
start_i, start_j, Ji_A_H_p_A * J.d_val_d_knot[j]);
}
accum.template addH<POSE_SIZE, POSE_SIZE>(
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<INTRINSICS_SIZE, POSE_SIZE>(
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<TIME_SIZE, POSE_SIZE>(
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<POSE_SIZE>(
start_i, J.d_val_d_knot[i].transpose() * Adj.transpose() *
cph.b_pose_accum);
}
accum.template addH<POSE_SIZE, POSE_SIZE>(T_i_c_start, T_i_c_start,
cph.H_pose_accum);
accum.template addB<POSE_SIZE>(T_i_c_start, -cph.b_pose_accum);
if (this->common_data.opt_intrinsics) {
accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
calib_start, T_i_c_start, -cph.H_intr_pose_accum);
accum.template addH<INTRINSICS_SIZE, INTRINSICS_SIZE>(
calib_start, calib_start, cph.H_intr_accum);
accum.template addB<INTRINSICS_SIZE>(calib_start,
cph.b_intr_accum);
}
if (this->common_data.opt_cam_time_offset) {
accum.template addH<TIME_SIZE, POSE_SIZE>(
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<TIME_SIZE, INTRINSICS_SIZE>(
start_cam_time_offset, calib_start,
d_T_wi_d_time.transpose() * Adj.transpose() *
cph.H_intr_pose_accum.transpose());
accum.template addH<TIME_SIZE, TIME_SIZE>(
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<TIME_SIZE>(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<MocapPoseDataIter>& 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<POSE_SIZE, POSE_SIZE>(
start_i, start_j,
mocap_var_inv * Ji_H_T_w_i * J_pose.d_val_d_knot[j]);
}
accum.template addB<POSE_SIZE>(
start_i, mocap_var_inv * J_pose.d_val_d_knot[i].transpose() *
d_res_d_T_w_i.transpose() * residual);
accum.template addH<POSE_SIZE, POSE_SIZE>(
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<POSE_SIZE, POSE_SIZE>(
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<TIME_SIZE, POSE_SIZE>(
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<POSE_SIZE, POSE_SIZE>(
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<POSE_SIZE, POSE_SIZE>(
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<POSE_SIZE, POSE_SIZE>(
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<TIME_SIZE, POSE_SIZE>(
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<TIME_SIZE, POSE_SIZE>(
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<TIME_SIZE, TIME_SIZE>(
start_mocap_time_offset, start_mocap_time_offset,
mocap_var_inv * d_res_d_time.transpose() * d_res_d_time);
// B
accum.template addB<POSE_SIZE>(
start_T_moc_w,
mocap_var_inv * d_res_d_T_moc_w.transpose() * residual);
accum.template addB<POSE_SIZE>(
start_T_i_mark,
mocap_var_inv * d_res_d_T_i_mark.transpose() * residual);
accum.template addB<TIME_SIZE>(
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

488
include/basalt/optimization/spline_optimize.h Normal file
View File

@ -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 <basalt/optimization/accumulator.h>
#include <basalt/optimization/spline_linearize.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/camera/unified_camera.hpp>
#include <basalt/utils/image.h>
#include <basalt/serialization/headers_serialization.h>
#include <chrono>
namespace basalt {
template <int N, typename Scalar>
class SplineOptimization {
public:
typedef LinearizeSplineOpt<N, Scalar, SparseHashAccumulator<Scalar>>
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<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Se3Spline<N, Scalar> 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<int64_t>::max();
max_time_us = std::numeric_limits<int64_t>::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<Scalar>& getAccelBias() const {
return calib->calib_accel_bias;
}
const CalibGyroBias<Scalar>& getGyroBias() const {
return calib->calib_gyro_bias;
}
void resetCalib(size_t num_cams, const std::vector<std::string>& cam_types) {
BASALT_ASSERT(cam_types.size() == num_cams);
calib.reset(new Calibration<Scalar>);
calib->intrinsics.resize(num_cams);
calib->T_i_c.resize(num_cams);
mocap_calib.reset(new MocapCalibration<Scalar>);
}
void resetMocapCalib() { mocap_calib.reset(new MocapCalibration<Scalar>); }
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<Scalar>);
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<Eigen::Vector4d>& 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<Eigen::Vector2d>& corners_pos,
const std::vector<int>& 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<PoseDataIter> pose_range(pose_measurements.begin(),
pose_measurements.end());
tbb::parallel_reduce(pose_range, lopt);
// lopt(pose_range);
}
if (use_april_corners) {
tbb::blocked_range<AprilgridCornersDataIter> 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<MocapPoseDataIter> mocap_pose_range(
mocap_measurements.begin(), mocap_measurements.end());
tbb::parallel_reduce(mocap_pose_range, lopt);
// lopt(mocap_pose_range);
}
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
accel_measurements.end());
tbb::parallel_reduce(accel_range, lopt);
tbb::blocked_range<GyroDataIter> 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<Scalar>::Ptr calib;
typename MocapCalibration<Scalar>::Ptr mocap_calib;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
typedef typename Eigen::deque<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::deque<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef
typename Eigen::deque<MocapPoseData>::const_iterator MocapPoseDataIter;
void applyInc(VectorX& inc_full,
const std::vector<size_t>& 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>(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<ACCEL_BIAS_SIZE>(
bias_block_offset + ACCEL_BIAS_OFFSET);
calib->calib_gyro_bias += inc_full.template segment<GYRO_BIAS_SIZE>(
bias_block_offset + GYRO_BIAS_OFFSET);
g += inc_full.template segment<G_SIZE>(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<POSE_SIZE>(
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<POSE_SIZE>(mocap_block_offset));
mocap_calib->T_i_mark *= Sophus::expd(
inc_full.template segment<POSE_SIZE>(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<PoseData> pose_measurements;
Eigen::deque<GyroData> gyro_measurements;
Eigen::deque<AccelData> accel_measurements;
Eigen::deque<AprilgridCornersData> aprilgrid_corners_measurements;
Eigen::deque<MocapPoseData> mocap_measurements;
typename LinearizeT::CalibCommonData ccd;
std::vector<size_t> offset_cam_intrinsics;
std::vector<size_t> offset_T_i_c;
size_t mocap_block_offset;
size_t bias_block_offset;
size_t opt_size;
SplineT spline;
Vector3 g;
Eigen::vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
int64_t dt_ns;
}; // namespace basalt
} // namespace basalt

254
include/basalt/utils/common_types.h Normal file
View File

@ -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 <bitset>
#include <cstdint>
#include <map>
#include <unordered_map>
#include <vector>
#include <tbb/concurrent_unordered_map.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <sophus/se3.hpp>
#include <basalt/utils/sophus_utils.hpp>
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<FrameId, CamId> 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<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>
corners;
/// collection of feature orientation (in radian) with same index as `corners`
/// (indexed by FeatureId)
std::vector<double> corner_angles;
/// collection of feature descriptors with same index as `corners` (indexed by
/// FeatureId)
std::vector<std::bitset<256>> corner_descriptors;
Eigen::vector<Eigen::Vector4d> corners_3d;
};
/// feature corners is a collection of { imageId => KeypointsData }
using Corners = tbb::concurrent_unordered_map<TimeCamId, KeypointsData>;
/// 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<std::pair<FeatureId, FeatureId>> matches;
/// collection of {featureId_i, featureId_j} pairs of inlier matches
std::vector<std::pair<FeatureId, FeatureId>> inliers;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// feature matches is a collection of { (imageId, imageId) => MatchData }
using Matches = tbb::concurrent_unordered_map<
std::pair<TimeCamId, TimeCamId>, MatchData,
tbb::tbb_hash<std::pair<TimeCamId, TimeCamId>>,
std::equal_to<std::pair<TimeCamId, TimeCamId>>,
Eigen::aligned_allocator<
std::pair<const std::pair<TimeCamId, TimeCamId>, MatchData>>>;
/// pair of image and feature indices
using ImageFeaturePair = std::pair<TimeCamId, FeatureId>;
/// 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<TimeCamId, FeatureId>;
/// 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<TrackId, FeatureTrack>;
/// 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<TimeCamId, Camera, std::less<TimeCamId>,
Eigen::aligned_allocator<std::pair<const TimeCamId, Camera>>>;
/// collection {trackId => Landmark} for all landmarks in the map.
/// trackIds correspond to feature_tracks
using Landmarks = std::unordered_map<TrackId, Landmark>;
/// camera candidate to be added to map
struct CameraCandidate {
TimeCamId tcid;
std::vector<TrackId> 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<CameraCandidate> 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<ProjectedLandmark>;
using ProjectedLandmarkConstPtr = std::shared_ptr<const ProjectedLandmark>;
/// all landmark projections for inlier and outlier observations for a single
/// image
struct ImageProjection {
std::vector<ProjectedLandmarkConstPtr> obs;
std::vector<ProjectedLandmarkConstPtr> outlier_obs;
};
/// projections for all images
using ImageProjections = std::map<TimeCamId, ImageProjection>;
/// inlier projections indexed per track
using TrackProjections =
std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>;
}
namespace cereal {
template <class Archive>
void serialize(Archive& ar, basalt::KeypointsData& c) {
ar(c.corners, c.corner_angles, c.corner_descriptors);
}
template <class Archive>
void serialize(Archive& ar, basalt::MatchData& c) {
ar(c.T_i_j, c.matches, c.inliers);
}
}

711
include/basalt/utils/image.h Normal file
View File

@ -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 <memory>
#include <pangolin/image/image.h>
#include <pangolin/image/managed_image.h>
#include <pangolin/image/typed_image.h>
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 <typename T>
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<T> toPangoImage() {
pangolin::Image<T> 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 <typename UnaryOperation>
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<T>& 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 <typename BinaryOperation>
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<T, T> MinMax() const {
PANGO_ASSERT(IsValid());
std::pair<T, T> minmax(std::numeric_limits<T>::max(),
std::numeric_limits<T>::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 <typename Tout = T>
Tout Sum() const {
return Accumulate((T)0,
[](const T& lhs, const T& rhs) { return lhs + rhs; });
}
template <typename Tout = T>
Tout Mean() const {
return Sum<Tout>() / 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 <typename TVec>
PANGO_HOST_DEVICE inline T& operator()(const TVec& p) {
PANGO_BOUNDS_ASSERT(InBounds(p[0], p[1]));
return RowPtr(p[1])[p[0]];
}
template <typename TVec>
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 <typename S>
inline S interp(const Eigen::Matrix<S, 2, 1>& p) const {
return interp<S>(p[0], p[1]);
}
template <typename S>
inline Eigen::Matrix<S, 3, 1> interpGrad(
const Eigen::Matrix<S, 2, 1>& p) const {
return interpGrad<S>(p[0], p[1]);
}
template <typename S>
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 <typename S>
inline Eigen::Matrix<S, 3, 1> 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<S, 3, 1> 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 <typename TVec, typename TBorder>
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<const T> SubImage(size_t x, size_t y,
size_t width,
size_t height) const {
PANGO_ASSERT((x + width) <= w && (y + height) <= h);
return Image<const T>(RowPtr(y) + x, width, height, pitch);
}
PANGO_HOST_DEVICE inline Image<T> SubImage(size_t x, size_t y, size_t width,
size_t height) {
PANGO_ASSERT((x + width) <= w && (y + height) <= h);
return Image<T>(RowPtr(y) + x, width, height, pitch);
}
PANGO_HOST_DEVICE inline Image<T> Row(int y) const {
return SubImage(0, y, w, 1);
}
PANGO_HOST_DEVICE inline Image<T> Col(int x) const {
return SubImage(x, 0, 1, h);
}
//////////////////////////////////////////////////////
// Data mangling
//////////////////////////////////////////////////////
template <typename TRecast>
PANGO_HOST_DEVICE inline Image<TRecast> Reinterpret() const {
PANGO_ASSERT(sizeof(TRecast) == sizeof(T),
"sizeof(TRecast) must match sizeof(T): % != %",
sizeof(TRecast), sizeof(T));
return UnsafeReinterpret<TRecast>();
}
template <typename TRecast>
PANGO_HOST_DEVICE inline Image<TRecast> UnsafeReinterpret() const {
return Image<TRecast>((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 <class T>
using DefaultImageAllocator = std::allocator<T>;
// Image that manages it's own memory, storing a strong pointer to it's memory
template <typename T, class Allocator = DefaultImageAllocator<T>>
class ManagedImage : public Image<T> {
public:
typedef std::shared_ptr<ManagedImage<T>> Ptr;
// Destructor
inline ~ManagedImage() { Deallocate(); }
// Null image
inline ManagedImage() {}
// Row image
inline ManagedImage(size_t w)
: Image<T>(Allocator().allocate(w), w, 1, w * sizeof(T)) {}
inline ManagedImage(size_t w, size_t h)
: Image<T>(Allocator().allocate(w * h), w, h, w * sizeof(T)) {}
inline ManagedImage(size_t w, size_t h, size_t pitch_bytes)
: Image<T>(Allocator().allocate((h * pitch_bytes) / sizeof(T) + 1), w, h,
pitch_bytes) {}
// Not copy constructable
inline ManagedImage(const ManagedImage<T>& other) = delete;
// Move constructor
inline ManagedImage(ManagedImage<T, Allocator>&& img) {
*this = std::move(img);
}
// Move asignment
inline void operator=(ManagedImage<T, Allocator>&& img) {
Deallocate();
Image<T>::pitch = img.pitch;
Image<T>::ptr = img.ptr;
Image<T>::w = img.w;
Image<T>::h = img.h;
img.ptr = nullptr;
}
// Move constructor
inline ManagedImage(pangolin::ManagedImage<T, Allocator>&& img) {
*this = std::move(img);
}
// Move asignment
inline void operator=(pangolin::ManagedImage<T, Allocator>&& img) {
Deallocate();
Image<T>::pitch = img.pitch;
Image<T>::ptr = img.ptr;
Image<T>::w = img.w;
Image<T>::h = img.h;
img.ptr = nullptr;
}
// Explicit copy constructor
template <typename TOther>
ManagedImage(const pangolin::CopyObject<TOther>& other) {
CopyFrom(other.obj);
}
// Explicit copy assignment
template <typename TOther>
void operator=(const pangolin::CopyObject<TOther>& other) {
CopyFrom(other.obj);
}
inline void Swap(ManagedImage<T>& img) {
std::swap(img.pitch, Image<T>::pitch);
std::swap(img.ptr, Image<T>::ptr);
std::swap(img.w, Image<T>::w);
std::swap(img.h, Image<T>::h);
}
inline void CopyFrom(const Image<T>& img) {
if (!Image<T>::IsValid() || Image<T>::w != img.w || Image<T>::h != img.h) {
Reinitialise(img.w, img.h);
}
Image<T>::CopyFrom(img);
}
inline void Reinitialise(size_t w, size_t h) {
if (!Image<T>::ptr || Image<T>::w != w || Image<T>::h != h) {
*this = ManagedImage<T, Allocator>(w, h);
}
}
inline void Reinitialise(size_t w, size_t h, size_t pitch) {
if (!Image<T>::ptr || Image<T>::w != w || Image<T>::h != h ||
Image<T>::pitch != pitch) {
*this = ManagedImage<T, Allocator>(w, h, pitch);
}
}
inline void Deallocate() {
if (Image<T>::ptr) {
Allocator().deallocate(Image<T>::ptr,
(Image<T>::h * Image<T>::pitch) / sizeof(T));
Image<T>::ptr = nullptr;
}
}
// Move asignment
template <typename TOther, typename AllocOther>
inline void OwnAndReinterpret(ManagedImage<TOther, AllocOther>&& img) {
Deallocate();
Image<T>::pitch = img.pitch;
Image<T>::ptr = (T*)img.ptr;
Image<T>::w = img.w;
Image<T>::h = img.h;
img.ptr = nullptr;
}
template <typename T1>
inline void ConvertFrom(const ManagedImage<T1>& 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<T>& 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 <typename T, class Allocator = DefaultImageAllocator<T>>
class ManagedImagePyr {
public:
inline ManagedImagePyr() {}
inline ManagedImagePyr(ManagedImage<T>& other, size_t num_levels) {
setFromImage(other, num_levels);
}
inline void setFromImage(const ManagedImage<T>& 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<const T> l = lvl(i);
Image<T> 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<const T>& img, Image<T>& img_sub) {
static_assert(std::is_same<T, uint16_t>::value ||
std::is_same<T, uint8_t>::value);
constexpr int kernel[5] = {1, 4, 6, 4, 1};
// accumulator
ManagedImage<int> 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<const T> 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 <typename S>
inline Eigen::Matrix<S, 2, 1> 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<S, 2, 1>(x, y);
}
inline pangolin::Image<T> toPangoImage() { return image.toPangoImage(); }
private:
inline Image<T> 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<T> image;
};
inline void rgb_to_gray(const pangolin::TypedImage& rgb,
basalt::ManagedImage<uint8_t>& 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

270
include/basalt/utils/imu_types.h Normal file
View File

@ -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 <atomic>
#include <map>
#include <memory>
#include <set>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <basalt/imu/imu_types.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/utils/sophus_utils.hpp>
#include <cereal/cereal.hpp>
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 <class Archive>
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 <class Archive>
void serialize(Archive& ar) {
ar(T_w_i);
ar(T_w_i_current);
ar(delta);
ar(linearized);
ar(t_ns);
}
};
struct AbsOrderMap {
std::map<int64_t, std::pair<int, int>> 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<MargData> Ptr;
AbsOrderMap aom;
Eigen::MatrixXd abs_H;
Eigen::VectorXd abs_b;
Eigen::map<int64_t, PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, PoseStateWithLin> frame_poses;
std::set<int64_t> kfs_all;
std::set<int64_t> kfs_to_marg;
std::vector<OpticalFlowResult::Ptr> 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 <class Archive>
void serialize(Archive& ar, basalt::AbsOrderMap& a) {
ar(a.total_size);
ar(a.items);
ar(a.abs_order_map);
}
template <class Archive>
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

111
include/basalt/utils/keypoints.h Normal file
View File

@ -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 <bitset>
#include <set>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <pangolin/image/managed_image.h>
#include <basalt/utils/image.h>
#include <basalt/utils/sophus_utils.hpp>
#include <basalt/utils/common_types.h>
#include <basalt/camera/generic_camera.hpp>
namespace basalt {
typedef std::bitset<256> Descriptor;
void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, int num_features);
void detectKeypoints(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, int PATCH_SIZE = 32,
int num_points_cell = 1,
const Eigen::vector<Eigen::Vector2d>& current_points =
Eigen::vector<Eigen::Vector2d>());
void computeAngles(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, bool rotate_features);
void computeDescriptors(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd);
void matchFastHelper(const std::vector<std::bitset<256>>& corner_descriptors_1,
const std::vector<std::bitset<256>>& corner_descriptors_2,
std::map<int, int>& matches, int threshold,
double test_dist);
void matchDescriptors(const std::vector<std::bitset<256>>& corner_descriptors_1,
const std::vector<std::bitset<256>>& corner_descriptors_2,
std::vector<std::pair<int, int>>& 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

120
include/basalt/utils/nfr.h Normal file
View File

@ -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 <sophus/se3.hpp>
#include <basalt/utils/imu_types.h>
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<double, 3, 6>* 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<double, 1, 6>* 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<double, 2, 6>* 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>();
}
}

55
include/basalt/utils/sim_utils.h Normal file
View File

@ -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 <basalt/utils/sophus_utils.hpp>
namespace basalt {
struct SimObservations {
Eigen::vector<Eigen::Vector2d> pos;
std::vector<int> id;
};
}
namespace cereal {
template <class Archive>
void serialize(Archive& ar, basalt::SimObservations& c) {
ar(c.pos, c.id);
}
}

52
include/basalt/utils/system_utils.h Normal file
View File

@ -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 <string>
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

77
include/basalt/utils/test_utils.h Normal file
View File

@ -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 <Eigen/Dense>
template <typename Derived1, typename Derived2, typename F>
void test_jacobian_code(const std::string& name,
const Eigen::MatrixBase<Derived1>& Ja, F func,
const Eigen::MatrixBase<Derived2>& x0,
double eps = 1e-8, double max_norm = 1e-4) {
typedef typename Derived1::Scalar Scalar;
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Jn = Ja;
Jn.setZero();
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inc = x0;
for (int i = 0; i < Jn.cols(); i++) {
inc.setZero();
inc[i] += eps;
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fpe = func(x0 + inc);
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> 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

225
include/basalt/utils/tracks.h Normal file
View File

@ -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 <cassert>
#include <cstdint>
#include <functional>
#include <iterator>
#include <map>
#include <memory>
#include <set>
#include <unordered_map>
#include <utility>
#include <vector>
#include <basalt/utils/common_types.h>
#include <basalt/utils/union_find.h>
namespace basalt {
/// TrackBuild class creates feature tracks from matches
struct TrackBuilder {
std::map<ImageFeaturePair, TrackId> 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<ImageFeaturePair> 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<TrackId, std::set<TimeCamId>> tracks;
std::set<TrackId> 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<TrackId> 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<TimeCamId>& image_ids,
const FeatureTracks& all_tracks,
std::vector<TrackId>& 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<TrackId>& track_ids) {
std::set<TimeCamId> 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<TrackId>& 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();
}
}

94
include/basalt/utils/union_find.h Normal file
View File

@ -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 <cstdint>
#include <limits>
#include <numeric>
#include <vector>
// 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<ValueType>::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<ValueType> m_cc_parent;
// A rank array used for union by rank
std::vector<ValueType> m_cc_rank;
// A 'size array' to know the size of each connected component
std::vector<ValueType> 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];
}
}
};

75
include/basalt/utils/vio_config.h Normal file
View File

@ -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 <string>
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

106
include/basalt/utils/vis_utils.h Normal file
View File

@ -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 <Eigen/Dense>
#include <pangolin/gl/gldraw.h>
#include <basalt/utils/sophus_utils.hpp>
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<Eigen::Vector3f> 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);
}

372
include/basalt/vi_estimator/ba_base.h Normal file
View File

@ -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 <basalt/utils/imu_types.h>
#include <tbb/blocked_range.h>
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<std::pair<TimeCamId, TimeCamId>> order;
Eigen::vector<Sophus::Matrix6d> d_rel_d_h;
Eigen::vector<Sophus::Matrix6d> d_rel_d_t;
};
struct FrameRelLinData {
Sophus::Matrix6d Hpp;
Sophus::Vector6d bp;
std::vector<int> lm_id;
Eigen::vector<Eigen::Matrix<double, 6, 3>> 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<int, Eigen::Matrix3d> Hll;
Eigen::unordered_map<int, Eigen::Vector3d> bl;
Eigen::unordered_map<int, std::vector<std::pair<size_t, size_t>>> lm_to_obs;
Eigen::vector<FrameRelLinData> Hpppl;
double error;
};
void computeError(double& error) const;
void linearizeHelper(
Eigen::vector<RelLinData>& rld_vec,
const Eigen::map<
TimeCamId, Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>&
obs_to_lin,
double& error) const;
static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H,
Eigen::VectorXd& b);
template <class CamT>
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<double, 2, POSE_SIZE>* d_res_d_xi = nullptr,
Eigen::Matrix<double, 2, 3>* d_res_d_p = nullptr,
Eigen::Vector4d* proj = nullptr) {
// Todo implement without jacobians
Eigen::Matrix<double, 4, 2> Jup;
Eigen::Vector4d p_h_3d;
p_h_3d = StereographicParam<double>::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<double, 4, POSE_SIZE> 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<double, 2, 4> 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<double, 4, 3> 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 <class CamT>
inline static bool linearizePoint(
const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
const CamT& cam, Eigen::Vector2d& res,
Eigen::Matrix<double, 2, 3>* d_res_d_p = nullptr,
Eigen::Vector4d* proj = nullptr) {
// Todo implement without jacobians
Eigen::Matrix<double, 4, 2> Jup;
Eigen::Vector4d p_h_3d;
p_h_3d = StereographicParam<double>::unproject(kpt_pos.dir, &Jup);
Eigen::Matrix<double, 2, 4> 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<double, 4, 3> 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<Eigen::Vector3d>& points,
std::vector<int>& 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<int>& idx_to_keep,
const std::set<int>& 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 <class AccumT>
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<POSE_SIZE>(
abs_h_idx, rld.d_rel_d_h[i].transpose() *
rel_b.segment<POSE_SIZE>(i * POSE_SIZE));
accum.template addB<POSE_SIZE>(
abs_ti_idx, rld.d_rel_d_t[i].transpose() *
rel_b.segment<POSE_SIZE>(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<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_h_idx, rld.d_rel_d_h[i].transpose() *
rel_H.block<POSE_SIZE, POSE_SIZE>(
POSE_SIZE * i, POSE_SIZE * j) *
rld.d_rel_d_h[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_h_idx, rld.d_rel_d_t[i].transpose() *
rel_H.block<POSE_SIZE, POSE_SIZE>(
POSE_SIZE * i, POSE_SIZE * j) *
rld.d_rel_d_h[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_tj_idx, rld.d_rel_d_h[i].transpose() *
rel_H.block<POSE_SIZE, POSE_SIZE>(
POSE_SIZE * i, POSE_SIZE * j) *
rld.d_rel_d_t[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_tj_idx, rld.d_rel_d_t[i].transpose() *
rel_H.block<POSE_SIZE, POSE_SIZE>(
POSE_SIZE * i, POSE_SIZE * j) *
rld.d_rel_d_t[j]);
}
}
}
template <class AccumT>
struct LinearizeAbsReduce {
using RelLinDataIter = Eigen::vector<RelLinData>::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<RelLinDataIter>& 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<int64_t, PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, PoseStateWithLin> frame_poses;
// Point management
Eigen::unordered_map<int, KeypointPosition> kpts;
Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
obs;
double obs_std_dev;
double huber_thresh;
basalt::Calibration<double> calib;
};
}

229
include/basalt/vi_estimator/keypoint_vio.h Normal file
View File

@ -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 <memory>
#include <thread>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <basalt/imu/preintegration.h>
#include <basalt/io/dataset_io.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/test_utils.h>
#include <basalt/camera/generic_camera.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <basalt/utils/sophus_utils.hpp>
#include <basalt/vi_estimator/ba_base.h>
#include <basalt/vi_estimator/vio_estimator.h>
namespace basalt {
class KeypointVioEstimator : public VioEstimatorBase,
public BundleAdjustmentBase {
public:
typedef std::shared_ptr<KeypointVioEstimator> Ptr;
static const int N = 9;
typedef Eigen::Matrix<double, N, 1> VecN;
typedef Eigen::Matrix<double, N, N> MatNN;
typedef Eigen::Matrix<double, N, 3> 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<double>& 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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& 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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& 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<int64_t, int>& 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<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& 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<N, N>(); }
void computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& res) const;
inline void setMaxStates(size_t val) { max_states = val; }
inline void setMaxKfs(size_t val) { max_kfs = val; }
Eigen::vector<Sophus::SE3d> getFrameStates() const {
Eigen::vector<Sophus::SE3d> res;
for (const auto& kv : frame_states) {
res.push_back(kv.second.getState().T_w_i);
}
return res;
}
Eigen::vector<Sophus::SE3d> getFramePoses() const {
Eigen::vector<Sophus::SE3d> res;
for (const auto& kv : frame_poses) {
res.push_back(kv.second.getPose());
}
return res;
}
Eigen::map<int64_t, Sophus::SE3d> getAllPosesMap() const {
Eigen::map<int64_t, Sophus::SE3d> 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<int64_t> kf_ids;
int64_t last_state_t_ns;
Eigen::map<int64_t, IntegratedImuMeasurement> imu_meas;
const Eigen::Vector3d g;
// Input
Eigen::map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
std::map<int64_t, int> 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<std::thread> processing_thread;
};
} // namespace basalt

218
include/basalt/vi_estimator/nfr_mapper.h Normal file
View File

@ -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 <memory>
#include <thread>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <basalt/utils/common_types.h>
#include <basalt/vi_estimator/ba_base.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/concurrent_vector.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
#include <mutex>
#include <DBoW3.h>
namespace basalt {
class NfrMapper : public BundleAdjustmentBase {
public:
using Ptr = std::shared_ptr<NfrMapper>;
using TimeCamId = std::pair<int64_t, std::size_t>;
using Matches = tbb::concurrent_unordered_map<
std::pair<TimeCamId, TimeCamId>, MatchData,
tbb::tbb_hash<std::pair<TimeCamId, TimeCamId>>,
std::equal_to<std::pair<TimeCamId, TimeCamId>>,
Eigen::aligned_allocator<
std::pair<const std::pair<TimeCamId, TimeCamId>, MatchData>>>;
template <class AccumT>
struct MapperLinearizeAbsReduce
: public BundleAdjustmentBase::LinearizeAbsReduce<AccumT> {
using RollPitchFactorConstIter =
Eigen::vector<RollPitchFactor>::const_iterator;
using RelPoseFactorConstIter = Eigen::vector<RelPoseFactor>::const_iterator;
using RelLinDataIter = Eigen::vector<RelLinData>::iterator;
MapperLinearizeAbsReduce(
AbsOrderMap& aom,
const Eigen::map<int64_t, PoseStateWithLin>* frame_poses)
: BundleAdjustmentBase::LinearizeAbsReduce<AccumT>(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<AccumT>(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<RelLinDataIter>& 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<RollPitchFactorConstIter>& 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<double, 2, POSE_SIZE> J;
Sophus::Vector2d res = basalt::rollPitchError(pose, rpf.R_w_i_meas, &J);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx, idx, J.transpose() * rpf.cov_inv * J);
this->accum.template addB<POSE_SIZE>(idx,
J.transpose() * rpf.cov_inv * res);
roll_pitch_error += res.transpose() * rpf.cov_inv * res;
}
}
void operator()(const tbb::blocked_range<RelPoseFactorConstIter>& 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<POSE_SIZE, POSE_SIZE>(
idx_i, idx_i, Ji.transpose() * rpf.cov_inv * Ji);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_i, idx_j, Ji.transpose() * rpf.cov_inv * Jj);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_j, idx_i, Jj.transpose() * rpf.cov_inv * Ji);
this->accum.template addH<POSE_SIZE, POSE_SIZE>(
idx_j, idx_j, Jj.transpose() * rpf.cov_inv * Jj);
this->accum.template addB<POSE_SIZE>(
idx_i, Ji.transpose() * rpf.cov_inv * res);
this->accum.template addB<POSE_SIZE>(
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<int64_t, PoseStateWithLin>* frame_poses;
};
NfrMapper(const basalt::Calibration<double>& 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<int64_t, PoseStateWithLin>& 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<RollPitchFactor> roll_pitch_factors;
Eigen::vector<RelPoseFactor> rel_pose_factors;
std::unordered_map<int64_t, OpticalFlowInput::Ptr> img_data;
tbb::concurrent_unordered_map<
TimeCamId, std::pair<DBoW3::BowVector, DBoW3::FeatureVector>>
bow_data;
Corners feature_corners;
Matches feature_matches;
FeatureTracks feature_tracks;
DBoW3::Database bow_database;
std::unordered_map<int, TimeCamId> bow_id_to_tcid;
VioConfig config;
};
} // namespace basalt

102
include/basalt/vi_estimator/vio_estimator.h Normal file
View File

@ -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 <atomic>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/utils/imu_types.h>
namespace basalt {
struct VioVisualizationData {
typedef std::shared_ptr<VioVisualizationData> Ptr;
int64_t t_ns;
Eigen::vector<Sophus::SE3d> states;
Eigen::vector<Sophus::SE3d> frames;
Eigen::vector<Eigen::Vector3d> points;
std::vector<int> point_ids;
OpticalFlowResult::Ptr opt_flow_res;
std::vector<Eigen::vector<Eigen::Vector4d>> projections;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class VioEstimatorBase {
public:
typedef std::shared_ptr<VioEstimatorBase> 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<int64_t> last_processed_t_ns;
std::atomic<bool> finished;
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr> vision_data_queue;
tbb::concurrent_bounded_queue<ImuData::Ptr> imu_data_queue;
tbb::concurrent_bounded_queue<PoseVelBiasState::Ptr>* out_state_queue =
nullptr;
tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue = nullptr;
tbb::concurrent_bounded_queue<VioVisualizationData::Ptr>* out_vis_queue =
nullptr;
};
class VioEstimatorFactory {
public:
static VioEstimatorBase::Ptr getVioEstimator(
const VioConfig& config, const Calibration<double>& 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<int64_t>& filter_t_ns,
const Eigen::vector<Eigen::Vector3d>& filter_t_w_i,
const std::vector<int64_t>& gt_t_ns,
Eigen::vector<Eigen::Vector3d>& gt_t_w_i);
} // namespace basalt

39
scripts/eval_full/gen_results.py Executable file
View File

@ -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)

26
scripts/eval_full/run_evaluations.sh Executable file
View File

@ -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

9
scripts/install_deps.sh Executable file
View File

@ -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

4
scripts/install_mac_os_deps.sh Executable file
View File

@ -0,0 +1,4 @@
#!/bin/sh
brew install clang-format tbb glew eigen ccache
brew install --with-toolchain llvm

4
scripts/install_ubuntu_deps.sh Executable file
View File

@ -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

7
scripts/update_submodules.sh Executable file
View File

@ -0,0 +1,7 @@
#!/usr/bin/env bash
set -x
git submodule sync --recursive
git submodule update --init --recursive

82
src/calibrate.cpp Normal file
View File

@ -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 <basalt/calibration/cam_calib.h>
#include <tbb/tbb.h>
#include <CLI/CLI.hpp>
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<std::string> 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;
}

94
src/calibrate_imu.cpp Normal file
View File

@ -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 <basalt/optimization/spline_optimize.h>
#include <basalt/calibration/cam_imu_calib.h>
#include <tbb/tbb.h>
#include <CLI/CLI.hpp>
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;
}

361
src/calibration/calibraiton_helper.cpp Normal file
View File

@ -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 <basalt/calibration/calibration_helper.h>
#include <basalt/utils/apriltag.h>
#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
namespace basalt {
template <class CamT>
bool estimateTransformation(
const CamT &cam_calib, const Eigen::vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &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<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem>
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<TimeCamId, CalibCornerData> &calib_corners,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>
&calib_corners_rejected) {
calib_corners.clear();
calib_corners_rejected.clear();
tbb::parallel_for(
tbb::blocked_range<size_t>(0, vio_data->get_image_timestamps().size()),
[&](const tbb::blocked_range<size_t> &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<ImageData> &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<double>::Ptr &calib, const VioDatasetPtr &vio_data,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData> &calib_corners,
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData>
&calib_init_poses) {
calib_init_poses.clear();
std::vector<TimeCamId> corners;
corners.reserve(calib_corners.size());
for (const auto &kv : calib_corners) {
corners.emplace_back(kv.first);
}
tbb::parallel_for(tbb::blocked_range<size_t>(0, corners.size()),
[&](const tbb::blocked_range<size_t> &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<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &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<int, Eigen::Vector2d> 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<double>::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<Eigen::Vector4d> 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<Eigen::Matrix4Xd> 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<Eigen::MatrixXd> 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<double, 5, 1> calib;
calib << 0.5 * gamma, 0.5 * gamma, _cu, _cv, 0.5 * _xi;
// std::cerr << "gamma " << gamma << std::endl;
UnifiedCamera<double> 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<double>::Ptr &calib, size_t cam_id,
const Eigen::vector<Eigen::Vector4d> &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<bool> 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<double> &cam_calib,
const Eigen::vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &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

798
src/calibration/cam_calib.cpp Normal file
View File

@ -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 <basalt/calibration/cam_calib.h>
#include <basalt/utils/system_utils.h>
#include <basalt/calibration/vignette.h>
#include <basalt/optimization/poses_optimize.h>
#include <basalt/serialization/headers_serialization.h>
#include <experimental/filesystem>
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<std::string> &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<std::function<void(void)>> load_dataset(
"ui.load_dataset", std::bind(&CamCalib::loadDataset, this));
pangolin::Var<std::function<void(void)>> detect_corners(
"ui.detect_corners", std::bind(&CamCalib::detectCorners, this));
pangolin::Var<std::function<void(void)>> init_cam_intrinsics(
"ui.init_cam_intr", std::bind(&CamCalib::initCamIntrinsics, this));
pangolin::Var<std::function<void(void)>> init_cam_poses(
"ui.init_cam_poses", std::bind(&CamCalib::initCamPoses, this));
pangolin::Var<std::function<void(void)>> init_cam_extrinsics(
"ui.init_cam_extr", std::bind(&CamCalib::initCamExtrinsics, this));
pangolin::Var<std::function<void(void)>> init_opt(
"ui.init_opt", std::bind(&CamCalib::initOptimization, this));
pangolin::Var<std::function<void(void)>> optimize(
"ui.optimize", std::bind(&CamCalib::optimize, this));
pangolin::Var<std::function<void(void)>> save_calib(
"ui.save_calib", std::bind(&CamCalib::saveCalib, this));
pangolin::Var<std::function<void(void)>> compute_vign(
"ui.compute_vign", std::bind(&CamCalib::computeVign, this));
setNumCameras(1);
}
void CamCalib::computeVign() {
Eigen::vector<Eigen::Vector2d> 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<TimeCamId, Eigen::vector<Eigen::Vector3d>> 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<ImageData> 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<Eigen::Vector3d> 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<uint16_t>::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<pangolin::ImageView> 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<size_t>(show_frame);
int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id];
const std::vector<ImageData> &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<Eigen::Vector2d> 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<bool> 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<basalt::ImageData> &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<Eigen::Vector2i> 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<uint64_t> 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<int64_t> 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<std::string, double> *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<std::chrono::milliseconds>(
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<float>(cr.radii[i]);
const Eigen::Vector2f c = cr.corners[i].cast<float>();
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<float>(cr_rej.radii[i]);
const Eigen::Vector2f c = cr_rej.corners[i].cast<float>();
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

1054
src/calibration/cam_imu_calib.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

306
src/calibration/vignette.cpp Normal file
View File

@ -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 <basalt/calibration/vignette.h>
namespace basalt {
VignetteEstimator::VignetteEstimator(
const VioDatasetPtr &vio_dataset,
const Eigen::vector<Eigen::Vector2d> &optical_centers,
const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>>
&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<double, 1, 1>(1));
}
while (vign_param[i].maxTimeNs() < int64_t(vign_size) * int64_t(1e9)) {
vign_param[i].knots_push_back(Eigen::Matrix<double, 1, 1>(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<TimeCamId, std::vector<double>> *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<double> 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<double> new_irradiance(irradiance.size(), 0);
std::vector<int> 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<std::vector<double>> new_vign_param(
vio_dataset->get_num_cams(), std::vector<double>(num_knots, 0));
std::vector<std::vector<double>> new_vign_param_count(
vio_dataset->get_num_cams(), std::vector<double>(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<std::vector<double>> num_proj_points(
2, std::vector<double>(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<float> 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<uint16_t> 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<uint16_t>::max()
: uint16_t(val * std::numeric_limits<uint16_t>::max());
vign_img(x, y) = val_int;
}
}
pangolin::SaveImage(vign_img.UnsafeReinterpret<uint8_t>(),
pangolin::PixelFormatFromString("GRAY16LE"),
path + "/vingette_" + std::to_string(k) + ".png");
}
}
} // namespace basalt

57
src/io/dataset_io.cpp Normal file
View File

@ -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 <basalt/io/dataset_io.h>
#include <basalt/io/dataset_io_euroc.h>
#include <basalt/io/dataset_io_rosbag.h>
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

228
src/io/marg_data_io.cpp Normal file
View File

@ -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 <basalt/io/marg_data_io.h>
#include <pangolin/image/image_io.h>
#include <basalt/serialization/headers_serialization.h>
#include <experimental/filesystem>
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<int64_t> 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<uint64_t> saved_images;
std::map<int64_t, OpticalFlowResult::Ptr> 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<int64_t, std::string> 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 <class Archive, class T>
void save(Archive& ar, const basalt::ManagedImage<T>& m) {
ar(m.w);
ar(m.h);
ar(cereal::binary_data(m.ptr, sizeof(T) * m.w * m.h));
}
template <class Archive, class T>
void load(Archive& ar, basalt::ManagedImage<T>& 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 <class Archive>
void serialize(Archive& ar, basalt::OpticalFlowResult& m) {
ar(m.t_ns);
ar(m.observations);
ar(m.input_images);
}
template <class Archive>
void serialize(Archive& ar, basalt::OpticalFlowInput& m) {
ar(m.t_ns);
ar(m.img_data);
}
template <class Archive>
void serialize(Archive& ar, basalt::ImageData& m) {
ar(m.exposure);
ar(m.img);
}
template <class Archive>
void serialize(Archive& ar, Eigen::AffineCompact2f& m) {
ar(m.matrix());
}
} // namespace cereal

647
src/mapper.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <iostream>
#include <thread>
#include <sophus/se3.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <CLI/CLI.hpp>
#include <basalt/io/dataset_io.h>
#include <basalt/io/marg_data_io.h>
#include <basalt/optimization/accumulator.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/imu_types.h>
#include <basalt/utils/nfr.h>
#include <basalt/utils/vis_utils.h>
#include <basalt/vi_estimator/nfr_mapper.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <basalt/calibration/calibration.hpp>
#include <experimental/filesystem>
#include <basalt/serialization/headers_serialization.h>
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<Eigen::Vector2i> image_resolutions = {{752, 480},
{752, 480}};
basalt::VioConfig vio_config;
basalt::NfrMapper::Ptr nrf_mapper;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i;
std::vector<int64_t> gt_frame_t_ns, image_t_ns;
Eigen::vector<Eigen::Vector3d> mapper_points;
std::vector<int> mapper_point_ids;
std::map<int64_t, basalt::MargData::Ptr> marg_data;
Eigen::vector<Eigen::Vector3d> edges_vis;
Eigen::vector<Eigen::Vector3d> roll_pitch_vis;
Eigen::vector<Eigen::Vector3d> 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<double> calib;
pangolin::Var<int> show_frame1("ui.show_frame1", 0, 0, 1);
pangolin::Var<int> show_cam1("ui.show_cam1", 0, 0, 0);
pangolin::Var<int> show_frame2("ui.show_frame2", 0, 0, 1);
pangolin::Var<int> show_cam2("ui.show_cam2", 0, 0, 0);
pangolin::Var<bool> lock_frames("ui.lock_frames", true, false, true);
pangolin::Var<bool> show_detected("ui.show_detected", true, false, true);
pangolin::Var<bool> show_matches("ui.show_matches", true, false, true);
pangolin::Var<bool> show_inliers("ui.show_inliers", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
pangolin::Var<int> num_opt_iter("ui.num_opt_iter", 10, 0, 20);
pangolin::Var<bool> show_gt("ui.show_gt", true, false, true);
pangolin::Var<bool> show_edges("ui.show_edges", true, false, true);
pangolin::Var<bool> show_points("ui.show_points", true, false, true);
using Button = pangolin::Var<std::function<void(void)>>;
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<std::shared_ptr<pangolin::ImageView>> img_view;
while (img_view.size() < calib.intrinsics.size()) {
std::shared_ptr<pangolin::ImageView> 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<size_t>(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<basalt::ImageData>& 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<size_t>(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<basalt::ImageData>& 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<basalt::MargData::Ptr> 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<Eigen::Vector3d> filter_t_w_i;
std::vector<int64_t> 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();
}

595
src/mapper_sim.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <iostream>
#include <thread>
#include <sophus/se3.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <CLI/CLI.hpp>
#include <basalt/io/dataset_io.h>
#include <basalt/io/marg_data_io.h>
#include <basalt/optimization/accumulator.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/imu_types.h>
#include <basalt/utils/nfr.h>
#include <basalt/utils/sim_utils.h>
#include <basalt/utils/vis_utils.h>
#include <basalt/vi_estimator/keypoint_vio.h>
#include <basalt/vi_estimator/nfr_mapper.h>
#include <basalt/calibration/calibration.hpp>
#include <experimental/filesystem>
#include <basalt/serialization/headers_serialization.h>
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<basalt::Se3Spline<5>> gt_spline;
Eigen::vector<Sophus::SE3d> gt_frame_T_w_i;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
std::vector<int64_t> gt_frame_t_ns;
Eigen::vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias,
noisy_accel, noisy_gyro, gt_vel;
std::vector<int64_t> gt_imu_t_ns;
Eigen::vector<Eigen::Vector3d> filter_points;
std::vector<int> filter_point_ids;
std::map<int64_t, basalt::MargData::Ptr> marg_data;
Eigen::vector<basalt::RollPitchFactor> roll_pitch_factors;
Eigen::vector<basalt::RelPoseFactor> rel_pose_factors;
Eigen::vector<Eigen::Vector3d> edges_vis;
Eigen::vector<Eigen::Vector3d> roll_pitch_vis;
Eigen::vector<Eigen::Vector3d> rel_edges_vis;
Eigen::vector<Eigen::Vector3d> mapper_points;
std::vector<int> mapper_point_ids;
basalt::NfrMapper::Ptr nrf_mapper;
std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
std::map<basalt::TimeCamId, basalt::SimObservations> 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<double> calib;
pangolin::Var<bool> show_edges("ui.show_edges", true, false, true);
pangolin::Var<bool> show_points("ui.show_points", true, false, true);
using Button = pangolin::Var<std::function<void(void)>>;
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<Sophus::SE3d> 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<basalt::MargData::Ptr> 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<int> idx_to_keep;
std::set<int> 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<double, 3, POSE_SIZE> d_pos_d_T_w_i;
Sophus::Matrix<double, 1, POSE_SIZE> d_yaw_d_T_w_i;
Sophus::Matrix<double, 2, POSE_SIZE> 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<POSE_SIZE, POSE_SIZE>(0, kf_start_idx) = d_res_d_T_w_i;
J.block<POSE_SIZE, POSE_SIZE>(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<Eigen::Vector3d> filter_t_w_i;
std::vector<int64_t> 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);
}

751
src/mapper_sim_naive.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <iostream>
#include <thread>
#include <sophus/se3.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <CLI/CLI.hpp>
#include <basalt/io/dataset_io.h>
#include <basalt/io/marg_data_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/sim_utils.h>
#include <basalt/vi_estimator/keypoint_vio.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/serialization/headers_serialization.h>
#include <basalt/utils/vis_utils.h>
// 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<Eigen::Vector3d> gt_points;
Eigen::vector<Sophus::SE3d> gt_frame_T_w_i;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
std::vector<int64_t> gt_frame_t_ns, kf_t_ns;
Eigen::vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias,
noisy_accel, noisy_gyro, gt_vel;
std::vector<int64_t> gt_imu_t_ns;
std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
std::map<basalt::TimeCamId, basalt::SimObservations> noisy_observations;
std::string marg_data_path;
// VIO vars
basalt::Calibration<double> calib;
basalt::KeypointVioEstimator::Ptr vio;
// Visualization vars
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
std::vector<pangolin::TypedImage> images;
// Pangolin vars
constexpr int UI_WIDTH = 200;
pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
pangolin::Plotter* plotter;
pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1000);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
pangolin::Var<bool> show_obs_noisy("ui.show_obs_noisy", true, false, true);
pangolin::Var<bool> show_obs_vio("ui.show_obs_vio", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
pangolin::Var<bool> show_accel("ui.show_accel", false, false, true);
pangolin::Var<bool> show_gyro("ui.show_gyro", false, false, true);
pangolin::Var<bool> show_gt_vel("ui.show_gt_vel", false, false, true);
pangolin::Var<bool> show_gt_pos("ui.show_gt_pos", true, false, true);
pangolin::Var<bool> show_gt_bg("ui.show_gt_bg", false, false, true);
pangolin::Var<bool> show_gt_ba("ui.show_gt_ba", false, false, true);
pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);
using Button = pangolin::Var<std::function<void(void)>>;
Button next_step_btn("ui.next_step", &next_step);
pangolin::Var<bool> 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<float>();
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<float> 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<std::shared_ptr<pangolin::ImageView>> img_view;
while (img_view.size() < calib.intrinsics.size()) {
std::shared_ptr<pangolin::ImageView> 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<float>();
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<float>();
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<Sophus::SE3d> 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<basalt::MargData::Ptr> marg_queue;
mdl.out_marg_queue = &marg_queue;
mdl.start(marg_data_path);
Eigen::map<int64_t, Sophus::SE3d> 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<Eigen::Vector3d> 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);
}

342
src/opt_flow.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <iostream>
#include <thread>
#include <sophus/se3.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <CLI/CLI.hpp>
#include <basalt/io/dataset_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/serialization/headers_serialization.h>
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<int> show_frame("ui.show_frame", 0, 0, 1500);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
using Button = pangolin::Var<std::function<void(void)>>;
Button next_step_btn("ui.next_step", &next_step);
pangolin::Var<bool> 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<int64_t, basalt::OpticalFlowResult::Ptr>
observations;
tbb::concurrent_bounded_queue<basalt::OpticalFlowResult::Ptr>
observations_queue;
basalt::Calibration<double> calib;
std::unordered_map<basalt::KeypointId, int> 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<std::shared_ptr<pangolin::ImageView>> img_view;
while (img_view.size() < calib.intrinsics.size()) {
std::shared_ptr<pangolin::ImageView> 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<size_t>(show_frame);
int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id];
const std::vector<basalt::ImageData>& 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<size_t>(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<basalt::KeypointId, Eigen::AffineCompact2f>& 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;
}
}

101
src/optical_flow/optical_flow.cpp Normal file
View File

@ -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 <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/frame_to_frame_optical_flow.h>
#include <basalt/optical_flow/patch_optical_flow.h>
namespace basalt {
OpticalFlowBase::Ptr OpticalFlowFactory::getOpticalFlow(
const VioConfig& config, const Calibration<double>& cam) {
OpticalFlowBase::Ptr res;
if (config.optical_flow_type == "patch") {
switch (config.optical_flow_pattern) {
case 24:
res.reset(new PatchOpticalFlow<float, Pattern24>(config, cam));
break;
case 52:
res.reset(new PatchOpticalFlow<float, Pattern52>(config, cam));
break;
case 51:
res.reset(new PatchOpticalFlow<float, Pattern51>(config, cam));
break;
case 50:
res.reset(new PatchOpticalFlow<float, Pattern50>(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<float, Pattern24>(config, cam));
break;
case 52:
res.reset(new FrameToFrameOpticalFlow<float, Pattern52>(config, cam));
break;
case 51:
res.reset(new FrameToFrameOpticalFlow<float, Pattern51>(config, cam));
break;
case 50:
res.reset(new FrameToFrameOpticalFlow<float, Pattern50>(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

420
src/utils/keypoints.cpp Normal file
View File

@ -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 <basalt/utils/keypoints.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
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<const uint16_t>& 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<cv::Point2f> 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<const uint16_t>& img_raw,
KeypointsData& kd, int PATCH_SIZE, int num_points_cell,
const Eigen::vector<Eigen::Vector2d>& 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<const uint16_t> 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<cv::KeyPoint> 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<const uint16_t>& 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<const uint16_t>& 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<int>();
Eigen::Vector2i vvb = (mat_rot * vb).array().round().cast<int>();
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<std::bitset<256>>& corner_descriptors_1,
const std::vector<std::bitset<256>>& corner_descriptors_2,
std::map<int, int>& 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<std::bitset<256>>& corner_descriptors_1,
const std::vector<std::bitset<256>>& corner_descriptors_2,
std::vector<std::pair<int, int>>& matches, int threshold,
double dist_2_best) {
matches.clear();
std::map<int, int> 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

130
src/utils/vio_config.cpp Normal file
View File

@ -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 <basalt/utils/vio_config.h>
#include <fstream>
#include <cereal/archives/json.hpp>
#include <cereal/cereal.hpp>
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 <class Archive>
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

525
src/vi_estimator/ba_base.cpp Normal file
View File

@ -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 <basalt/vi_estimator/ba_base.h>
#include <tbb/parallel_for.h>
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<POSE_SIZE>(i * POSE_SIZE) =
rld.d_rel_d_h[i] * inc.segment<POSE_SIZE>(abs_h_idx) +
rld.d_rel_d_t[i] * inc.segment<POSE_SIZE>(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<double, 3, POSE_SIZE> H_l_p_other =
frld_other.Hpl[other_obs[k].second].transpose();
H_l_p_x += H_l_p_other * rel_inc.segment<POSE_SIZE>(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<RelLinData>& rld_vec,
const Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>&
obs_to_lin,
double& error) const {
error = 0;
rld_vec.clear();
std::vector<TimeCamId> 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<size_t>(0, obs_tcid_vec.size()),
[&](const tbb::blocked_range<size_t>& 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<double, 2, POSE_SIZE> d_res_d_xi;
Eigen::Matrix<double, 2, 3> 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<double, 2, 3> 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, POSE_SIZE>(POSE_SIZE * i, POSE_SIZE * i) += frld.Hpp;
b.segment<POSE_SIZE>(POSE_SIZE * i) += frld.bp;
for (size_t j = 0; j < frld.lm_id.size(); j++) {
Eigen::Matrix<double, POSE_SIZE, 3> 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>(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<double, 3, POSE_SIZE> H_l_p_other =
frld_other.Hpl[other_obs[k].second].transpose();
H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i, POSE_SIZE * other_i) -=
H_pl_H_ll_inv * H_l_p_other;
}
}
}
}
void BundleAdjustmentBase::get_current_points(
Eigen::vector<Eigen::Vector3d>& points, std::vector<int>& ids) const {
points.clear();
ids.clear();
for (const auto& kv_kpt : kpts) {
Eigen::Vector3d pt_cam =
StereographicParam<double>::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<int>& idx_to_keep,
const std::set<int>& 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<int, Eigen::Dynamic, 1> 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<Eigen::Matrix<int, Eigen::Dynamic, 1>> p(
indices);
const Eigen::PermutationMatrix<Eigen::Dynamic> 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);
}
}

1054
src/vi_estimator/keypoint_vio.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

266
src/vi_estimator/keypoint_vio_linearize.cpp Normal file
View File

@ -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 <basalt/vi_estimator/keypoint_vio.h>
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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& 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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& 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

675
src/vi_estimator/nfr_mapper.cpp Normal file
View File

@ -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 <basalt/optimization/accumulator.h>
#include <basalt/utils/keypoints.h>
#include <basalt/utils/nfr.h>
#include <basalt/utils/tracks.h>
#include <basalt/vi_estimator/nfr_mapper.h>
#include <DBoW3.h>
namespace basalt {
NfrMapper::NfrMapper(const Calibration<double>& 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<int> idx_to_keep;
std::set<int> 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<double, 3, POSE_SIZE> d_pos_d_T_w_i;
Sophus::Matrix<double, 1, POSE_SIZE> d_yaw_d_T_w_i;
Sophus::Matrix<double, 2, POSE_SIZE> 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<POSE_SIZE, POSE_SIZE>(0, kf_start_idx) = d_res_d_T_w_i;
J.block<POSE_SIZE, POSE_SIZE>(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<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs, rld_error);
// SparseHashAccumulator<double> 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<SparseHashAccumulator<double>> lopt(aom,
&frame_poses);
tbb::blocked_range<Eigen::vector<RelLinData>::iterator> range(
rld_vec.begin(), rld_vec.end());
tbb::blocked_range<Eigen::vector<RollPitchFactor>::const_iterator> range1(
roll_pitch_factors.begin(), roll_pitch_factors.end());
tbb::blocked_range<Eigen::vector<RelPoseFactor>::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<POSE_SIZE>(idx));
}
// Update points
tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
auto update_points_func = [&](const tbb::blocked_range<size_t>& 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<std::chrono::microseconds>(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<int64_t, PoseStateWithLin>& 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<int64_t> 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<size_t>(0, keys.size()),
[&](const tbb::blocked_range<size_t>& 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<const uint16_t> img =
kv->second->img_data[i].img->Reinterpret<const uint16_t>();
detectKeypointsMapping(img, kd,
config.mapper_detection_num_points);
computeAngles(img, kd, true);
computeDescriptors(img, kd);
std::vector<bool> 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<std::chrono::microseconds>(t2 - t1);
auto elapsed2 =
std::chrono::duration_cast<std::chrono::microseconds>(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<TimeCamId> keys;
std::unordered_map<TimeCamId, size_t> 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<match_pair> ids_to_match;
tbb::blocked_range<size_t> keys_range(0, keys.size());
auto compute_pairs = [&](const tbb::blocked_range<size_t>& 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<int> total_matched = 0;
tbb::blocked_range<size_t> range(0, ids_to_match.size());
auto match_func = [&](const tbb::blocked_range<size_t>& 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<std::chrono::microseconds>(t2 - t1);
auto elapsed2 =
std::chrono::duration_cast<std::chrono::microseconds>(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<double>::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

153
src/vi_estimator/vio_estimator.cpp Normal file
View File

@ -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 <basalt/vi_estimator/vio_estimator.h>
#include <basalt/vi_estimator/keypoint_vio.h>
namespace basalt {
VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator(
const VioConfig& config, const Calibration<double>& 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<int64_t>& filter_t_ns,
const Eigen::vector<Eigen::Vector3d>& filter_t_w_i,
const std::vector<int64_t>& gt_t_ns,
Eigen::vector<Eigen::Vector3d>& gt_t_w_i) {
Eigen::vector<Eigen::Vector3d> est_associations;
Eigen::vector<Eigen::Vector3d> 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<double, 3, Eigen::Dynamic> 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<Eigen::Matrix3d> 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

682
src/vio.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <condition_variable>
#include <iostream>
#include <thread>
#include <sophus/se3.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <CLI/CLI.hpp>
#include <basalt/io/dataset_io.h>
#include <basalt/io/marg_data_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/serialization/headers_serialization.h>
#include <basalt/utils/vis_utils.h>
// 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<std::function<void(void)>>;
pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
pangolin::Plotter* plotter;
pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1500);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);
pangolin::Var<bool> 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<bool> continue_btn("ui.continue_btn", false, false, true);
pangolin::Var<bool> continue_fast("ui.continue_fast", true, false, true);
Button align_svd_btn("ui.align_svd", &alignButton);
pangolin::Var<bool> follow("ui.follow", true, false, true);
// Visualization variables
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
std::vector<int64_t> vio_t_ns;
Eigen::vector<Eigen::Vector3d> vio_t_w_i;
std::vector<int64_t> gt_t_ns;
Eigen::vector<Eigen::Vector3d> gt_t_w_i;
std::string marg_data_path;
size_t last_frame_processed = 0;
tbb::concurrent_unordered_map<int64_t, int> timestamp_to_id;
std::mutex m;
std::condition_variable cv;
bool step_by_step = false;
// VIO variables
basalt::Calibration<double> 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<std::mutex> 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 <euroc, bag>.")
->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<std::thread> 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<float> 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<std::thread> 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<std::shared_ptr<pangolin::ImageView>> img_view;
while (img_view.size() < calib.intrinsics.size()) {
std::shared_ptr<pangolin::ImageView> 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<size_t>(show_frame);
int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id];
std::vector<basalt::ImageData> 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<Eigen::Vector3d> 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); }

900
src/vio_sim.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <iostream>
#include <thread>
#include <sophus/se3.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>
#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>
#include <CLI/CLI.hpp>
#include <basalt/io/dataset_io.h>
#include <basalt/io/marg_data_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/sim_utils.h>
#include <basalt/utils/vis_utils.h>
#include <basalt/vi_estimator/keypoint_vio.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/serialization/headers_serialization.h>
#include <basalt/utils/vis_utils.h>
// 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<Eigen::Vector3d> gt_points;
Eigen::vector<Sophus::SE3d> gt_frame_T_w_i;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
std::vector<int64_t> gt_frame_t_ns;
Eigen::vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias,
noisy_accel, noisy_gyro, gt_vel;
std::vector<int64_t> gt_imu_t_ns;
std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
std::map<basalt::TimeCamId, basalt::SimObservations> noisy_observations;
std::string marg_data_path;
// VIO vars
basalt::Calibration<double> calib;
basalt::KeypointVioEstimator::Ptr vio;
// Visualization vars
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
std::vector<pangolin::TypedImage> images;
// Pangolin vars
constexpr int UI_WIDTH = 200;
pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
pangolin::Plotter* plotter;
pangolin::Var<int> show_frame("ui.show_frame", 0, 0, NUM_FRAMES - 1);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
pangolin::Var<bool> show_obs_noisy("ui.show_obs_noisy", true, false, true);
pangolin::Var<bool> show_obs_vio("ui.show_obs_vio", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
pangolin::Var<bool> show_accel("ui.show_accel", false, false, true);
pangolin::Var<bool> show_gyro("ui.show_gyro", false, false, true);
pangolin::Var<bool> show_gt_vel("ui.show_gt_vel", false, false, true);
pangolin::Var<bool> show_gt_pos("ui.show_gt_pos", true, false, true);
pangolin::Var<bool> show_gt_bg("ui.show_gt_bg", false, false, true);
pangolin::Var<bool> show_gt_ba("ui.show_gt_ba", false, false, true);
pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);
using Button = pangolin::Var<std::function<void(void)>>;
Button next_step_btn("ui.next_step", &next_step);
pangolin::Var<bool> 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<float>();
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<float> 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<std::shared_ptr<pangolin::ImageView>> img_view;
while (img_view.size() < calib.intrinsics.size()) {
std::shared_ptr<pangolin::ImageView> 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<float>();
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<float>();
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<float> 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<Sophus::SE3d> 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);
}

31
test/CMakeLists.txt Normal file
View File

@ -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)

127
test/src/test_image.cpp Normal file
View File

@ -0,0 +1,127 @@
#include <Eigen/Dense>
#include <basalt/utils/image.h>
#include "gtest/gtest.h"
#include "test_utils.h"
void setImageData(uint16_t *imageArray, int size) {
double norm = RAND_MAX;
norm /= (double)std::numeric_limits<uint16_t>::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<uint16_t> img(640, 480);
setImageData(img.ptr, img.size());
Eigen::Vector3d vg = img.interpGrad(offset);
Eigen::Matrix<double, 1, 2> J = vg.tail<2>();
// std::cerr << "vg\n" << vg << std::endl;
test_jacobian("d_val_d_p", J,
[&](const Eigen::Vector2d &x) {
Eigen::Matrix<double, 1, 1> 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<uint16_t> 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<double>() + 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<double>() + 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<double>() + 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<double>() + 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<double>() + 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<uint16_t> img(640, 480);
setImageData(img.ptr, img.size());
Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
Eigen::Vector3d valGrad = img.interpGrad<double>(pd);
Eigen::Matrix<double, 1, 2> J = valGrad.tail<2>();
test_jacobian(
"d_res_d_x", J,
[&](const Eigen::Vector2d &x) {
return Eigen::Matrix<double, 1, 1>(img.interp<double>(pd + x));
},
Eigen::Vector2d::Zero(), 1);
}

137
test/src/test_nfr.cpp Normal file
View File

@ -0,0 +1,137 @@
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/nfr.h>
#include <iostream>
#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<double, 3, 6> 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<double, 1, 6> 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<double, 1, 1>(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<double, 2, 6> 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);
}
}

109
test/src/test_spline_opt.cpp Normal file
View File

@ -0,0 +1,109 @@
#include <basalt/optimization/spline_optimize.h>
#include <iostream>
#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<double> accel_bias_full;
accel_bias_full.setRandom();
basalt::CalibGyroBias<double> 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";
}
}

408
test/src/test_vio.cpp Normal file
View File

@ -0,0 +1,408 @@
#include <basalt/spline/se3_spline.h>
#include <basalt/vi_estimator/keypoint_vio.h>
#include <iostream>
#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<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec;
Eigen::map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, basalt::PoseStateWithLin> 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<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec;
Eigen::map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, basalt::PoseStateWithLin> 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<double> cam =
basalt::ExtendedUnifiedCamera<double>::getTestProjections()[0];
basalt::KeypointVioEstimator::KeypointPosition kpt_pos;
Eigen::Vector4d point3d;
cam.unproject(Eigen::Vector2d::Random() * 50, point3d);
kpt_pos.dir = basalt::StereographicParam<double>::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<double>::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<double, 2, 6> d_res_d_xi;
Eigen::Matrix<double, 2, 3> 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);
}
}

Some files were not shown because too many files have changed in this diff Show More