v01

.clang-format

@@ -0,0 +1,7 @@
+---
+Language:       Cpp
+BasedOnStyle:   Google
+IndentWidth:    2
+IncludeBlocks:  Preserve
+...
+

.clang-tidy

@@ -0,0 +1,80 @@
+Checks: 'readability-*, -readability-magic-numbers, -readability-function-cognitive-complexity, -readability-else-after-return, -readability-redundant-access-specifiers, performance-*, modernize-*, -modernize-use-trailing-return-type, -modernize-avoid-c-arrays, -modernize-use-nodiscard, -modernize-use-auto, -modernize-pass-by-value, misc-assert-side-effect, -clang-analyzer-osx.*, -clang-analyzer-cplusplus.Move, -clang-analyzer-core.uninitialized.UndefReturn, -clang-analyzer-optin.portability.UnixAPI, -clang-analyzer-unix.Malloc'
+WarningsAsErrors: '*'
+HeaderFilterRegex: '.*/thirdparty/basalt-headers/include/basalt/*'
+
+CheckOptions:
+# Classes, structs, ...
+  - key:    readability-identifier-naming.NamespaceCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ClassCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.StructCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.EnumCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.UnionCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.TypedefCase
+    value:  CamelCase
+
+# Variables, member variables, ...
+  - key:    readability-identifier-naming.ParameterCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ParameterIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.VariableCase
+    value:  lower_case
+  - key:    readability-identifier-naming.VariableIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.MemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.PublicMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.PublicMemberIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.ProtectedMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ProtectedMemberSuffix
+    value:  _
+  - key:    readability-identifier-naming.ProtectedMemberIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.PrivateMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.PrivateMemberIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.PrivateMemberSuffix
+    value:  _
+
+# Functions, methods, ...
+  - key:    readability-identifier-naming.FunctionCase
+    value:  camelBack
+  - key:    readability-identifier-naming.MethodCase
+    value:  camelBack
+
+# Constants
+  - key:    readability-identifier-naming.ConstantPrefix
+    value:  ''
+  - key:    readability-identifier-naming.ConstantCase
+    value:  UPPER_CASE
+  - key:    readability-identifier-naming.ConstantMemberPrefix
+    value:  ''
+  - key:    readability-identifier-naming.ConstantMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ConstantMemberIgnoredRegexp
+    value:  '^.*_$'
+  - key:    readability-identifier-naming.ConstantParameterPrefix
+    value:  ''
+  - key:    readability-identifier-naming.ConstantParameterCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ConstantParameterIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.LocalConstantParameterPrefix
+    value:  ''
+  - key:    readability-identifier-naming.LocalConstantCase
+    value:  lower_case
+  - key:    readability-identifier-naming.LocalConstantIgnoredRegexp
+    value:  'Q[12]?_.*|[A-Z]|[JH]_.*|[TR]_[a-z]+_[a-z]+.*'
+  - key:    readability-identifier-naming.ConstexprVariablePrefix
+    value:  ''
+  - key:    readability-identifier-naming.ConstexprVariableCase
+    value:  UPPER_CASE

.gitignore

@@ -0,0 +1,8 @@
+cmake-build*
+.idea
+CMakeLists.txt.user
+build*
+scripts/eval/eval_*
+scripts/eval_full/eval_*
+stats_*.*json
+__pycache__

.gitlab-ci.yml

@@ -0,0 +1,474 @@
+image: vladyslavusenko/b_image_focal:latest
+
+variables:
+  GIT_SUBMODULE_STRATEGY: recursive
+  BUILD_TYPE: Release
+  CXX_MARCH: native
+  CMAKE_INSTALL_PREFIX: /usr/
+  DEB_DIR: deb
+
+stages:
+  - docker
+  - build
+  - test
+  - eval
+  - results
+  - deploy
+  - repository
+
+# template for docker builds with ccache
+.prepare_docker_template: &prepare_docker_definition
+  tags:
+    - docker
+  before_script:
+    - mkdir -p ccache
+    - export CCACHE_BASEDIR=${PWD}
+    - export CCACHE_DIR=${PWD}/ccache
+    - ccache -s
+  cache:
+    paths:
+    - ccache/
+    key: ${CI_JOB_NAME}
+
+# template for build & unit test & make deb configurations
+.compile_test_package_template: &compile_test_package_definition
+  stage: build
+  script:
+    - mkdir build
+    - cd build
+    - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+    - make -j3
+    - ctest
+    - make package
+    - cd ..
+    - mkdir $DEB_DIR
+    - mv build/*.deb $DEB_DIR/
+    - dpkg -i $DEB_DIR/*.deb
+    # smoke test to see if all executables at least start up
+    - basalt_calibrate --help
+    - basalt_calibrate_imu --help
+    - basalt_mapper --help
+    - basalt_mapper_sim --help
+    - basalt_mapper_sim_naive --help
+    - basalt_opt_flow --help
+    - basalt_vio --help
+    - basalt_vio_sim --help
+
+# template for build & unit test configurations (no deb)
+.compile_test_template: &compile_test_definition
+  stage: build
+  script:
+    - mkdir build
+    - cd build
+    - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+    - make -j3
+    - ctest
+    # smoke test to see if all executables at least start up
+    - ./basalt_calibrate --help
+    - ./basalt_calibrate_imu --help
+    - ./basalt_mapper --help
+    - ./basalt_mapper_sim --help
+    - ./basalt_mapper_sim_naive --help
+    - ./basalt_opt_flow --help
+    - ./basalt_vio --help
+    - ./basalt_vio_sim --help
+
+# template to test debian
+.test_deb_template: &test_deb_definition
+  stage: test
+  tags:
+    - docker
+  script:
+    - dpkg -i $DEB_DIR/*.deb
+    # smoke test to see if all executables at least start up
+    - basalt_calibrate --help
+    - basalt_calibrate_imu --help
+    - basalt_mapper --help
+    - basalt_mapper_sim --help
+    - basalt_mapper_sim_naive --help
+    - basalt_opt_flow --help
+    - basalt_vio --help
+    - basalt_vio_sim --help
+
+# template to evaluate on EuRoC sequences
+.eval_euroc_template: &eval_euroc_definition
+  stage: eval
+  parallel: 10
+  tags: [docker, dataset-eval]
+  variables:
+    GIT_STRATEGY: none
+  artifacts:
+    paths:
+    - scripts/eval_full/eval_results/*
+    expire_in: 1 week
+  script:
+    - dpkg -i deb_focal/*.deb
+    - cd scripts/eval_full
+    - ./run_evaluations.sh
+
+# template to evaluate on TUM-VI sequences
+.eval_tumvi_template: &eval_tumvi_definition
+  stage: eval
+  parallel: 4
+  tags: [docker, dataset-eval]
+  variables:
+    GIT_STRATEGY: none
+  artifacts:
+    paths:
+    - scripts/eval_full/eval_results_tumvi/*
+    expire_in: 1 week
+  script:
+    - dpkg -i deb_focal/*.deb
+    - cd scripts/eval_full
+    - ./run_evaluations_tumvi.sh
+
+
+.eval_kitti_template: &eval_kitti_definition
+  stage: eval
+  parallel: 10
+  tags: [docker, dataset-eval]
+  variables:
+    GIT_STRATEGY: none
+  artifacts:
+    paths:
+    - scripts/eval_full/eval_results_kitti/*
+    expire_in: 1 week
+  script:
+    - dpkg -i deb_focal/*.deb
+    - cd scripts/eval_full
+    - ./run_evaluations_kitti.sh
+
+bionic-release-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_bionic:latest
+#  only:
+#    - master
+  variables:
+    # compile w/ clang; GCC 7.5 has multiple issues: magic_enum is not
+    # supported, Eigen hast false positive maybe-uninitialized
+    # warnings, structured bindings give false positive
+    # "unused-variable" warnings. Using GCC 9 from the Ubuntu's
+    # toolchain testing PPA would make the built debian package depend
+    # on the updatd libstdc++. So we use clang instead, which works
+    # with the default libstdc++ on bionic and doesn't lead to
+    # additional runtime dependencies for the debian package.
+    CC: clang-12
+    CXX: clang++-12
+    CXX_MARCH: 'haswell'
+    DEB_DIR: deb_bionic
+  artifacts:
+    paths:
+    - deb_bionic/*.deb
+    expire_in: 1 week
+
+focal-release-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_focal:latest
+  variables:
+    CXX_MARCH: 'haswell'
+    DEB_DIR: deb_focal
+  artifacts:
+    paths:
+    - deb_focal/*.deb
+    - scripts/eval_full/*
+    expire_in: 1 week
+
+focal-debug-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_focal:latest
+  only:
+    - master
+  variables:
+    BUILD_TYPE: CiDebug
+
+focal-relwithdebinfo-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_focal:latest
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+  only:
+    - master
+
+focal-gcc10-relwithdebinfo-compile:
+  <<: *prepare_docker_definition
+  image: vladyslavusenko/b_image_focal:latest
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+    CC: gcc-10
+    CXX: g++-10
+  only:
+#    - master
+  stage: build
+  script:
+    - add-apt-repository ppa:ubuntu-toolchain-r/ppa -y
+    - apt-get update
+    - apt-get install -y g++-10
+    - mkdir build
+    - cd build
+    - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+    - make -j3
+    - ctest
+    # smoke test to see if all executables at least start up
+    - ./basalt_calibrate --help
+    - ./basalt_calibrate_imu --help
+    - ./basalt_mapper --help
+    - ./basalt_mapper_sim --help
+    - ./basalt_mapper_sim_naive --help
+    - ./basalt_opt_flow --help
+    - ./basalt_vio --help
+    - ./basalt_vio_sim --help
+
+# Compilation with GCC 11 is broken due to a bug that is fixed only in future relases 11.3 and 12
+# See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=100438
+# See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=99744#c15
+# focal-gcc11-relwithdebinfo-compile:
+#   <<: *prepare_docker_definition
+#   image: vladyslavusenko/b_image_focal:latest
+#   variables:
+#     BUILD_TYPE: CiRelWithDebInfo
+#     CC: gcc-11
+#     CXX: g++-11
+#   only:
+# #    - master
+#   stage: build
+#   script:
+#     - add-apt-repository ppa:ubuntu-toolchain-r/test -y
+#     - apt-get update
+#     - apt-get install -y g++-11
+#     - mkdir build
+#     - cd build
+#     - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+#     - make -j3
+#     - ctest
+#     # smoke test to see if all executables at least start up
+#     - ./basalt_calibrate --help
+#     - ./basalt_calibrate_imu --help
+#     - ./basalt_mapper --help
+#     - ./basalt_mapper_sim --help
+#     - ./basalt_mapper_sim_naive --help
+#     - ./basalt_opt_flow --help
+#     - ./basalt_vio --help
+#     - ./basalt_vio_sim --help
+
+catalina-relwithdebinfo-compile:
+  <<: *compile_test_definition
+  tags: [macos, "10.15"]
+  only:
+#    - master
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+
+catalina-brewclang-relwithdebinfo-compile:
+  <<: *compile_test_definition
+  tags: [macos, "10.15"]
+  only:
+#    - master
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+    CC: /usr/local/opt/llvm/bin/clang
+    CXX: /usr/local/opt/llvm/bin/clang++
+
+bigsur-relwithdebinfo-compile:
+  <<: *compile_test_definition
+  tags: [macos, "11", x86_64]
+  only:
+#    - master
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+
+monterey-arm-relwithdebinfo-compile:
+  <<: *compile_test_definition
+  tags: [macos, "12", arm64]
+  only:
+#    - master
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+
+
+# check if clang-format would make any changes
+clang-format:
+  tags:
+    - docker
+  stage: build
+  variables:
+    GIT_SUBMODULE_STRATEGY: none
+  script:
+    - ./scripts/clang-format-all.sh
+    # check if any files are now modified and error if yes
+    - (if [ -n "`git diff --name-only --diff-filter=M --ignore-submodules`" ]; then echo $'\n    Some files are not properly formatted. You can use "./scripts/clang-format-all.sh".\n'; git diff --diff-filter=M; false; fi)
+
+bionic-test:
+  <<: *test_deb_definition
+  image: vladyslavusenko/b_image_bionic:latest
+  only:
+    - master
+  variables:
+    DEB_DIR: deb_bionic
+    GIT_STRATEGY: none
+
+focal-test:
+  <<: *test_deb_definition
+  image: vladyslavusenko/b_image_focal:latest
+  only:
+    - master
+  variables:
+    DEB_DIR: deb_focal
+    GIT_STRATEGY: none
+
+# evaluate on EuRoC sequences
+eval_euroc_master:
+  <<: *eval_euroc_definition
+  only:
+    - master
+
+# evaluate on EuRoC sequences
+eval_euroc:
+  <<: *eval_euroc_definition
+  when: manual
+  except:
+    - master
+  allow_failure: false
+
+# evaluate on TUM-VI sequences
+eval_tumvi_master:
+  <<: *eval_tumvi_definition
+  only:
+    - master
+
+# evaluate on TUM-VI sequences
+eval_tumvi:
+  <<: *eval_tumvi_definition
+  when: manual
+  except:
+    - master
+  allow_failure: false
+
+# evaluate on KITTI sequences
+eval_kitti_master:
+  <<: *eval_kitti_definition
+  only:
+    - master
+
+# evaluate on KITTI sequences
+eval_kitti:
+  <<: *eval_kitti_definition
+  when: manual
+  except:
+    - master
+  allow_failure: false
+
+# aggregate results for all EuRoC sequences
+gen_results:
+  stage: results
+  variables:
+    GIT_STRATEGY: none
+  tags: 
+    - docker
+  when: on_success
+  artifacts:
+    paths:
+    - euroc_results.txt
+    - kitti_results.txt
+    - tumvi_results.txt
+    - scripts/eval_full/eval_results/*
+  script:
+    - cd scripts/eval_full
+    - ./gen_results.py eval_results > euroc_results.txt
+    - cat euroc_results.txt
+    - ./gen_results_kitti.py eval_results_kitti > kitti_results.txt
+    - cat kitti_results.txt
+    - ./gen_results_tumvi.py eval_results_tumvi > tumvi_results.txt
+    - cat tumvi_results.txt
+    - mv euroc_results.txt ../../
+    - mv kitti_results.txt ../../
+    - mv tumvi_results.txt ../../
+
+# deploy deb packages
+deploy:
+  stage: deploy
+  when: manual
+  variables:
+    GIT_STRATEGY: none
+  tags: 
+    - docker
+  only:
+    - master
+  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
+  script:
+    - scp $SCP_FLAGS deb_bionic/*.deb $REPOSITORY_URL/bionic/
+    - scp $SCP_FLAGS deb_focal/*.deb $REPOSITORY_URL/focal/
+    
+
+.docker_build_template: &docker_build_definition
+  image: docker:stable
+  stage: docker
+  when: manual
+  only:
+    - master
+  services:
+    - docker:stable-dind
+  tags:
+    - docker-dind
+  before_script:
+    - echo "$CI_REGISTRY_PASSWORD" | docker login -u "$CI_REGISTRY_USER" --password-stdin $CI_REGISTRY
+  script:
+    - cd docker/"$B_IMAGE_NAME"
+    - docker build -t "$CI_REGISTRY_IMAGE"/"$B_IMAGE_NAME" .
+    - docker push "$CI_REGISTRY_IMAGE"/"$B_IMAGE_NAME"
+
+
+docker-build-bionic:
+  <<: *docker_build_definition
+  variables:
+    DOCKER_TLS_CERTDIR: '/certs'
+    GIT_SUBMODULE_STRATEGY: none
+    B_IMAGE_NAME: b_image_bionic
+
+docker-build-focal:
+  <<: *docker_build_definition
+  variables:
+    DOCKER_TLS_CERTDIR: '/certs'
+    GIT_SUBMODULE_STRATEGY: none
+    B_IMAGE_NAME: b_image_focal
+
+
+
+.repository_check_template: &repository_check_definition
+  stage: repository
+  when: manual
+  tags:
+    - docker
+  only:
+    - master
+  script:
+    - apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y gnupg2 rsync linux-generic
+    - apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 0AD9A3000D97B6C9
+    - echo "deb [arch=amd64] http://packages.usenko.eu/ubuntu $REPO_NAME $REPO_NAME/main" > /etc/apt/sources.list.d/basalt.list
+    - apt-get update && apt-get dist-upgrade -y
+    - DEBIAN_FRONTEND=noninteractive apt-get install -y basalt
+    - basalt_rs_t265_vio --help
+    - basalt_rs_t265_record --help
+
+bionic-repository-check:
+  image: ubuntu:18.04
+  <<: *repository_check_definition
+  variables:
+    GIT_STRATEGY: none
+    REPO_NAME: bionic
+
+focal-repository-check:
+  image: ubuntu:20.04
+  <<: *repository_check_definition
+  variables:
+    GIT_STRATEGY: none
+    REPO_NAME: focal

.gitmodules

@@ -0,0 +1,25 @@
+[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
+	url = https://github.com/NikolausDemmel/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
+[submodule "thirdparty/magic_enum"]
+	path = thirdparty/magic_enum
+	url = https://github.com/Neargye/magic_enum.git

.style.yapf

@@ -0,0 +1,3 @@
+[style]
+based_on_style = google
+column_limit = 120

CMakeLists.txt

@@ -0,0 +1,411 @@
+cmake_minimum_required(VERSION 3.10...3.18)
+
+project(basalt)
+
+set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/" ${CMAKE_MODULE_PATH})
+
+if(NOT EIGEN_ROOT)
+  set(EIGEN_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/basalt-headers/thirdparty/eigen")
+endif()
+
+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 "20.04")
+    set(DEBIAN_DEPENDS "libtbb2, liblz4-1, libbz2-1.0, libboost-filesystem1.71.0, libboost-date-time1.71.0, libboost-program-options1.71.0, libboost-regex1.71.0, libopencv-dev, libglew2.1, libjpeg8, libpng16-16, librealsense2, librealsense2-dkms, librealsense2-gl, librealsense2-utils")
+
+  elseif(${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, librealsense2, librealsense2-dkms, librealsense2-gl, librealsense2-utils")
+
+  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, libstdc++6, librealsense2, librealsense2-dkms, librealsense2-gl, librealsense2-utils")
+  endif()
+
+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
+if (NOT CMAKE_C_COMPILER_LAUNCHER AND NOT CMAKE_CXX_COMPILER_LAUNCHER)
+  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})
+  else()
+    message(STATUS "Dind't find ccache")
+  endif()
+else()
+  message(STATUS "Compiler launcher already set. Not configuring ccache.")
+  message(STATUS "CMAKE_C_COMPILER_LAUNCHER: ${CMAKE_C_COMPILER_LAUNCHER}")
+  message(STATUS "CMAKE_CXX_COMPILER_LAUNCHER: ${CMAKE_CXX_COMPILER_LAUNCHER}")
+endif()
+
+if( NOT CMAKE_BUILD_TYPE )
+  set( CMAKE_BUILD_TYPE Release)
+endif()
+
+if(NOT CXX_MARCH)
+  set(CXX_MARCH native)
+endif()
+
+IF(NOT APPLE OR NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64")
+  set(BASALT_MARCH_FLAGS "-march=${CXX_MARCH}")
+ELSE()
+  message(STATUS "Running on Apple ${CMAKE_SYSTEM_PROCESSOR}. Disabled -march flag.")
+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-error")
+
+
+# save flags passed by user
+set(BASALT_PASSED_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+
+set(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g -DEIGEN_INITIALIZE_MATRICES_BY_NAN")          # cmake default: "-g"
+set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DEIGEN_INITIALIZE_MATRICES_BY_NAN")  # cmake default: "-O2 -g -DNDEBUG"
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")                                      # cmake default: "-O3 -DNDEBUG"
+set(CMAKE_CXX_FLAGS_CIDEBUG  "-O0 -DEIGEN_INITIALIZE_MATRICES_BY_NAN")          # CI version with no debug symbols
+set(CMAKE_CXX_FLAGS_CIRELWITHDEBINFO "-O3 -DEIGEN_INITIALIZE_MATRICES_BY_NAN")  # CI version with no debug symbols
+
+# base set of compile flags
+set(BASALT_CXX_FLAGS "-Wall -Wextra -Werror -Wno-error=unused-parameter -ftemplate-backtrace-limit=0")
+
+# clang-specific compile flags
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+  set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-exceptions -fcolor-diagnostics -frelaxed-template-template-args -Wno-error=deprecated-declarations")
+
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 10)
+      # These are disabled to avoid lot's of warnings in Eigen code with clang 10
+      set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-misleading-indentation -Wno-error=deprecated-copy")
+  endif()
+
+  #   - Added TBB_USE_GLIBCXX_VERSION macro to specify the version of GNU
+  #     libstdc++ when it cannot be properly recognized, e.g. when used
+  #     with Clang on Linux* OS. Adopted from https://github.com/wjakob/tbb
+  if(NOT TBB_USE_GLIBCXX_VERSION AND UNIX AND NOT APPLE)
+    if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+      string(REPLACE "." "0" TBB_USE_GLIBCXX_VERSION ${CMAKE_CXX_COMPILER_VERSION})
+    endif()
+    add_definitions(-DTBB_USE_GLIBCXX_VERSION=${TBB_USE_GLIBCXX_VERSION})
+  endif()
+else()
+  set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-error=maybe-uninitialized -Wno-error=implicit-fallthrough")
+
+  if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 9)
+      # These are disabled to avoid lot's of warnings in Eigen code with gcc-9
+      set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-error=deprecated-copy")
+  endif()
+  if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 10.0)
+    # These are disabled due to warnings in pangolin
+    #set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-error=parentheses")
+    set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-parentheses")
+  endif()
+endif()
+
+
+# Set platform / compiler specific compile flags and checks
+if(APPLE)
+  # Need to investigate how to reliably detect and use OpenMP on macOS...
+#  set(USE_OPENMP_DEFAULT OFF)
+
+  # Among others, setting CMAKE_FIND_FRAMEWORK to LAST fixed issues
+  # with installed Mono that contains old headers (libpng, ...).
+  # See: https://github.com/openMVG/openMVG/issues/1349#issuecomment-401492811
+  set(CMAKE_FIND_FRAMEWORK LAST)
+
+  if(CMAKE_SYSTEM_VERSION VERSION_LESS 19.0.0)
+      # use brewed llvm's libc++
+      include_directories("/usr/local/opt/llvm/include/c++/v1")
+      link_directories("/usr/local/opt/llvm/lib")
+      add_compile_options("-nostdinc++")
+      #set(STD_CXX_FS c++fs)
+
+      # Workaround for cmake not to filter the manually added standard include path
+      # See: https://gitlab.kitware.com/cmake/cmake/issues/19227#note_669894
+      list(REMOVE_ITEM CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES "/usr/local/opt/llvm/include/c++/v1")
+  endif()
+
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    message(STATUS "Detected macOS with non-Apple clang")
+
+  elseif(CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+    message(STATUS "Detected macOS with Apple clang")
+    # Apple clang on macOS < 10.14 Mojave is too old
+    if(CMAKE_SYSTEM_VERSION VERSION_LESS 18.0.0)
+      message(WARNING "Detected Darwin version ${CMAKE_SYSTEM_VERSION}, which is earlier than macos 10.14 Mojave. Apple clang is too old and not supported. Use clang from homebrew.")
+    endif()
+
+  else()
+    message(WARNING "Detected macOS with unsupported compiler ${CMAKE_CXX_COMPILER_ID}")
+  endif()
+
+elseif(UNIX)
+#  set(USE_OPENMP_DEFAULT ON)
+
+  # assume libstdc++
+  set(STD_CXX_FS stdc++fs)
+
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    message(STATUS "Detected Linux with clang.")
+
+    # Note: Whatever the issue was, this seems to not be an issue any more, at least with more recent clang and Ubuntu versions (tested clang-12 and Ubuntu 18.04)
+    #message(WARNING "Clang on Linux is currently not fully supported. You'll likely need to get a recent version of TBB.")
+    #set(LINUX_CLANG 1)
+
+  elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+    message(STATUS "Detected Linux with gcc.")
+
+  else()
+    message(WARNING "Detected Linux with unsupported compiler ${CMAKE_CXX_COMPILER_ID}")
+  endif()
+
+else()
+  message(WARNING "Only Linux and macOS are currently supported")
+endif()
+
+
+# OpenMP option and compile flags
+#
+# Note: OpenMP and TBB don't mix well, so we disable Eigen's parallelization.
+# It's trying to paralellize matrix products during SC, which we run in a parallel_reduce using TBB.
+# Turns out using OpenMP can slow down the computby factor 10-100x! So for now we discable it completely.
+# One place where Eigen's parallelization could still have been useful is the CG solver in the mapper.
+# We could in the future investiagte other implementations (paralellized with TBB) or selectively enabling
+# Eigen's parallelization just for CG, setting number of threads to 1 everywhere else.
+# Another way to ensure Eigen doesn't use OpenMP regardless of how it was built is setting the environment
+# variable OMP_NUM_THREADS=1 beofre running the application.
+#
+# See: https://eigen.tuxfamily.org/dox/TopicMultiThreading.html
+#
+# If we enable BLAS / LAPACK either directly or via thirdparty libs like ceres,
+# make sure to disable OpenMP for the linked BLAS library. In particular on Ubuntu it seems OpenBLAS is often installed,
+# and it can have similar issues in multithreaded applications if it's own parallelization with OpenMP is enabled.
+# You can set the environment varaibles OPENBLAS_NUM_THREADS=1 or OMP_NUM_THREADS=1. This is also mentioned in the ceres
+# installation documentation.
+#
+# See also: https://github.com/xianyi/OpenBLAS/wiki/faq#multi-threaded
+#
+# Set EIGEN_DONT_PARALLELIZE to be sure it doesn't use openmp,
+# just in case some dependency enables openmp without us noticing.
+set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE")
+
+#option(USE_OPENMP "Use OpenMP (e.g. for parallel computation in Eigen)" ${USE_OPENMP_DEFAULT})
+#if(USE_OPENMP)
+#  message(STATUS "OpenMP Enabled")
+#  set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -fopenmp")
+#else()
+#  message(STATUS "OpenMP Disabled")
+#endif()
+
+# Make specialization for double / float optional.
+# Disabling them can be useful for better compile times during development.
+option(BASALT_INSTANTIATIONS_DOUBLE "Instatiate templates for Scalar=double." ON)
+option(BASALT_INSTANTIATIONS_FLOAT "Instatiate templates for Scalar=float." ON)
+
+if(BASALT_INSTANTIATIONS_DOUBLE)
+  list(APPEND BASALT_COMPILE_DEFINITIONS BASALT_INSTANTIATIONS_DOUBLE)
+endif()
+if(BASALT_INSTANTIATIONS_FLOAT)
+  list(APPEND BASALT_COMPILE_DEFINITIONS BASALT_INSTANTIATIONS_FLOAT)
+endif()
+
+
+# setup combined compiler flags
+set(CMAKE_CXX_FLAGS "${BASALT_CXX_FLAGS} ${BASALT_MARCH_FLAGS} ${BASALT_PASSED_CXX_FLAGS}")
+
+
+set(EIGEN_INCLUDE_DIR_HINTS ${EIGEN_ROOT})
+find_package(Eigen3 3.4.0 EXACT REQUIRED MODULE)
+message(STATUS "Found Eigen headers in: ${EIGEN3_INCLUDE_DIR}")
+if(NOT EIGEN3_INCLUDE_DIR MATCHES "^${EIGEN_ROOT}")
+  message(WARNING "Found Eigen headers are outside of specified EIGEN_ROOT '${EIGEN_ROOT}'")
+endif()
+
+find_package(TBB REQUIRED)
+message(STATUS "Found TBB ${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR} (interface version ${TBB_INTERFACE_VERSION}) headers in: ${TBB_INCLUDE_DIRS}")
+if (TBB_INTERFACE_VERSION LESS 11004)
+  # enable global_control header for earlier TBB versions (Ubuntu 16.04, 18.04)
+  add_definitions(-DTBB_PREVIEW_GLOBAL_CONTROL)
+endif()
+
+# NOTE: not specifying version, since 2, 3 or 4 is fine
+find_package(OpenCV REQUIRED COMPONENTS core imgproc calib3d highgui)
+message(STATUS "Found OpenCV ${OpenCV_VERSION} headers in: ${OpenCV_INCLUDE_DIRS}")
+message(STATUS "Found OpenCV_LIBS: ${OpenCV_LIBS}")
+
+# Add our own custom scoped opencv target since none is provided by OpenCV itself
+add_library(basalt::opencv INTERFACE IMPORTED)
+set_property(TARGET basalt::opencv PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${OpenCV_INCLUDE_DIRS})
+set_property(TARGET basalt::opencv PROPERTY INTERFACE_LINK_LIBRARIES ${OpenCV_LIBS})
+
+find_package(fmt REQUIRED)
+message(STATUS "Found {fmt} ${fmt_VERSION} in: ${fmt_DIR}")
+
+add_subdirectory(thirdparty)
+
+# custom scoped cli11 target
+add_library(basalt::cli11 INTERFACE IMPORTED)
+set_property(TARGET basalt::cli11 PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/CLI11/include)
+
+# custom scoped magic_enum target
+add_library(basalt::magic_enum INTERFACE IMPORTED)
+set_property(TARGET basalt::magic_enum PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/magic_enum/include)
+
+
+add_library(basalt SHARED
+  src/io/dataset_io.cpp
+  src/io/marg_data_io.cpp
+  src/calibration/aprilgrid.cpp
+  src/calibration/calibraiton_helper.cpp
+  src/calibration/vignette.cpp
+  src/optical_flow/optical_flow.cpp
+  src/linearization/landmark_block.cpp
+  src/linearization/linearization_base.cpp
+  src/linearization/linearization_abs_qr.cpp
+  src/linearization/linearization_abs_sc.cpp
+  src/linearization/linearization_rel_sc.cpp
+  src/utils/vio_config.cpp
+  src/utils/system_utils.cpp
+  src/utils/time_utils.cpp
+  src/utils/keypoints.cpp
+  src/vi_estimator/marg_helper.cpp
+  src/vi_estimator/sqrt_keypoint_vio.cpp
+  src/vi_estimator/sqrt_keypoint_vo.cpp
+  src/vi_estimator/vio_estimator.cpp
+  src/vi_estimator/ba_base.cpp
+  src/vi_estimator/sqrt_ba_base.cpp
+  src/vi_estimator/sc_ba_base.cpp
+  src/vi_estimator/nfr_mapper.cpp
+  src/vi_estimator/landmark_database.cpp)
+
+
+target_link_libraries(basalt
+  PUBLIC ${STD_CXX_FS} basalt::opencv basalt::basalt-headers TBB::tbb
+  PRIVATE basalt::magic_enum rosbag apriltag opengv nlohmann::json fmt::fmt)
+target_include_directories(basalt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_compile_definitions(basalt PUBLIC ${BASALT_COMPILE_DEFINITIONS})
+#target_compile_definitions(basalt PUBLIC BASALT_DISABLE_ASSERTS)
+
+
+add_executable(basalt_calibrate src/calibrate.cpp src/calibration/cam_calib.cpp)
+target_link_libraries(basalt_calibrate basalt pangolin basalt::cli11)
+
+add_executable(euler2RotTest test/ivan-tests/euler2RotTest.cpp)
+
+add_executable(basalt_calibrate_imu src/calibrate_imu.cpp src/calibration/cam_imu_calib.cpp)
+target_link_libraries(basalt_calibrate_imu basalt pangolin basalt::cli11)
+
+
+add_executable(basalt_vio_sim src/vio_sim.cpp)
+target_link_libraries(basalt_vio_sim basalt pangolin basalt::cli11)
+
+add_executable(basalt_mapper_sim src/mapper_sim.cpp)
+target_link_libraries(basalt_mapper_sim basalt pangolin basalt::cli11)
+
+# mapper sim native doesn't use template free interface
+if(BASALT_INSTANTIATIONS_DOUBLE)
+  add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp)
+  target_link_libraries(basalt_mapper_sim_naive basalt pangolin basalt::cli11)
+endif()
+
+add_executable(basalt_mapper src/mapper.cpp)
+target_link_libraries(basalt_mapper basalt pangolin basalt::cli11)
+
+
+add_executable(basalt_opt_flow src/opt_flow.cpp)
+target_link_libraries(basalt_opt_flow basalt pangolin basalt::cli11)
+
+add_executable(basalt_vio src/vio.cpp)
+target_link_libraries(basalt_vio basalt pangolin basalt::cli11)
+
+add_executable(basalt_time_alignment src/time_alignment.cpp)
+target_link_libraries(basalt_time_alignment basalt pangolin basalt::cli11)
+
+add_executable(basalt_kitti_eval src/kitti_eval.cpp)
+target_link_libraries(basalt_kitti_eval basalt::basalt-headers basalt::cli11)
+
+find_package(realsense2 QUIET)
+if(realsense2_FOUND)
+  add_executable(basalt_rs_t265_record src/rs_t265_record.cpp src/device/rs_t265.cpp)
+  target_link_libraries(basalt_rs_t265_record basalt realsense2::realsense2 pangolin basalt::cli11)
+
+  add_executable(basalt_rs_t265_vio src/rs_t265_vio.cpp src/device/rs_t265.cpp)
+  target_link_libraries(basalt_rs_t265_vio basalt realsense2::realsense2 pangolin basalt::cli11)
+endif()
+
+
+
+install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper basalt_opt_flow basalt_vio basalt_kitti_eval basalt_time_alignment basalt
+  EXPORT BasaltTargets
+  RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
+  LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
+
+if(BASALT_INSTANTIATIONS_DOUBLE)
+  install(TARGETS basalt_mapper_sim_naive
+    EXPORT BasaltTargets
+    RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
+    LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
+    ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
+endif()
+
+if(realsense2_FOUND)
+    install(TARGETS basalt_rs_t265_record basalt_rs_t265_vio
+      EXPORT BasaltTargets
+      RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
+      LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
+      ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
+endif()
+
+file(GLOB SCRIPTS_TO_INSTALL "${CMAKE_CURRENT_SOURCE_DIR}/scripts/basalt_*.py")
+install(PROGRAMS ${SCRIPTS_TO_INSTALL} DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
+
+file(GLOB CONFIG_FILES "${CMAKE_CURRENT_SOURCE_DIR}/data/*.json")
+install(FILES ${CONFIG_FILES}
+  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().
+enable_testing()
+add_subdirectory(thirdparty/basalt-headers/test)
+add_subdirectory(test)
+# Restore original install() behavior.
+macro(install)
+  _install(${ARGN})
+endmacro()

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,75 @@
+[![pipeline status](https://gitlab.com/VladyslavUsenko/basalt/badges/master/pipeline.svg)](https://gitlab.com/VladyslavUsenko/basalt/commits/master)
+
+## 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.
+
+Some reusable components of the system are available as a separate [header-only library](https://gitlab.com/VladyslavUsenko/basalt-headers) ([Documentation](https://vladyslavusenko.gitlab.io/basalt-headers/)).
+
+There is also a [Github mirror](https://github.com/VladyslavUsenko/basalt-mirror) of this project to enable easy forking.
+
+## 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 IEEE Robotics and Automation Letters (RA-L) [[DOI:10.1109/LRA.2019.2961227]](https://doi.org/10.1109/LRA.2019.2961227) [[arXiv:1904.06504]](https://arxiv.org/abs/1904.06504).
+
+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).
+
+Calibration (describes B-spline trajectory representation used in camera-IMU calibration):
+* **Efficient Derivative Computation for Cumulative B-Splines on Lie Groups**, C. Sommer, V. Usenko, D. Schubert, N. Demmel, D. Cremers, In 2020 Conference on Computer Vision and Pattern Recognition (CVPR), [[DOI:10.1109/CVPR42600.2020.01116]](https://doi.org/10.1109/CVPR42600.2020.01116), [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860).
+
+Optimization (describes square-root optimization and marginalization used in VIO/VO):
+* **Square Root Marginalization for Sliding-Window Bundle Adjustment**, N. Demmel, D. Schubert, C. Sommer, D. Cremers, V. Usenko, In 2021 International Conference on Computer Vision (ICCV), [[arXiv:2109.02182]](https://arxiv.org/abs/2109.02182)
+
+
+## Installation
+### APT installation for Ubuntu 20.04 and 18.04 (Fast)
+Set up keys, add the repository to the sources list, update the Ubuntu package index and install Basalt:
+```
+sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 0AD9A3000D97B6C9
+sudo sh -c 'echo "deb [arch=amd64] http://packages.usenko.eu/ubuntu $(lsb_release -sc) $(lsb_release -sc)/main" > /etc/apt/sources.list.d/basalt.list'
+sudo apt-get update
+sudo apt-get dist-upgrade
+sudo apt-get install basalt
+```
+
+### Source installation for Ubuntu >= 18.04 and MacOS >= 10.14 Mojave
+Clone the source code for the project and build it. For MacOS you should have [Homebrew](https://brew.sh/) installed.
+```
+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
+```
+
+## Usage
+* [Camera, IMU and Mocap calibration. (TUM-VI, Euroc, UZH-FPV and Kalibr datasets)](doc/Calibration.md)
+* [Visual-inertial odometry and mapping. (TUM-VI and Euroc datasets)](doc/VioMapping.md)
+* [Visual odometry (no IMU). (KITTI dataset)](doc/Vo.md)
+* [Simulation tools to test different components of the system.](doc/Simulation.md)
+* [Batch evaluation tutorial (ICCV'21 experiments)](doc/BatchEvaluation.md)
+
+## Device support
+* [Tutorial on Camera-IMU and Motion capture calibration with Realsense T265.](doc/Realsense.md)
+
+## Development
+* [Development environment setup.](doc/DevSetup.md)
+
+## Licence
+The code is provided under a BSD 3-clause license. See the LICENSE file for details.
+Note also the different licenses of thirdparty submodules.
+
+Some improvements are ported back from the fork
+[granite](https://github.com/DLR-RM/granite) (MIT license).

cmake_modules/FindEigen3.cmake

@@ -0,0 +1,82 @@
+# - 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.
+
+# Adaptations (c) Nikolaus Demmel 2019
+# - pass NO_DEFAULT_PATH --> only works when passing exact HINTS
+
+
+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}
+      NO_DEFAULT_PATH
+    )
+
+  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)

cmake_modules/FindOpenGV.cmake

@@ -0,0 +1,85 @@
+
+# This is FindOPENGV.cmake
+# CMake module to locate the OPENGV package
+#
+# The following cache variables may be set before calling this script:
+#
+# OPENGV_DIR (or OPENGV_ROOT): (Optional) The install prefix OR source tree of opengv (e.g. /usr/local or src/opengv)
+# OPENGV_BUILD_NAME:          (Optional) If compiling against a source tree, the name of the build directory
+#                            within it (e.g build-debug).  Without this defined, this script tries to
+#                            intelligently find the build directory based on the project's build directory name
+#                            or based on the build type (Debug/Release/etc).
+#
+# The following variables will be defined:
+#
+# OPENGV_FOUND          : TRUE if the package has been successfully found
+# OPENGV_INCLUDE_DIR    : paths to OPENGV's INCLUDE directories
+# OPENGV_LIBS           : paths to OPENGV's libraries
+#
+# NOTES on compiling against an uninstalled OPENGV build tree:
+# - A OPENGV source tree will be automatically searched for in the directory
+#   'opengv' next to your project directory, after searching
+#   CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr.
+# - The build directory will be searched first with the same name as your
+#   project's build directory, e.g. if you build from 'MyProject/build-optimized',
+#   'opengv/build-optimized' will be searched first.  Next, a build directory for
+#   your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is
+#   'Release', then 'opengv/build-release' will be searched next.  Finally, plain
+#   'opengv/build' will be searched.
+# - You can control the opengv build directory name directly by defining the CMake
+#   cache variable 'OPENGV_BUILD_NAME', then only 'opengv/${OPENGV_BUILD_NAME} will
+#   be searched.
+# - Use the standard CMAKE_PREFIX_PATH, or OPENGV_DIR, to find a specific opengv
+#   directory.
+
+# Get path suffixes to help look for opengv
+if(OPENGV_BUILD_NAME)
+  set(opengv_build_names "${OPENGV_BUILD_NAME}/opengv")
+else()
+  # lowercase build type
+  string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix)
+  # build suffix of this project
+  get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME)
+  
+  set(opengv_build_names "${my_build_name}/opengv" "build-${build_type_suffix}/opengv" "build/opengv" "build/lib")
+endif()
+
+# Use OPENGV_ROOT or OPENGV_DIR equivalently
+if(OPENGV_ROOT AND NOT OPENGV_DIR)
+  set(OPENGV_DIR "${OPENGV_ROOT}")
+endif()
+
+if(OPENGV_DIR)
+  # Find include dirs
+  find_path(OPENGV_INCLUDE_DIR opengv/types.hpp
+    PATHS "${OPENGV_DIR}/include" "${OPENGV_DIR}" NO_DEFAULT_PATH
+    DOC "OPENGV include directories")
+
+  # Find libraries
+  find_library(OPENGV_LIBS NAMES opengv
+    HINTS "${OPENGV_DIR}/lib" "${OPENGV_DIR}" NO_DEFAULT_PATH
+    PATH_SUFFIXES ${opengv_build_names}
+    DOC "OPENGV libraries")
+else()
+  # Find include dirs
+  set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../opengv" /usr/local/include /usr/include)
+  find_path(OPENGV_INCLUDE_DIR opengv/types.hpp
+    PATHS ${extra_include_paths}
+    DOC "OPENGV include directories")
+  if(NOT OPENGV_INCLUDE_DIR)
+    message(STATUS "Searched for opengv headers in default paths plus ${extra_include_paths}")
+  endif()
+
+  # Find libraries
+  find_library(OPENGV_LIBS NAMES opengv
+    HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../opengv" /usr/local/lib /usr/lib
+    PATH_SUFFIXES ${opengv_build_names}
+    DOC "OPENGV libraries")
+endif()
+
+# handle the QUIETLY and REQUIRED arguments and set OPENGV_FOUND to TRUE
+# if all listed variables are TRUE
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(OPENGV DEFAULT_MSG
+                                  OPENGV_LIBS OPENGV_INCLUDE_DIR)
+ 

cmake_modules/FindTBB.cmake

@@ -0,0 +1,468 @@
+# - Find ThreadingBuildingBlocks include dirs and libraries
+# Use this module by invoking find_package with the form:
+#  find_package(TBB
+#    [REQUIRED]             # Fail with error if TBB is not found
+#    )                      #
+# Once done, this will define
+#
+#  TBB_FOUND - system has TBB
+#  TBB_INCLUDE_DIRS - the TBB include directories
+#  TBB_LIBRARIES - TBB libraries to be lined, doesn't include malloc or
+#                  malloc proxy
+#  TBB::tbb - imported target for the TBB library
+#
+#  TBB_VERSION_MAJOR - Major Product Version Number
+#  TBB_VERSION_MINOR - Minor Product Version Number
+#  TBB_INTERFACE_VERSION - Engineering Focused Version Number
+#  TBB_COMPATIBLE_INTERFACE_VERSION - The oldest major interface version
+#                                     still supported. This uses the engineering
+#                                     focused interface version numbers.
+#
+#  TBB_MALLOC_FOUND - system has TBB malloc library
+#  TBB_MALLOC_INCLUDE_DIRS - the TBB malloc include directories
+#  TBB_MALLOC_LIBRARIES - The TBB malloc libraries to be lined
+#  TBB::malloc - imported target for the TBB malloc library
+#
+#  TBB_MALLOC_PROXY_FOUND - system has TBB malloc proxy library
+#  TBB_MALLOC_PROXY_INCLUDE_DIRS = the TBB malloc proxy include directories
+#  TBB_MALLOC_PROXY_LIBRARIES - The TBB malloc proxy libraries to be lined
+#  TBB::malloc_proxy - imported target for the TBB malloc proxy library
+#
+#
+# This module reads hints about search locations from variables:
+#  ENV TBB_ARCH_PLATFORM - for eg. set it to "mic" for Xeon Phi builds
+#  ENV TBB_ROOT or just TBB_ROOT - root directory of tbb installation
+#  ENV TBB_BUILD_PREFIX - specifies the build prefix for user built tbb
+#                         libraries. Should be specified with ENV TBB_ROOT
+#                         and optionally...
+#  ENV TBB_BUILD_DIR - if build directory is different than ${TBB_ROOT}/build
+#
+#
+# Modified by Robert Maynard from the original OGRE source
+#
+#-------------------------------------------------------------------
+# This file is part of the CMake build system for OGRE
+#     (Object-oriented Graphics Rendering Engine)
+# For the latest info, see http://www.ogre3d.org/
+#
+# The contents of this file are placed in the public domain. Feel
+# free to make use of it in any way you like.
+#-------------------------------------------------------------------
+#
+# =========================================================================
+# Taken from Copyright.txt in the root of the VTK source tree as per
+# instructions to substitute the full license in place of the summary
+# reference when distributing outside of VTK
+# =========================================================================
+#
+#  Program:   Visualization Toolkit
+#  Module:    Copyright.txt
+#
+# Copyright (c) 1993-2015 Ken Martin, Will Schroeder, Bill Lorensen
+# 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 name of Ken Martin, Will Schroeder, or Bill Lorensen nor the names
+#   of any 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 AUTHORS 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.
+#
+# =========================================================================*/
+
+#=============================================================================
+#  FindTBB helper functions and macros
+#
+
+#====================================================
+# Fix the library path in case it is a linker script
+#====================================================
+function(tbb_extract_real_library library real_library)
+  if(NOT UNIX OR NOT EXISTS ${library})
+    set(${real_library} "${library}" PARENT_SCOPE)
+    return()
+  endif()
+
+  #Read in the first 4 bytes and see if they are the ELF magic number
+  set(_elf_magic "7f454c46")
+  file(READ ${library} _hex_data OFFSET 0 LIMIT 4 HEX)
+  if(_hex_data STREQUAL _elf_magic)
+    #we have opened a elf binary so this is what
+    #we should link to
+    set(${real_library} "${library}" PARENT_SCOPE)
+    return()
+  endif()
+
+  file(READ ${library} _data OFFSET 0 LIMIT 1024)
+  if("${_data}" MATCHES "INPUT \\(([^(]+)\\)")
+    #extract out the .so name from REGEX MATCH command
+    set(_proper_so_name "${CMAKE_MATCH_1}")
+
+    #construct path to the real .so which is presumed to be in the same directory
+    #as the input file
+    get_filename_component(_so_dir "${library}" DIRECTORY)
+    set(${real_library} "${_so_dir}/${_proper_so_name}" PARENT_SCOPE)
+  else()
+    #unable to determine what this library is so just hope everything works
+    #and pass it unmodified.
+    set(${real_library} "${library}" PARENT_SCOPE)
+  endif()
+endfunction()
+
+#===============================================
+# Do the final processing for the package find.
+#===============================================
+macro(findpkg_finish PREFIX TARGET_NAME)
+  if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY)
+    set(${PREFIX}_FOUND TRUE)
+    set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR})
+    set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY})
+  else ()
+    if (${PREFIX}_FIND_REQUIRED AND NOT ${PREFIX}_FIND_QUIETLY)
+      message(FATAL_ERROR "Required library ${PREFIX} not found.")
+    endif ()
+  endif ()
+
+  if (NOT TARGET "TBB::${TARGET_NAME}")
+    if (${PREFIX}_LIBRARY_RELEASE)
+      tbb_extract_real_library(${${PREFIX}_LIBRARY_RELEASE} real_release)
+    endif ()
+    if (${PREFIX}_LIBRARY_DEBUG)
+      tbb_extract_real_library(${${PREFIX}_LIBRARY_DEBUG} real_debug)
+    endif ()
+    add_library(TBB::${TARGET_NAME} UNKNOWN IMPORTED)
+    set_target_properties(TBB::${TARGET_NAME} PROPERTIES
+      INTERFACE_INCLUDE_DIRECTORIES "${${PREFIX}_INCLUDE_DIR}")
+    if (${PREFIX}_LIBRARY_DEBUG AND ${PREFIX}_LIBRARY_RELEASE)
+      set_target_properties(TBB::${TARGET_NAME} PROPERTIES
+        IMPORTED_LOCATION "${real_release}"
+        IMPORTED_LOCATION_DEBUG "${real_debug}"
+        IMPORTED_LOCATION_RELEASE "${real_release}")
+    elseif (${PREFIX}_LIBRARY_RELEASE)
+      set_target_properties(TBB::${TARGET_NAME} PROPERTIES
+        IMPORTED_LOCATION "${real_release}")
+    elseif (${PREFIX}_LIBRARY_DEBUG)
+      set_target_properties(TBB::${TARGET_NAME} PROPERTIES
+        IMPORTED_LOCATION "${real_debug}")
+    endif ()
+  endif ()
+
+  #mark the following variables as internal variables
+  mark_as_advanced(${PREFIX}_INCLUDE_DIR
+                   ${PREFIX}_LIBRARY
+                   ${PREFIX}_LIBRARY_DEBUG
+                   ${PREFIX}_LIBRARY_RELEASE)
+endmacro()
+
+#===============================================
+# Generate debug names from given release names
+#===============================================
+macro(get_debug_names PREFIX)
+  foreach(i ${${PREFIX}})
+    set(${PREFIX}_DEBUG ${${PREFIX}_DEBUG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i})
+  endforeach()
+endmacro()
+
+#===============================================
+# See if we have env vars to help us find tbb
+#===============================================
+macro(getenv_path VAR)
+   set(ENV_${VAR} $ENV{${VAR}})
+   # replace won't work if var is blank
+   if (ENV_${VAR})
+     string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} )
+   endif ()
+endmacro()
+
+#===============================================
+# Couple a set of release AND debug libraries
+#===============================================
+macro(make_library_set PREFIX)
+  if (${PREFIX}_RELEASE AND ${PREFIX}_DEBUG)
+    set(${PREFIX} optimized ${${PREFIX}_RELEASE} debug ${${PREFIX}_DEBUG})
+  elseif (${PREFIX}_RELEASE)
+    set(${PREFIX} ${${PREFIX}_RELEASE})
+  elseif (${PREFIX}_DEBUG)
+    set(${PREFIX} ${${PREFIX}_DEBUG})
+  endif ()
+endmacro()
+
+#===============================================
+# Ensure that the release & debug libraries found are from the same installation.
+#===============================================
+macro(find_tbb_library_verifying_release_debug_locations PREFIX)
+  find_library(${PREFIX}_RELEASE
+    NAMES ${${PREFIX}_NAMES}
+    HINTS ${TBB_LIB_SEARCH_PATH})
+  if (${PREFIX}_RELEASE)
+    # To avoid finding a mismatched set of release & debug libraries from
+    # different installations if the first found does not have debug libraries
+    # by forcing the search for debug to only occur within the detected release
+    # library directory (if found).  Although this would break detection if the
+    # release & debug libraries were shipped in different directories, this is
+    # not the case in the official TBB releases for any platform.
+    get_filename_component(
+      FOUND_RELEASE_LIB_DIR "${${PREFIX}_RELEASE}" DIRECTORY)
+    find_library(${PREFIX}_DEBUG
+      NAMES ${${PREFIX}_NAMES_DEBUG}
+      HINTS ${FOUND_RELEASE_LIB_DIR}
+      NO_DEFAULT_PATH)
+  else()
+    find_library(${PREFIX}_DEBUG
+      NAMES ${${PREFIX}_NAMES_DEBUG}
+      HINTS ${TBB_LIB_SEARCH_PATH})
+  endif()
+endmacro()
+
+#=============================================================================
+#  Now to actually find TBB
+#
+
+# Get path, convert backslashes as ${ENV_${var}}
+getenv_path(TBB_ROOT)
+
+# initialize search paths
+set(TBB_PREFIX_PATH ${TBB_ROOT} ${ENV_TBB_ROOT})
+set(TBB_INC_SEARCH_PATH "")
+set(TBB_LIB_SEARCH_PATH "")
+
+
+# If user built from sources
+set(TBB_BUILD_PREFIX $ENV{TBB_BUILD_PREFIX})
+if (TBB_BUILD_PREFIX AND ENV_TBB_ROOT)
+  getenv_path(TBB_BUILD_DIR)
+  if (NOT ENV_TBB_BUILD_DIR)
+    set(ENV_TBB_BUILD_DIR ${ENV_TBB_ROOT}/build)
+  endif ()
+
+  # include directory under ${ENV_TBB_ROOT}/include
+  list(APPEND TBB_LIB_SEARCH_PATH
+    ${ENV_TBB_BUILD_DIR}/${TBB_BUILD_PREFIX}_release
+    ${ENV_TBB_BUILD_DIR}/${TBB_BUILD_PREFIX}_debug)
+endif ()
+
+
+# For Windows, let's assume that the user might be using the precompiled
+# TBB packages from the main website. These use a rather awkward directory
+# structure (at least for automatically finding the right files) depending
+# on platform and compiler, but we'll do our best to accommodate it.
+# Not adding the same effort for the precompiled linux builds, though. Those
+# have different versions for CC compiler versions and linux kernels which
+# will never adequately match the user's setup, so there is no feasible way
+# to detect the "best" version to use. The user will have to manually
+# select the right files. (Chances are the distributions are shipping their
+# custom version of tbb, anyway, so the problem is probably nonexistent.)
+if (WIN32 AND MSVC)
+  set(COMPILER_PREFIX "vc7.1")
+  if (MSVC_VERSION EQUAL 1400)
+    set(COMPILER_PREFIX "vc8")
+  elseif(MSVC_VERSION EQUAL 1500)
+    set(COMPILER_PREFIX "vc9")
+  elseif(MSVC_VERSION EQUAL 1600)
+    set(COMPILER_PREFIX "vc10")
+  elseif(MSVC_VERSION EQUAL 1700)
+    set(COMPILER_PREFIX "vc11")
+  elseif(MSVC_VERSION EQUAL 1800)
+    set(COMPILER_PREFIX "vc12")
+  elseif(MSVC_VERSION GREATER_EQUAL 1900)
+    set(COMPILER_PREFIX "vc14")
+  endif ()
+
+  # for each prefix path, add ia32/64\${COMPILER_PREFIX}\lib to the lib search path
+  foreach (dir IN LISTS TBB_PREFIX_PATH)
+    if (CMAKE_CL_64)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia64/${COMPILER_PREFIX}/lib)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia64/${COMPILER_PREFIX})
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${COMPILER_PREFIX}/lib)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${COMPILER_PREFIX})
+    else ()
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${COMPILER_PREFIX}/lib)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${COMPILER_PREFIX})
+    endif ()
+  endforeach ()
+endif ()
+
+# For OS X binary distribution, choose libc++ based libraries for Mavericks (10.9)
+# and above and AppleClang
+if (CMAKE_SYSTEM_NAME STREQUAL "Darwin" AND
+    NOT CMAKE_SYSTEM_VERSION VERSION_LESS 13.0)
+  set (USE_LIBCXX OFF)
+  cmake_policy(GET CMP0025 POLICY_VAR)
+
+  if (POLICY_VAR STREQUAL "NEW")
+    if (CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+      set (USE_LIBCXX ON)
+    endif ()
+  else ()
+    if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+      set (USE_LIBCXX ON)
+    endif ()
+  endif ()
+
+  if (USE_LIBCXX)
+    foreach (dir IN LISTS TBB_PREFIX_PATH)
+      list (APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/libc++ ${dir}/libc++/lib)
+    endforeach ()
+  endif ()
+endif ()
+
+# check compiler ABI
+if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  set(COMPILER_PREFIX)
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
+    list(APPEND COMPILER_PREFIX "gcc4.8")
+  endif()
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.7)
+    list(APPEND COMPILER_PREFIX "gcc4.7")
+  endif()
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.4)
+    list(APPEND COMPILER_PREFIX "gcc4.4")
+  endif()
+  list(APPEND COMPILER_PREFIX "gcc4.1")
+elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  set(COMPILER_PREFIX)
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.0) # Complete guess
+    list(APPEND COMPILER_PREFIX "gcc4.8")
+  endif()
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.6)
+    list(APPEND COMPILER_PREFIX "gcc4.7")
+  endif()
+  list(APPEND COMPILER_PREFIX "gcc4.4")
+else() # Assume compatibility with 4.4 for other compilers
+  list(APPEND COMPILER_PREFIX "gcc4.4")
+endif ()
+
+# if platform architecture is explicitly specified
+set(TBB_ARCH_PLATFORM $ENV{TBB_ARCH_PLATFORM})
+if (TBB_ARCH_PLATFORM)
+  foreach (dir IN LISTS TBB_PREFIX_PATH)
+    list(APPEND TBB_LIB_SEARCH_PATH ${dir}/${TBB_ARCH_PLATFORM}/lib)
+    list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/${TBB_ARCH_PLATFORM})
+  endforeach ()
+endif ()
+
+foreach (dir IN LISTS TBB_PREFIX_PATH)
+  foreach (prefix IN LISTS COMPILER_PREFIX)
+    if (CMAKE_SIZEOF_VOID_P EQUAL 8)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${prefix})
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/lib)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${prefix}/lib)
+    else ()
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${prefix})
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/lib)
+      list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${prefix}/lib)
+    endif ()
+  endforeach()
+endforeach ()
+
+# add general search paths
+foreach (dir IN LISTS TBB_PREFIX_PATH)
+  list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib ${dir}/Lib ${dir}/lib/tbb
+    ${dir}/Libs)
+  list(APPEND TBB_INC_SEARCH_PATH ${dir}/include ${dir}/Include
+    ${dir}/include/tbb)
+endforeach ()
+
+set(TBB_LIBRARY_NAMES tbb)
+get_debug_names(TBB_LIBRARY_NAMES)
+
+find_path(TBB_INCLUDE_DIR
+          NAMES tbb/tbb.h
+          HINTS ${TBB_INC_SEARCH_PATH})
+find_tbb_library_verifying_release_debug_locations(TBB_LIBRARY)
+make_library_set(TBB_LIBRARY)
+
+findpkg_finish(TBB tbb)
+
+#if we haven't found TBB no point on going any further
+if (NOT TBB_FOUND)
+  return()
+endif ()
+
+#=============================================================================
+# Look for TBB's malloc package
+set(TBB_MALLOC_LIBRARY_NAMES tbbmalloc)
+get_debug_names(TBB_MALLOC_LIBRARY_NAMES)
+
+find_path(TBB_MALLOC_INCLUDE_DIR
+          NAMES tbb/tbb.h
+          HINTS ${TBB_INC_SEARCH_PATH})
+find_tbb_library_verifying_release_debug_locations(TBB_MALLOC_LIBRARY)
+make_library_set(TBB_MALLOC_LIBRARY)
+
+findpkg_finish(TBB_MALLOC tbbmalloc)
+
+#=============================================================================
+# Look for TBB's malloc proxy package
+set(TBB_MALLOC_PROXY_LIBRARY_NAMES tbbmalloc_proxy)
+get_debug_names(TBB_MALLOC_PROXY_LIBRARY_NAMES)
+
+find_path(TBB_MALLOC_PROXY_INCLUDE_DIR
+          NAMES tbb/tbbmalloc_proxy.h
+          HINTS ${TBB_INC_SEARCH_PATH})
+find_tbb_library_verifying_release_debug_locations(TBB_MALLOC_PROXY_LIBRARY)
+make_library_set(TBB_MALLOC_PROXY_LIBRARY)
+
+findpkg_finish(TBB_MALLOC_PROXY tbbmalloc_proxy)
+
+
+#=============================================================================
+#parse all the version numbers from tbb
+if(NOT TBB_VERSION)
+  set(TBB_VERSION_FILE_PRIOR_TO_TBB_2021_1
+    "${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h")
+  set(TBB_VERSION_FILE_AFTER_TBB_2021_1
+    "${TBB_INCLUDE_DIR}/oneapi/tbb/version.h")
+
+  if (EXISTS "${TBB_VERSION_FILE_PRIOR_TO_TBB_2021_1}")
+    set(TBB_VERSION_FILE "${TBB_VERSION_FILE_PRIOR_TO_TBB_2021_1}")
+  elseif (EXISTS "${TBB_VERSION_FILE_AFTER_TBB_2021_1}")
+    set(TBB_VERSION_FILE "${TBB_VERSION_FILE_AFTER_TBB_2021_1}")
+  else()
+    message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIR} "
+      "missing version header.")
+  endif()
+
+ #only read the start of the file
+ file(STRINGS
+      "${TBB_VERSION_FILE}"
+      TBB_VERSION_CONTENTS
+      REGEX "VERSION")
+
+  string(REGEX REPLACE
+    ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
+    TBB_VERSION_MAJOR "${TBB_VERSION_CONTENTS}")
+
+  string(REGEX REPLACE
+    ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
+    TBB_VERSION_MINOR "${TBB_VERSION_CONTENTS}")
+
+  string(REGEX REPLACE
+        ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
+        TBB_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}")
+
+  string(REGEX REPLACE
+        ".*#define TBB_COMPATIBLE_INTERFACE_VERSION ([0-9]+).*" "\\1"
+        TBB_COMPATIBLE_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}")
+
+endif()

data/aprilgrid_5x4_uzh.json

@@ -0,0 +1,6 @@
+{
+  "tagCols": 5,
+  "tagRows": 4,
+  "tagSize": 0.075,
+  "tagSpacing": 0.2
+}

data/aprilgrid_6x6.json

@@ -0,0 +1,6 @@
+{
+  "tagCols": 6,
+  "tagRows": 6,
+  "tagSize": 0.088,
+  "tagSpacing": 0.3
+}

data/euroc_config.json

@@ -0,0 +1,56 @@
+{
+    "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.optical_flow_skip_frames": 1,
+        "config.vio_linearization_type": "ABS_QR",
+        "config.vio_sqrt_marg": true,
+        "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_debug": false,
+        "config.vio_extended_logging": false,
+        "config.vio_obs_std_dev": 0.5,
+        "config.vio_obs_huber_thresh": 1.0,
+        "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": true,
+        "config.vio_lm_lambda_initial": 1e-4,
+        "config.vio_lm_lambda_min": 1e-6,
+        "config.vio_lm_lambda_max": 1e2,
+        "config.vio_lm_landmark_damping_variant": 1,
+        "config.vio_lm_pose_damping_variant": 1,
+        "config.vio_scale_jacobian": false,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
+        "config.vio_marg_lost_landmarks": true,
+        "config.vio_kf_marg_feature_ratio": 0.1,
+        "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.04,
+        "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,
+        "config.mapper_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_factors": true,
+        "config.mapper_use_lm": true,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
+    }
+}

data/euroc_config_no_factors.json

@@ -0,0 +1,56 @@
+{
+    "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.optical_flow_skip_frames": 1,
+        "config.vio_linearization_type": "ABS_QR",
+        "config.vio_sqrt_marg": true,
+        "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_debug": false,
+        "config.vio_extended_logging": false,
+        "config.vio_obs_std_dev": 0.5,
+        "config.vio_obs_huber_thresh": 1.0,
+        "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_initial": 1e-8,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
+        "config.vio_lm_landmark_damping_variant": 1,
+        "config.vio_lm_pose_damping_variant": 1,
+        "config.vio_scale_jacobian": false,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
+        "config.vio_marg_lost_landmarks": true,
+        "config.vio_kf_marg_feature_ratio": 0.1,
+        "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.04,
+        "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,
+        "config.mapper_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_factors": false,
+        "config.mapper_use_lm": true,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
+    }
+}

data/euroc_config_no_weights.json

@@ -0,0 +1,56 @@
+{
+    "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.optical_flow_skip_frames": 1,
+        "config.vio_linearization_type": "ABS_QR",
+        "config.vio_sqrt_marg": true,
+        "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_debug": false,
+        "config.vio_extended_logging": false,
+        "config.vio_obs_std_dev": 0.5,
+        "config.vio_obs_huber_thresh": 1.0,
+        "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_initial": 1e-8,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
+        "config.vio_lm_landmark_damping_variant": 1,
+        "config.vio_lm_pose_damping_variant": 1,
+        "config.vio_scale_jacobian": false,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
+        "config.vio_marg_lost_landmarks": true,
+        "config.vio_kf_marg_feature_ratio": 0.1,
+        "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.04,
+        "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,
+        "config.mapper_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": true,
+        "config.mapper_use_factors": true,
+        "config.mapper_use_lm": true,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
+    }
+}

data/euroc_config_vo.json

@@ -0,0 +1,56 @@
+{
+    "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.optical_flow_skip_frames": 1,
+        "config.vio_linearization_type": "ABS_QR",
+        "config.vio_sqrt_marg": true,
+        "config.vio_max_states": 3,
+        "config.vio_max_kfs": 7,
+        "config.vio_min_frames_after_kf": 1,
+        "config.vio_new_kf_keypoints_thresh": 0.8,
+        "config.vio_debug": false,
+        "config.vio_extended_logging": false,
+        "config.vio_obs_std_dev": 0.5,
+        "config.vio_obs_huber_thresh": 1.0,
+        "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": true,
+        "config.vio_lm_lambda_initial": 1e-4,
+        "config.vio_lm_lambda_min": 1e-5,
+        "config.vio_lm_lambda_max": 1e2,
+        "config.vio_lm_landmark_damping_variant": 1,
+        "config.vio_lm_pose_damping_variant": 1,
+        "config.vio_scale_jacobian": false,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
+        "config.vio_marg_lost_landmarks": true,
+        "config.vio_kf_marg_feature_ratio": 0.2,
+        "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.04,
+        "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,
+        "config.mapper_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_factors": true,
+        "config.mapper_use_lm": true,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
+    }
+}

data/euroc_ds_calib.json

@@ -0,0 +1,229 @@
+{
+    "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.003025405479279035,
+          0.1200005286487319,
+          0.06708820471592454,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0
+      ],
+      "calib_gyro_bias": [
+          -0.002186848441668376,
+          0.020427823167917037,
+          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,
+            0.016,
+            0.016
+        ],
+        "gyro_noise_std": [
+            0.000282,
+            0.000282,
+            0.000282
+        ],
+        "accel_bias_std": [
+            0.001,
+            0.001,
+            0.001
+        ],
+        "gyro_bias_std": [
+            0.0001,
+            0.0001,
+            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
+    }
+}

data/euroc_eucm_calib.json

@@ -0,0 +1,213 @@
+{
+    "value0": {
+        "T_imu_cam": [
+            {
+                "px": -0.016774788924641534,
+                "py": -0.068938940687127,
+                "pz": 0.005139123188382424,
+                "qx": -0.007239825785317818,
+                "qy": 0.007541278561558601,
+                "qz": 0.7017845426564943,
+                "qw": 0.7123125505904486
+            },
+            {
+                "px": -0.01507436282032619,
+                "py": 0.0412627204046637,
+                "pz": 0.00316287258752953,
+                "qx": -0.0023360576185881625,
+                "qy": 0.013000769689092388,
+                "qz": 0.7024677108343111,
+                "qw": 0.7115930283929829
+            }
+        ],
+        "intrinsics": [
+            {
+                "camera_type": "eucm",
+                "intrinsics": {
+                    "fx": 460.76484651566468,
+                    "fy": 459.4051018049483,
+                    "cx": 365.8937161309615,
+                    "cy": 249.33499869752445,
+                    "alpha": 0.5903365915227143,
+                    "beta": 1.127468196965374
+                }
+            },
+            {
+                "camera_type": "eucm",
+                "intrinsics": {
+                    "fx": 459.55216904505178,
+                    "fy": 458.17181312352059,
+                    "cx": 379.4066773637502,
+                    "cy": 255.98301446219285,
+                    "alpha": 0.6049889282227827,
+                    "beta": 1.0907289821146678
+                }
+            }
+        ],
+        "resolution": [
+            [
+                752,
+                480
+            ],
+            [
+                752,
+                480
+            ]
+        ],
+        "vignette": [
+            {
+                "value0": 0,
+                "value1": 50000000000,
+                "value2": [
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         0.977014799737916
+                    ],
+                    [
+                         0.963099657065011
+                    ],
+                    [
+                         0.922192868484681
+                    ],
+                    [
+                         0.854947649264748
+                    ],
+                    [
+                         0.775979681731632
+                    ],
+                    [
+                         0.722888431826212
+                    ],
+                    [
+                         0.0686113338332548
+                    ],
+                    [
+                         0.0686113338332548
+                    ],
+                    [
+                         0.0686113338332548
+                    ],
+                    [
+                         0.0686113338332548
+                    ]
+                ]
+            },
+            {
+                "value0": 0,
+                "value1": 50000000000,
+                "value2": [
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         1.0
+                    ],
+                    [
+                         0.977014799737916
+                    ],
+                    [
+                         0.963099657065011
+                    ],
+                    [
+                         0.922192868484681
+                    ],
+                    [
+                         0.854947649264748
+                    ],
+                    [
+                         0.775979681731632
+                    ],
+                    [
+                         0.722888431826212
+                    ],
+                    [
+                         0.0686113338332548
+                    ],
+                    [
+                         0.0686113338332548
+                    ],
+                    [
+                         0.0686113338332548
+                    ],
+                    [
+                         0.0686113338332548
+                    ]
+                ]
+            }
+        ],
+        "calib_accel_bias": [
+            -0.003025405479279035,
+            0.1200005286487319,
+            0.06708820471592454,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "calib_gyro_bias": [
+            -0.002186848441668376,
+            0.020427823167917037,
+            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, 0.016, 0.016],
+        "gyro_noise_std": [0.000282, 0.000282, 0.000282],
+        "accel_bias_std": [0.001, 0.001, 0.001],
+        "gyro_bias_std": [0.0001, 0.0001, 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
+    }
+}

data/iccv21/basalt_batch_config.toml

@@ -0,0 +1,424 @@
+
+##############################################################################
+## Base config that ends up in basalt_config.json
+## These values are overwritten by the named config elements below.
+##############################################################################
+
+# config read by Basalt
+[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.optical_flow_skip_frames" = 1
+
+"config.vio_linearization_type" = "ABS_QR"
+"config.vio_sqrt_marg" = true
+"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_debug" = false
+"config.vio_extended_logging" = false
+"config.vio_obs_std_dev" = 0.5
+"config.vio_obs_huber_thresh" = 1.0
+"config.vio_min_triangulation_dist" = 0.05
+"config.vio_outlier_threshold" = 3.0
+"config.vio_filter_iteration" = 4
+"config.vio_max_iterations" = 7
+"config.vio_enforce_realtime" = false
+"config.vio_use_lm" = true
+"config.vio_lm_lambda_initial" = 1e-4
+"config.vio_lm_lambda_min" = 1e-7
+"config.vio_lm_lambda_max" = 1e2
+"config.vio_lm_landmark_damping_variant" = 1
+"config.vio_lm_pose_damping_variant" = 1
+"config.vio_scale_jacobian" = false
+"config.vio_init_pose_weight" = 1e8
+"config.vio_init_ba_weight" = 1e1
+"config.vio_init_bg_weight" = 1e2
+"config.vio_marg_lost_landmarks" = true
+"config.vio_kf_marg_feature_ratio" = 0.1
+
+"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.04
+"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
+"config.mapper_bow_num_bits" = 16
+"config.mapper_min_triangulation_dist" = 0.07
+"config.mapper_no_factor_weights" = false
+"config.mapper_use_factors" = true
+"config.mapper_use_lm" = true
+"config.mapper_lm_lambda_min" = 1e-32
+"config.mapper_lm_lambda_max" = 1e3
+
+# which executable to run
+[batch_run]
+executable = "basalt_vio"
+
+# environment variables set for the executable
+[batch_run.env]
+OPENBLAS_NUM_THREADS = 1
+OMP_NUM_THREADS = 1
+
+# command line arguments to the executable ("--" is prepended)
+[batch_run.args]
+"config-path" = "basalt_config.json"
+"show-gui" = "0"
+"step-by-step" = "false"
+#"max-frames" = "1000"
+"use-imu" = "false"
+"use-double" = "false"
+"num-threads" = "0"
+
+
+##############################################################################
+## Named config elements to overwrite the base config: batch run script options
+##############################################################################
+
+[_batch.config.vio]
+batch_run.args."use-imu" = "true"
+
+[_batch.config.vo]
+batch_run.args."use-imu" = "false"
+
+
+
+[_batch.config.double]
+batch_run.args."use-double" = "true"
+
+[_batch.config.float]
+batch_run.args."use-double" = "false"
+
+
+
+[_batch.config.threads0]
+batch_run.args."num-threads" = "0"
+
+[_batch.config.threads1]
+batch_run.args."num-threads" = "1"
+
+
+
+[_batch.config.savetum]
+batch_run.args."save-trajectory" = "tum"
+
+# saving gt produces quite large output, but is needed for trajectory plots
+[_batch.config.savetumgt]
+batch_run.args."save-trajectory" = "tum"
+batch_run.args."save-groundtruth" = "1"
+
+
+
+# run each experiment twice to make sure file system caches are hot
+[_batch.config.runtwice]
+batch_run.num_runs = 2
+
+
+
+##############################################################################
+## Named config elements to overwrite the base config: datasets
+##############################################################################
+
+# NOTE: Replace the absolute paths in "dataset-path" entries with the
+# path to your download of EuRoC, TUMVI, Kitti. The "cam-calib" paths
+# are relative and point to locations in the basalt repository. This
+# assumes a folder setup like:
+#
+# parent-folder/
+# ├─ basalt/
+# │  ├─ data/
+# │  │  ├─ euroc_ds_calib.json
+# │  │  ├─ ...
+# │  ├─ ...
+# ├─ experiments/
+# │  ├─ iccv_tutorial/
+# │  │  ├─ basalt_batch_config.toml
+# │  │  ├─ ...
+#
+# The calibration for Kitti is expected in basalt format (you can use the
+# basalt_convert_kitti_calib.py script)
+
+[_batch.config.eurocMH01]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/MH_01_easy"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocMH02]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/MH_02_easy"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocMH03]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/MH_03_medium"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocMH04]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/MH_04_difficult"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocMH05]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/MH_05_difficult"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocV101]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/V1_01_easy"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocV102]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/V1_02_medium"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocV103]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/V1_03_difficult"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocV201]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/V2_01_easy"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocV202]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/V2_02_medium"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.eurocV203]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/euroc/V2_03_difficult"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/euroc_ds_calib.json"
+batch_run.args."dataset-type" = "euroc"
+
+
+
+[_batch.config.tumvi-corridor1]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-corridor1_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-corridor2]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-corridor2_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-corridor3]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-corridor3_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-corridor4]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-corridor4_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-corridor5]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-corridor5_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-magistrale1]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-magistrale1_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-magistrale2]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-magistrale2_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-magistrale3]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-magistrale3_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-magistrale4]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-magistrale4_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-magistrale5]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-magistrale5_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-magistrale6]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-magistrale6_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-room1]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-room1_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-room2]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-room2_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-room3]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-room3_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-room4]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-room4_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-room5]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-room5_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-room6]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-room6_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-slides1]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-slides1_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-slides2]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-slides2_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+[_batch.config.tumvi-slides3]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/tumvi/512_16/dataset-slides3_512_16"
+batch_run.args."cam-calib" = "../../../../../../basalt/data/tumvi_512_eucm_calib.json"
+batch_run.args."dataset-type" = "euroc"
+
+
+
+[_batch.config.kitti00]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/00"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/00/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti01]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/01"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/01/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti02]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/02"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/02/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti03]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/03"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/03/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti04]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/04"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/04/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti05]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/05"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/05/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti06]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/06"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/06/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti07]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/07"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/07/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti08]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/08"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/08/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti09]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/09"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/09/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+[_batch.config.kitti10]
+batch_run.args."dataset-path" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/10"
+batch_run.args."cam-calib" = "/storage/local/ssd/demmeln/tsm-exclude/kitti_odom_grey/sequences/10/basalt_calib.json"
+batch_run.args."dataset-type" = "kitti"
+
+
+
+##############################################################################
+## Named config elements to overwrite the base config: basalt config
+##############################################################################
+
+[_batch.config.debug]
+value0."config.vio_debug" = true
+value0."config.vio_extended_logging" = true
+
+[_batch.config.nodebug]
+value0."config.vio_debug" = false
+value0."config.vio_extended_logging" = false
+
+
+
+[_batch.config.extlog]
+value0."config.vio_extended_logging" = true
+
+[_batch.config.noextlog]
+value0."config.vio_extended_logging" = false
+
+
+
+[_batch.config.eurocvo]
+value0."config.optical_flow_detection_grid_size" = 40
+value0."config.vio_min_frames_after_kf" = 1
+value0."config.vio_new_kf_keypoints_thresh" = 0.8
+value0."config.vio_kf_marg_feature_ratio" = 0.2
+
+[_batch.config.tumvivio]
+value0."config.optical_flow_detection_grid_size" =  40
+
+[_batch.config.kittivo]
+value0."config.optical_flow_detection_grid_size" = 30
+value0."config.optical_flow_epipolar_error" = 0.001
+value0."config.optical_flow_levels" = 4
+value0."config.vio_min_frames_after_kf" = 1
+
+
+
+[_batch.config.sq-sc-sc]
+value0."config.vio_sqrt_marg" = false
+value0."config.vio_linearization_type" = "ABS_SC"
+
+[_batch.config.sqrt-sc-sc]
+value0."config.vio_sqrt_marg" = true
+value0."config.vio_linearization_type" = "ABS_SC"
+
+[_batch.config.sqrt-nsllt-qr]
+value0."config.vio_sqrt_marg" = true
+value0."config.vio_linearization_type" = "ABS_QR"
+
+
+
+##############################################################################
+## Named alternatives, which generate a different experiment for each entry ("or" relation)
+##############################################################################
+
+[_batch.alternatives]
+all_euroc = ["eurocMH01", "eurocMH02", "eurocMH03", "eurocMH04", "eurocMH05", "eurocV101", "eurocV102", "eurocV103", "eurocV201", "eurocV202"]
+some_euroc = ["eurocMH01", "eurocMH02", "eurocV101", "eurocV103", "eurocV201", "eurocV202"]
+
+some_tumvi = ["tumvi-corridor1", "tumvi-magistrale1", "tumvi-room1", "tumvi-slides1"]
+more_tumvi = ["tumvi-corridor1", "tumvi-corridor2", "tumvi-magistrale1", "tumvi-magistrale2", "tumvi-room1", "tumvi-room2", "tumvi-slides1", "tumvi-slides2"]
+some_more_tumvi = ["tumvi-corridor1", "tumvi-corridor2", "tumvi-corridor3", "tumvi-corridor4", "tumvi-corridor5", "tumvi-magistrale1", "tumvi-magistrale2", "tumvi-magistrale3", "tumvi-magistrale4", "tumvi-magistrale5", "tumvi-magistrale6", "tumvi-room1", "tumvi-room2", "tumvi-room3", "tumvi-room4", "tumvi-room5", "tumvi-room6", "tumvi-slides1", "tumvi-slides2", "tumvi-slides3"]
+
+all_kitti = ["kitti00", "kitti01", "kitti02", "kitti03", "kitti04", "kitti05", "kitti06", "kitti07", "kitti08", "kitti09", "kitti10"]
+
+all_imu = ["vio", "vo"]
+all_double = ["double", "float"]
+
+all_meth = ["sq-sc-sc", "sqrt-sc-sc", "sqrt-nsllt-qr"]
+
+
+
+##############################################################################
+## Config combinations, which composing named configs and alternatives ("and" relation)
+##############################################################################
+
+[_batch.combinations]
+vio_euroc = ["vio",             "savetumgt", "extlog", "runtwice", "all_meth", "all_double", "all_euroc"]
+vio_tumvi = ["vio", "tumvivio", "savetumgt", "extlog", "runtwice", "all_meth", "all_double", "more_tumvi"]
+vo_kitti  = ["vo",  "kittivo",  "savetumgt", "extlog", "runtwice", "all_meth", "all_double", "all_kitti"]
+
+#vio_euroc = ["vio",             "runtwice", "all_meth", "all_double", "all_euroc"]
+#vio_tumvi = ["vio", "tumvivio", "runtwice", "all_meth", "all_double", "more_tumvi"]
+#vo_kitti  = ["vo",  "kittivo",  "runtwice", "all_meth", "all_double", "all_kitti"]
+
+
+
+##############################################################################
+## Revision is the primary subfolder name where generated configs are placed
+##############################################################################
+
+[_batch]
+revision = "01_iccv_all"
+#revision = "02_iccv_runtime"

data/iccv21/experiments-iccv.toml

@@ -0,0 +1,593 @@
+[options]
+base_path = "$config_dir"
+output_path = "./tables/experiments-iccv"
+cache_dir = "./tables/cache"
+show_values_failed_runs = false
+screenread = true
+#overwrite_cache = true
+
+
+###################
+## abbreviate some sequence names for more compact tables / legends
+[seq_displayname_mapping]
+"tumvi-corridor1" = "tumvi-corr1"
+"tumvi-corridor2" = "tumvi-corr2"
+"tumvi-magistrale1" = "tumvi-mag1"
+"tumvi-magistrale2" = "tumvi-mag2"
+
+
+###################
+## define method names in table headers / legends globally
+[[substitutions]]
+DISPLAY_NAME_SQRTVIO64 = "$\\sqrt{VIO}$-64"
+DISPLAY_NAME_SQRTVIO32 = "$\\sqrt{VIO}$-32"
+DISPLAY_NAME_SQRTVO64 = "$\\sqrt{VO}$-64"
+DISPLAY_NAME_SQRTVO32 = "$\\sqrt{VO}$-32"
+DISPLAY_NAME_SQVIO64 = "$VIO$-64"
+DISPLAY_NAME_SQVIO32 = "$VIO$-32"
+DISPLAY_NAME_SQVO64 = "$VO$-64"
+DISPLAY_NAME_SQVO32 = "$VO$-32"
+
+
+###################
+## where to find experimental runs
+[[substitutions]]
+
+EXP_PATTERN_VIO = "01_iccv_all/*-*/vio_*/"
+EXP_PATTERN_VO = "01_iccv_all/*-*/vo_*/"
+
+#EXP_PATTERN_VIO = "02_iccv_runtime/*-*/vio_*/"
+#EXP_PATTERN_VO = "02_iccv_runtime/*-*/vo_*/"
+
+
+###################
+## which kind of plots to show
+[[substitutions]]
+SHOW_TRAJECTORY_PLOTS = true
+SHOW_EIGENVALUE_PLOTS = true
+SHOW_NULLSPACE_PLOTS = true
+
+
+###################
+## define which sequences to show in plots
+[[substitutions]]
+SEQUENCES_EUROC = [
+    "eurocMH01",
+    "eurocMH02",
+    "eurocMH03",
+    "eurocMH04",
+    "eurocMH05",
+    "eurocV101",
+    "eurocV102",
+    "eurocV103",
+    "eurocV201",
+    "eurocV202",
+]
+SEQUENCES_TUMVI = [
+    "tumvi-corridor1",
+    "tumvi-corridor2",
+    "tumvi-magistrale1",
+    "tumvi-magistrale2",
+    "tumvi-room1",
+    "tumvi-room2",
+    "tumvi-slides1",
+    "tumvi-slides2",
+]
+SEQUENCES_KITTI = [
+    "kitti00",
+    "kitti02",
+    "kitti03",
+    "kitti04",
+    "kitti05",
+    "kitti06",
+    "kitti07",
+    "kitti08",
+    "kitti09",
+    "kitti10",
+]
+
+
+###################
+## VIO experiments
+[[experiments]]
+name = "vio_sqrt-nsllt-qr-64"
+display_name = "${DISPLAY_NAME_SQRTVIO64}${}^{\\textrm{NS}}_{\\textrm{QR}}$"
+pattern = "${EXP_PATTERN_VIO}/*_sqrt-nsllt-qr_double_*"
+[[experiments]]
+name = "vio_sqrt-nsllt-qr-32"
+display_name = "${DISPLAY_NAME_SQRTVIO32}${}^{\\textrm{NS}}_{\\textrm{QR}}$"
+pattern = "${EXP_PATTERN_VIO}/*_sqrt-nsllt-qr_float_*"
+[[experiments]]
+name = "vio_sqrt-sc-sc-64"
+display_name = "${DISPLAY_NAME_SQRTVIO64}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VIO}/*_sqrt-sc-sc_double_*"
+[[experiments]]
+name = "vio_sqrt-sc-sc-32"
+display_name = "${DISPLAY_NAME_SQRTVIO32}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VIO}/*_sqrt-sc-sc_float_*"
+[[experiments]]
+name = "vio_sq-sc-sc-64"
+display_name = "${DISPLAY_NAME_SQVIO64}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VIO}/*_sq-sc-sc_double_*"
+[[experiments]]
+name = "vio_sq-sc-sc-32"
+display_name = "${DISPLAY_NAME_SQVIO32}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VIO}/*_sq-sc-sc_float_*"
+
+###################
+## VIO experiments -- aliases with shorter display names for plots
+[[experiments]]
+name = "vio_sqrt-64"
+display_name = "${DISPLAY_NAME_SQRTVIO64}"
+extend = "vio_sqrt-nsllt-qr-64"
+[[experiments]]
+name = "vio_sqrt-32"
+display_name = "${DISPLAY_NAME_SQRTVIO32}"
+extend = "vio_sqrt-nsllt-qr-32"
+[[experiments]]
+name = "vio_sq-64"
+display_name = "${DISPLAY_NAME_SQVIO64}"
+extend = "vio_sq-sc-sc-64"
+[[experiments]]
+name = "vio_sq-32"
+display_name = "${DISPLAY_NAME_SQVIO32}"
+extend = "vio_sq-sc-sc-32"
+
+
+
+###################
+## VO experiments
+[[experiments]]
+name = "vo_sqrt-nsllt-qr-64"
+display_name = "${DISPLAY_NAME_SQRTVO64}${}^{\\textrm{NS}}_{\\textrm{QR}}$"
+pattern = "${EXP_PATTERN_VO}/*_sqrt-nsllt-qr_double_*"
+[[experiments]]
+name = "vo_sqrt-nsllt-qr-32"
+display_name = "${DISPLAY_NAME_SQRTVO32}${}^{\\textrm{NS}}_{\\textrm{QR}}$"
+pattern = "${EXP_PATTERN_VO}/*_sqrt-nsllt-qr_float_*"
+[[experiments]]
+name = "vo_sqrt-sc-sc-64"
+display_name = "${DISPLAY_NAME_SQRTVO64}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VO}/*_sqrt-sc-sc_double_*"
+[[experiments]]
+name = "vo_sqrt-sc-sc-32"
+display_name = "${DISPLAY_NAME_SQRTVO32}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VO}/*_sqrt-sc-sc_float_*"
+[[experiments]]
+name = "vo_sq-sc-sc-64"
+display_name = "${DISPLAY_NAME_SQVO64}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VO}/*_sq-sc-sc_double_*"
+[[experiments]]
+name = "vo_sq-sc-sc-32"
+display_name = "${DISPLAY_NAME_SQVO32}${}^{\\textrm{SC}}_{\\textrm{SC}}$"
+pattern = "${EXP_PATTERN_VO}/*_sq-sc-sc_float_*"
+
+###################
+## VIO experiments -- aliases with shorter display names for plots
+[[experiments]]
+name = "vo_sqrt-64"
+display_name = "${DISPLAY_NAME_SQRTVO64}"
+extend = "vo_sqrt-nsllt-qr-64"
+[[experiments]]
+name = "vo_sqrt-32"
+display_name = "${DISPLAY_NAME_SQRTVO32}"
+extend = "vo_sqrt-nsllt-qr-32"
+[[experiments]]
+name = "vo_sq-64"
+display_name = "${DISPLAY_NAME_SQVO64}"
+extend = "vo_sq-sc-sc-64"
+[[experiments]]
+name = "vo_sq-32"
+display_name = "${DISPLAY_NAME_SQVO32}"
+extend = "vo_sq-sc-sc-32"
+
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Accuracy VIO & VO"
+show = true
+
+###################
+## accuracy results table for all VIO sequences
+[[results]]
+class = "results_table"
+name = "vio ate"
+show = true
+export_latex = "vio_ate"
+metrics_legend = false
+rotate_header = true
+escape_latex_header = false
+vertical_bars = false
+color_failed = ""
+filter_regex = "euroc|tumvi.*1|tumvi.*2"
+experiments = [
+    "vio_sqrt-nsllt-qr-64",
+    "vio_sqrt-nsllt-qr-32",
+    "vio_sq-sc-sc-64",
+    "vio_sq-sc-sc-32",
+]
+metrics = [
+    "ate_rmse",
+]
+
+###################
+## accuracy results table for all VO sequences
+[[results]]
+class = "results_table"
+name = "vo ate"
+show = true
+export_latex = "vo_ate"
+metrics_legend = false
+rotate_header = true
+escape_latex_header = false
+vertical_bars = false
+color_failed = ""
+#filter_regex = "kitti00|kitti0[2-9]|kitti10"
+#filter_regex = "kitti"
+override_as_failed = ["kitti01"]
+experiments = [
+    "vo_sqrt-nsllt-qr-64",
+    "vo_sqrt-nsllt-qr-32",
+    "vo_sq-sc-sc-64",
+    "vo_sq-sc-sc-32",
+]
+metrics = [
+    {name = "ate_rmse", failed_threshold = 100, decimals = 2},
+]
+
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Runtime VIO & VO"
+show = true
+
+###################
+## runtime results table for all VIO sequences
+[[results]]
+class = "results_table"
+name = "vio time optimize / marginalize"
+show = true
+export_latex = "vio_runtime"
+metrics_legend = false
+rotate_header = true
+escape_latex_header = false
+vertical_bars = false
+color_failed = ""
+multirow = false
+filter_regex = "euroc|tumvi.*1|tumvi.*2"
+experiments = [
+    "vio_sqrt-nsllt-qr-64",
+    "vio_sqrt-nsllt-qr-32",
+    "vio_sq-sc-sc-64",
+    "vio_sq-sc-sc-32",
+]
+metrics = [
+    {name = "time_opt", decimals = 1},
+    {name = "time_marg", decimals = 1},
+]
+
+###################
+## runtime results table for all VO sequences
+[[results]]
+class = "results_table"
+name = "vo time optimize / marginalize"
+show = true
+export_latex = "vo_runtime"
+metrics_legend = false
+rotate_header = true
+escape_latex_header = false
+vertical_bars = false
+color_failed = ""
+multirow = false
+#filter_regex = "kitti00|kitti0[2-9]|kitti10"
+#filter_regex = "kitti"
+override_as_failed = ["kitti01"]
+experiments = [
+    "vo_sqrt-nsllt-qr-64",
+    "vo_sqrt-nsllt-qr-32",
+    "vo_sq-sc-sc-64",
+    "vo_sq-sc-sc-32",
+]
+metrics = [
+    "time_opt",
+    "time_marg",
+]
+
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Ablation VIO & VO"
+show = true
+
+###################
+## ablation study table for VIO summarized over all EuRoC sequences
+[[results]]
+class = "summarize_sequences_table"
+name = "summary euroc ablation"
+export_latex = "vio_euroc_ablation"
+escape_latex_header = false
+filter_regex = "euroc"
+experiments =  [
+    "vio_sqrt-nsllt-qr-64",
+    "vio_sqrt-nsllt-qr-32",
+    "vio_sqrt-sc-sc-64",
+    "vio_sqrt-sc-sc-32",
+]
+metrics = [
+    {name = "ate_rmse", display_name = "ATE [m]", geometric_mean = false,  decimals = 3},
+    {name = "time_exec_realtimefactor", display_name = "real-time", geometric_mean = true, format_string = "{:.1f}x"},
+    {name = "time_exec", display_name = "t total [s]", geometric_mean = false, decimals = 1},
+    {name = "time_opt", display_name = "t opt [s]", geometric_mean = false, decimals = 1},
+    {name = "time_marg", display_name = "t marg [s]", geometric_mean = false,  decimals = 1},
+#    {name = "time_marg", display_name = "t marg (frac. of meas)", relative_to_metric = "time_measure", geometric_mean = true,  format_string = "{:.2f}"},
+#    {name = "time_opt", display_name = "t opt (frac. of meas)", relative_to_metric = "time_measure", geometric_mean = true, format_string = "{:.2f}"},
+#    {name = "time_marg", display_name = "t marg (frac. of total)", relative_to_metric = "time_exec", geometric_mean = true,  format_string = "{:.2f}"},
+#    {name = "time_opt", display_name = "t opt (frac. of total)", relative_to_metric = "time_exec", geometric_mean = true, format_string = "{:.2f}"},
+    {name = "avg_num_it", display_name = "avg #it", geometric_mean = false, decimals = 1},
+    {name = "avg_num_it_failed", display_name = "avg #it-failed", geometric_mean = false, decimals = 1},
+]
+
+###################
+## ablation study table for VO summarized over all Kitti sequences (w/o kitti01)
+[[results]]
+class = "summarize_sequences_table"
+name = "summary kitti (w/o kitti01) ablation"
+show = true
+export_latex = "vo_kitti_ablation"
+escape_latex_header = false
+filter_regex = "kitti00|kitti0[2-9]|kitti10"
+experiments =  [
+    "vo_sqrt-nsllt-qr-64",
+    "vo_sqrt-nsllt-qr-32",
+    "vo_sqrt-sc-sc-64",
+    "vo_sqrt-sc-sc-32",
+]
+metrics = [
+    {name = "ate_rmse", display_name = "ATE [m]", geometric_mean = false,  decimals = 3},
+    {name = "time_exec_realtimefactor", display_name = "real-time", geometric_mean = true, format_string = "{:.1f}x"},
+    {name = "time_exec", display_name = "t total [s]", geometric_mean = false, decimals = 1},
+    {name = "time_opt", display_name = "t opt [s]", geometric_mean = false, decimals = 1},
+    {name = "time_marg", display_name = "t marg [s]", geometric_mean = false,  decimals = 1},    
+    {name = "avg_num_it", display_name = "avg #it", geometric_mean = false, decimals = 1},
+    {name = "avg_num_it_failed", display_name = "avg #it-failed", geometric_mean = false, decimals = 1},
+]
+
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Trajectories EuRoC"
+show = "<SHOW_TRAJECTORY_PLOTS>"
+
+###################
+## template for trajectory plots for a set of sequences
+[[templates]]
+_name = "TRAJECTORY"
+_arguments = ["SEQUENCE", "EXPERIMENTS", "AXES"]
+class = "plot"
+name = "trajectory"
+type = "trajectory"
+figsize = [4.0,2.5]
+trajectory_axes = "<AXES>"
+sequence = "<SEQUENCE>"
+experiments = "<EXPERIMENTS>"
+
+###################
+## instantiate trajectory plot template for EuRoC
+[[results]]
+  [results._template]
+  _name = "TRAJECTORY"
+  AXES = "xy"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_EUROC>"}
+  EXPERIMENTS = [
+    "vio_sqrt-32",
+    "vio_sq-32",
+  ]
+
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Eigenvalues EuRoC"
+show = "<SHOW_EIGENVALUE_PLOTS>"
+
+###################
+## template for eigenvalue plots for a set of sequences
+[[templates]]
+_name = "EIGENVALUES"
+_arguments = ["SEQUENCE", "EXPERIMENTS"]
+class = "plot"
+type = "eigenvalues"
+name = "vio"
+#legend_loc = "upper left"
+#ylim.top = 1e6
+#ylim.bottom = -1e6
+sequence = "<SEQUENCE>"
+experiments = "<EXPERIMENTS>"
+
+###################
+## instantiate eigenvalue plot template for EuRoC
+[[results]]
+  [results._template]
+  _name = "EIGENVALUES"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_EUROC>"}
+  EXPERIMENTS = [
+    "vio_sqrt-64",
+    "vio_sqrt-32",
+    "vio_sq-64",
+    "vio_sq-32",
+  ]
+
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Nullspace EuRoC"
+show = "<SHOW_NULLSPACE_PLOTS>"
+
+###################
+## template for nullspace plots for a set of sequences
+[[templates]]
+_name = "NULLSPACE"
+_arguments = ["SEQUENCE", "EXPERIMENTS"]
+class = "plot"
+type = "nullspace"
+name = "vio"
+#legend_loc = "lower right"
+sequence = "<SEQUENCE>"
+experiments = "<EXPERIMENTS>"
+
+###################
+## instantiate nullspace plot template for EuRoC
+[[results]]
+  [results._template]
+  _name = "NULLSPACE"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_EUROC>"}
+  EXPERIMENTS = [
+    "vio_sqrt-64",
+    "vio_sqrt-32",
+    "vio_sq-64",
+    "vio_sq-32",
+  ]
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Trajectories TUMVI"
+show = "<SHOW_TRAJECTORY_PLOTS>"
+
+###################
+## instantiate trajectory plot template for TUMVI
+[[results]]
+  [results._template]
+  _name = "TRAJECTORY"
+  AXES = "xy"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_TUMVI>"}
+  EXPERIMENTS = [
+      "vio_sqrt-32",
+      "vio_sq-32",
+  ]
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Eigenvalues TUMVI"
+show = "<SHOW_EIGENVALUE_PLOTS>"
+
+###################
+## instantiate eigenvalue plot template for TUMVI
+[[results]]
+  [results._template]
+  _name = "EIGENVALUES"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_TUMVI>"}
+  EXPERIMENTS = [
+    "vio_sqrt-64",
+    "vio_sqrt-32",
+    "vio_sq-64",
+    "vio_sq-32",
+  ]
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Nullspace TUMVI"
+show = "<SHOW_NULLSPACE_PLOTS>"
+
+###################
+## instantiate nullspace plot template for TUMVI
+[[results]]
+  [results._template]
+  _name = "NULLSPACE"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_TUMVI>"}
+  EXPERIMENTS = [
+    "vio_sqrt-64",
+    "vio_sqrt-32",
+    "vio_sq-64",
+    "vio_sq-32",
+  ]
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Trajectories Kitti"
+show = "<SHOW_TRAJECTORY_PLOTS>"
+
+###################
+## instantiate trajectory plot template for Kitti
+[[results]]
+  [results._template]
+  _name = "TRAJECTORY"
+  AXES = "xz"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_KITTI>"}
+  EXPERIMENTS = [
+      "vo_sqrt-32",
+      "vo_sq-32",
+  ]
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Eigenvalues Kitti"
+show = "<SHOW_EIGENVALUE_PLOTS>"
+
+###################
+## instantiate eigenvalue plot template for Kitti
+[[results]]
+  [results._template]
+  _name = "EIGENVALUES"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_KITTI>"}
+  EXPERIMENTS = [
+    "vo_sqrt-64",
+    "vo_sqrt-32",
+    "vo_sq-64",
+    "vo_sq-32",
+  ]
+
+
+################################################################################
+################################################################################
+[[results]]
+class = "section"
+name = "Nullspace Kitti"
+show = "<SHOW_NULLSPACE_PLOTS>"
+
+###################
+## instantiate nullspace plot template for Kitti
+[[results]]
+  [results._template]
+  _name = "NULLSPACE"
+  SEQUENCE = {_argument = "product", _value = "<SEQUENCES_KITTI>"}
+  EXPERIMENTS = [
+    "vo_sqrt-64",
+    "vo_sqrt-32",
+    "vo_sq-64",
+    "vo_sq-32",
+  ]
+
+
+

data/kitti_config.json

@@ -0,0 +1,56 @@
+{
+    "value0": {
+        "config.optical_flow_type": "frame_to_frame",
+        "config.optical_flow_detection_grid_size": 30,
+        "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.001,
+        "config.optical_flow_levels": 4,
+        "config.optical_flow_skip_frames": 1,
+        "config.vio_linearization_type": "ABS_QR",
+        "config.vio_sqrt_marg": true,
+        "config.vio_max_states": 3,
+        "config.vio_max_kfs": 7,
+        "config.vio_min_frames_after_kf": 1,
+        "config.vio_new_kf_keypoints_thresh": 0.7,
+        "config.vio_debug": false,
+        "config.vio_extended_logging": false,
+        "config.vio_obs_std_dev": 0.5,
+        "config.vio_obs_huber_thresh": 1.0,
+        "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_initial": 1e-4,
+        "config.vio_lm_lambda_min": 1e-6,
+        "config.vio_lm_lambda_max": 1e2,
+        "config.vio_lm_landmark_damping_variant": 1,
+        "config.vio_lm_pose_damping_variant": 1,
+        "config.vio_scale_jacobian": false,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
+        "config.vio_marg_lost_landmarks": true,
+        "config.vio_kf_marg_feature_ratio": 0.1,
+        "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.04,
+        "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,
+        "config.mapper_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_factors": true,
+        "config.mapper_use_lm": true,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
+    }
+}

data/tumvi_512_config.json

@@ -0,0 +1,56 @@
+{
+    "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.optical_flow_skip_frames": 1,
+        "config.vio_linearization_type": "ABS_QR",
+        "config.vio_sqrt_marg": true,
+        "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_debug": false,
+        "config.vio_extended_logging": false,
+        "config.vio_obs_std_dev": 0.5,
+        "config.vio_obs_huber_thresh": 1.0,
+        "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_initial": 1e-7,
+        "config.vio_lm_lambda_min": 1e-6,
+        "config.vio_lm_lambda_max": 1e2,
+        "config.vio_lm_landmark_damping_variant": 1,
+        "config.vio_lm_pose_damping_variant": 1,
+        "config.vio_scale_jacobian": false,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
+        "config.vio_marg_lost_landmarks": true,
+        "config.vio_kf_marg_feature_ratio": 0.1,
+        "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.04,
+        "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,
+        "config.mapper_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_factors": true,
+        "config.mapper_use_lm": false,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
+    }
+}

data/tumvi_512_ds_calib.json

@@ -0,0 +1,381 @@
+{
+    "value0": {
+        "T_imu_cam": [
+            {
+                "px": 0.04548094812071685,
+                "py": -0.07145370002838907,
+                "pz": -0.046315428444919249,
+                "qx": -0.013392900690257393,
+                "qy": -0.6945866755293793,
+                "qz": 0.7192437840259219,
+                "qw": 0.007639340823570553
+            },
+            {
+                "px": -0.05546984222234079,
+                "py": -0.06999334244486549,
+                "pz": -0.049092582974927929,
+                "qx": -0.01340980138125811,
+                "qy": -0.7115668842588793,
+                "qz": 0.7024477338114514,
+                "qw": 0.007741299385907546
+            }
+        ],
+        "intrinsics": [
+            {
+                "camera_type": "ds",
+                "intrinsics": {
+                    "fx": 158.28600034966977,
+                    "fy": 158.2743455478755,
+                    "cx": 254.96116578191653,
+                    "cy": 256.8894394501779,
+                    "xi": -0.17213086034353243,
+                    "alpha": 0.5931177593944744
+                }
+            },
+            {
+                "camera_type": "ds",
+                "intrinsics": {
+                    "fx": 157.91830144176309,
+                    "fy": 157.8901286125632,
+                    "cx": 252.56547609702953,
+                    "cy": 255.02489416194656,
+                    "xi": -0.17114780716007858,
+                    "alpha": 0.5925543396658507
+                }
+            }
+        ],
+        "resolution": [
+            [
+                512,
+                512
+            ],
+            [
+                512,
+                512
+            ]
+        ],
+        "calib_accel_bias": [
+            -0.0011791549110128743,
+            -0.007714616665106584,
+            0.00006733124206236036,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "calib_gyro_bias": [
+            -0.00007403817898158015,
+            0.0000024090695654577384,
+            -0.00001847724206614636,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "imu_update_rate": 200.0,
+        "accel_noise_std": [0.016, 0.016, 0.016],
+        "gyro_noise_std": [0.000282, 0.000282, 0.000282],
+        "accel_bias_std": [0.001, 0.001, 0.001],
+        "gyro_bias_std": [0.0001, 0.0001, 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
+                    ]
+                ]
+            }
+        ]
+    }
+}

data/tumvi_512_eucm_calib.json

@@ -0,0 +1,381 @@
+{
+    "value0": {
+        "T_imu_cam": [
+            {
+                "px": 0.04548094812071685,
+                "py": -0.07145370002838907,
+                "pz": -0.046315428444919249,
+                "qx": -0.013392900690257393,
+                "qy": -0.6945866755293793,
+                "qz": 0.7192437840259219,
+                "qw": 0.007639340823570553
+            },
+            {
+                "px": -0.05546984222234079,
+                "py": -0.06999334244486549,
+                "pz": -0.049092582974927929,
+                "qx": -0.01340980138125811,
+                "qy": -0.7115668842588793,
+                "qz": 0.7024477338114514,
+                "qw": 0.007741299385907546
+            }
+        ],
+        "intrinsics": [
+            {
+                "camera_type": "eucm",
+                "intrinsics": {
+                    "fx": 191.14799836282189,
+                    "fy": 191.13150963902818,
+                    "cx": 254.9585771534443,
+                    "cy": 256.88154645599448,
+                    "alpha": 0.6291060881178562,
+                    "beta": 1.0418067381860868
+                }
+            },
+            {
+                "camera_type": "eucm",
+                "intrinsics": {
+                    "fx": 190.47905769226575,
+                    "fy": 190.44567561523216,
+                    "cx": 252.55882115024333,
+                    "cy": 255.02104780344699,
+                    "alpha": 0.6281040684983363,
+                    "beta": 1.041250259119081
+                }
+            }
+        ],
+        "resolution": [
+            [
+                512,
+                512
+            ],
+            [
+                512,
+                512
+            ]
+        ],
+        "calib_accel_bias": [
+            -0.0011791549110128743,
+            -0.007714616665106584,
+            0.00006733124206236036,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "calib_gyro_bias": [
+            -0.00007403817898158015,
+            0.0000024090695654577384,
+            -0.00001847724206614636,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "imu_update_rate": 200.0,
+        "accel_noise_std": [0.016, 0.016, 0.016],
+        "gyro_noise_std": [0.000282, 0.000282, 0.000282],
+        "accel_bias_std": [0.001, 0.001, 0.001],
+        "gyro_bias_std": [0.0001, 0.0001, 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
+                    ]
+                ]
+            }
+        ]
+    }
+}

doc/BatchEvaluation.md

@@ -0,0 +1,261 @@
+# Batch Evaluation of Square Root Optimization and Marginalization
+
+In this tutorial we detail how you can use the batch evaluation
+scripts to reproduce the results of the ICCV'21 paper Demmel et al.,
+"Square Root Marginalization for Sliding-Window Bundle Adjustment".
+In the paper we discuss how square root estimation techniques can be
+used in Basalt's optimization-based sliding-window odometry to make
+optimization faster and marginalization numerially more stable.  See
+the [project page](https://go.vision.in.tum.de/rootvo) for further
+details.
+
+Basalt's VIO/VO now runs with single-precision floating point numbers
+by default, using the new square root formulation. The conventional
+squared (Hessian-based) formualtion is still available via config
+options. For manual testing, you can pass `--use-double true` or
+`--use-double false` (default) as command line arguments to
+`basalt_vio`, and change `config.vio_sqrt_marg` in the config file,
+which controls if the marginalization prior is stored in Hessian or
+Jacobian form (default: `true`), as well as
+`config.vio_linearization_type` in the config file, which controls
+whether to use Schur complement, or nullspace projection and
+QR-decomposition for optimization and marginalization (`"ABS_SC"` or
+`"ABS_QR"` (default)).
+
+In the following tutorial we systematically compare the different
+formulations in single and double precision to reproduce the results
+from the ICCV'21 paper. You can of course adjust the correspondig
+config files to evaluate other aspects of the system.
+
+## Prerequisites
+
+  1. **Source installation of Basalt:** The batch evaluation scripts
+     by default assume that the `build` folder is directly inside the
+     source checkout `basalt`. See
+     [README.md](../README.md#source-installation-for-ubuntu-1804-and-macos-1014-mojave)
+     for instructions.
+  2. **Downloads of the datasets:** We evaluate EuRoC (all 11
+     sequneces), TUMVI (euroc format in 512x512 resultion; sequences:
+     corridor1-2, magistrale1-2, room1-2, slides1-2), and Kitti
+     Odometry (sequences 00-10).  It's recommended to store the data
+     locally on an SSD to ensure that reading the images is not the
+     bottleneck during evaluation (on a multicore desktop Basalt runs
+     many times faster than real-time). There are instructions for
+     downloading these dataset: [EuRoC](VioMapping.md#euroc-dataset),
+     [TUMVI](VioMapping.md#tum-vi-dataset),
+     [KITTI](Vo.md#kitti-dataset). Calibration for EuRoC and TUMVI is
+     provided in the `data` folder. For KITTI you can use the
+     `basalt_convert_kitti_calib.py` script to convert the provided
+     calibration to a Basalt-compatible format (see
+     [KITTI](Vo.md#kitti-dataset)).
+  3. **Dependencies of evaluation scripts:** You need pip packages
+     `py_ubjson`, `matplotlib`, `numpy`, `munch`, `scipy`, `pylatex`,
+     `toml`. How to install depends on your Python setup (virtualenv,
+     conda, ...). To just install for the local user with pip you can
+     use the command `python3 -m pip install --user -U py_ubjson
+     matplotlib numpy munch scipy pylatex toml`. For generating result
+     tables and plots you additionally need latexmk and a LaTeX
+     distribution (Ubuntu: `sudo apt install texlive-latex-extra
+     latexmk`; macOS with Homebrew: `brew install --cask mactex`).
+  
+## Folder Structure
+
+The batch evaluation scripts and config files assume a certain folder
+structure inside a "parent" folder, since relative paths are used to
+find the compiled executable and calibration files. So **it's
+important to follow the folder structure**. 
+
+```
+parent-folder/
+├─ basalt/
+│  ├─ build/
+│  │  ├─ basalt_vio
+│  │  ├─ ...
+│  ├─ data/
+│  │  ├─ euroc_ds_calib.json
+│  │  ├─ ...
+│  ├─ ...
+├─ experiments/
+│  ├─ iccv_tutorial/
+│  │  ├─ basalt_batch_config.toml
+│  │  ├─ experiments-iccv.toml
+│  │  ├─ 01_iccv_all/
+│  │  │  ├─ ...
+│  │  ├─ 02_iccv_runtime/
+│  │  │  ├─ ...
+```
+
+As a sibling of the `basalt` source checkout we'll have an
+`experiments` folder, and inside, a folder `iccv_tutorial` for this
+tutorial. Into that folder, we copy the provided
+`basalt_batch_config.toml` file that defines the configurations we
+want to evaluate and from which we generate individual config files
+for each VIO / VO run. We also copy the provided
+`experiments-iccv.toml` config file, which defines the results tables
+and plots that we generate from the experiments' logs.
+
+> *Note:* Commands in this tutorial are assumed to be executed from
+> within `parent-folder` unless specified otherwise.
+
+```bash
+mkdir -p experiments/iccv_tutorial
+cp basalt/data/iccv21/basalt_batch_config.toml experiments/iccv_tutorial/
+cp basalt/data/iccv21/experiments-iccv.toml experiments/iccv_tutorial/
+```
+
+## Generate Experimental Configs
+
+First, edit the copied configuration file
+`experiments/iccv_tutorial/basalt_batch_config.toml` and modify all
+`"dataset-path"` lines to point to the locations where you downloaded
+the datasets to.
+
+Now, we can generate per-experiment config files:
+
+```bash
+cd experiments/iccv_tutorial/
+../../basalt/scripts/batch/generate-batch-configs.py .
+```
+
+This will create subfolder `01_iccv_all` containing folders
+`vio_euroc`, `vio_tumvi`, and `vo_kitti`, which in turn contain all
+generated `basalt_config_...json` files, one for each experiment we
+will run.
+
+## Run Experiments
+
+We can now run all experiments for those generate configs. Each config
+/ sequence combination will automatically be run twice and only the
+second run is evaluated, which is meant to ensure that file system
+caches are hot.
+
+Since we also evaluate runtimes, we recommend that you don't use the
+machine running the experiments for anything else and also ensure no
+expensive background tasks are running during the evaluation. On one
+of our desktops with 12 (virtual) cores the total evaluation of all
+sequences takes aroudn 3:30h. Your milage may vary of course depending
+on the hardware.
+
+```bash
+cd experiments/iccv_tutorial/
+time ../../basalt/scripts/batch/run-all-in.sh 01_iccv_all/
+```
+
+Inside `01_iccv_all`, a new folder with the start-time of the
+experimental run is created, e.g., `20211006-143137`, and inside that
+you can again see the same per-dataset subfolders `vio_euroc`,
+`vio_tumvi`, and `vo_kitti`, inside of which there is a folder for
+each config / run. Inside these per-run folders you can find log files
+including the command line output, which you can inspect in case
+something doesn't work.
+
+In a second terminal, you can check the status of evaluation while it
+is running (adjust the argument to match the actual folder name).
+
+```bash
+cd experiments/iccv_tutorial/
+../../basalt/scripts/batch/list-jobs.sh 01_iccv_all/20211006-143137
+```
+
+If you see failed experiments for the square root solver in single
+precision, don't worry, that is expected.
+
+## Generate Results Tables and Plots
+
+After all experimental runs have completed, you can generate a PDF
+file with tabulated results and plots, similar to those in the ICCV'21
+paper.
+
+```bash
+cd experiments/iccv_tutorial/
+../../basalt/scripts/batch/generate-tables.py --config experiments-iccv.toml --open
+```
+
+The results are in the generated `tables/experiments-iccv.pdf` file
+(and with the `--open` argument should automatically open with the
+default PDF reader).
+
+## Better Runtime Evaluation
+
+The experiments above have the extended logging of eigenvalue and
+nullspace information enabled, which does cost a little extra
+runtime. To get a better runtime comparison, you can re-run the
+experiments without this extended logging. The downside is, that you
+can only generate the results tables, but not the plots.
+
+We assume that you have already followed the tutorial above, including
+the initial folder setup. For these modified experiments, we redo all
+three steps (generating config files; running experiments; generating
+results) with slight modifications.
+
+First, edit the `experiments/iccv_tutorial/basalt_batch_config.toml`
+file at the bottom, and uncomment the commented entries in
+`_batch.combinations` as well as the commented `revision`. At the same
+time, comment out the initially uncommented lines. It should look
+something like this after the modifications:
+
+```toml
+[_batch.combinations]
+#vio_euroc = ["vio",             "savetumgt", "extlog", "runtwice", "all_meth", "all_double", "all_euroc"]
+#vio_tumvi = ["vio", "tumvivio", "savetumgt", "extlog", "runtwice", "all_meth", "all_double", "more_tumvi"]
+#vo_kitti  = ["vo",  "kittivo",  "savetumgt", "extlog", "runtwice", "all_meth", "all_double", "all_kitti"]
+
+vio_euroc = ["vio",             "runtwice", "all_meth", "all_double", "all_euroc"]
+vio_tumvi = ["vio", "tumvivio", "runtwice", "all_meth", "all_double", "more_tumvi"]
+vo_kitti  = ["vo",  "kittivo",  "runtwice", "all_meth", "all_double", "all_kitti"]
+```
+
+```toml
+[_batch]
+#revision = "01_iccv_all"
+revision = "02_iccv_runtime"
+```
+
+You can see that we removed the `savetumgt` and `extlog` named config
+elements and that generated config files and results for this second
+run of experiments will be placed in `02_iccv_runtime`.
+
+Now generate config files and start the experimental runs:
+
+```
+cd experiments/iccv_tutorial/
+../../basalt/scripts/batch/generate-batch-configs.py .
+time ../../basalt/scripts/batch/run-all-in.sh 02_iccv_runtime/
+```
+
+Before generating the results PDF you need to now edit the
+`experiments-iccv.toml` file, point it to the new location for
+experimental logs and disable the generation of plots. Check the place
+towards the start of the file where substitutions for
+`EXP_PATTERN_VIO` and `EXP_PATTERN_VO` are defined, as well as
+`SHOW_TRAJECTORY_PLOTS`, `SHOW_EIGENVALUE_PLOTS`, and
+`SHOW_NULLSPACE_PLOTS`. After your modifications, that section should
+look something like:
+
+```toml
+###################
+## where to find experimental runs
+[[substitutions]]
+
+#EXP_PATTERN_VIO = "01_iccv_all/*-*/vio_*/"
+#EXP_PATTERN_VO = "01_iccv_all/*-*/vo_*/"
+
+EXP_PATTERN_VIO = "02_iccv_runtime/*-*/vio_*/"
+EXP_PATTERN_VO = "02_iccv_runtime/*-*/vo_*/"
+
+
+###################
+## which kind of plots to show
+[[substitutions]]
+SHOW_TRAJECTORY_PLOTS = false
+SHOW_EIGENVALUE_PLOTS = false
+SHOW_NULLSPACE_PLOTS = false
+```
+
+Now we can generate the results tables for the new experimental runs
+with the same command as before:
+
+```bash
+cd experiments/iccv_tutorial/
+../../basalt/scripts/batch/generate-tables.py --config experiments-iccv.toml --open
+```

doc/Calibration.md

@@ -0,0 +1,174 @@
+# Calibration
+
+Here, we explain how to use the calibration tools with [TUM-VI](https://vision.in.tum.de/data/datasets/visual-inertial-dataset), [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets), [UZH-FPV](http://rpg.ifi.uzh.ch/uzh-fpv.html) and [Kalibr](https://github.com/ethz-asl/kalibr/wiki/downloads) 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 --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --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.
+* `--aprilgrid` path to the configuration file for the aprilgrid.
+* `--cam-types` camera models for the image streams in the dataset. For more details 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. After pressing a button the system will print the output to the command line:
+* `load_dataset` loads the dataset.
+* `detect_corners` starts corner detection in the background 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. Alternatively, you can use the `opt_until_converge` checkbox that will run the optimization until it converges automatically.
+* `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 (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. Works only when `show_corners` is enabled.
+* `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.
+* `opt_until_converge` runs the optimization until convergence.
+* `stop_thresh` defines the stopping criteria. Optimization will stop when the maximum increment is smaller than this value.
+
+
+### 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 --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --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. Alternatively, you can use the `opt_until_converge` checkbox that will run the optimization until it converges automatically.
+* `init_mocap` initializes the transformation from the Aprilgrid calibration pattern to the Mocap coordinate system.
+* `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 contained in the dataset.
+* `show_accel` shows accelerometer data.
+* `show_gyro` shows gyroscope data.
+* `show_pos` shows spline position for `show_spline` and positions generated from camera pose initialization transformed into IMU coordinate frame for `show_data`.
+* `show_rot_error` shows the rotation error between spline and camera pose initializations transformed into IMU coordinate frame.
+* `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 misalignment 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.
+* `opt_until_convg` runs the optimization until convergence.
+* `stop_thresh` defines the stopping criteria. Optimization will stop when the maximum increment is smaller than this value.
+
+
+**NOTE:** In this case we use a 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 --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --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 --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --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)
+
+
+## UZH dataset
+Download the datasets for camera and camera-IMU calibration:
+```
+mkdir ~/uzh_calib_data
+cd ~/uzh_calib_data
+wget http://rpg.ifi.uzh.ch/datasets/uzh-fpv/calib/indoor_forward_calib_snapdragon_cam.bag
+wget http://rpg.ifi.uzh.ch/datasets/uzh-fpv/calib/indoor_forward_calib_snapdragon_imu.bag
+```
+
+### Camera calibration
+Run the camera calibration:
+```
+basalt_calibrate --dataset-path ~/uzh_calib_data/indoor_forward_calib_snapdragon_cam.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_5x4_uzh.json --result-path ~/uzh_calib_result/ --cam-types ds ds
+```
+![uzh_cam_calib](/doc/img/uzh_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 ~/uzh_calib_data/indoor_forward_calib_snapdragon_imu.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_5x4_uzh.json --result-path ~/uzh_calib_result/ --gyro-noise-std 0.05 --accel-noise-std 0.1 --gyro-bias-std 4e-5 --accel-bias-std 0.002
+```
+![uzh_imu_calib](/doc/img/uzh_imu_calib.png)
+
+
+## Kalibr dataset
+Download the datasets for camera and camera-IMU calibration from [here (Sample datasets)](https://github.com/ethz-asl/kalibr/wiki/downloads):
+```
+mkdir ~/kalibr_calib_data
+cd ~/kalibr_calib_data
+# Download data
+tar xvf static.tar.gz
+tar xvf dynamic.tar.gz
+```
+
+### Camera calibration
+Run the camera calibration:
+```
+basalt_calibrate --dataset-path ~/kalibr_calib_data/static/static.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --result-path ~/kalibr_calib_result/ --cam-types ds ds ds ds
+```
+![kalibr_cam_calib](/doc/img/kalibr_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 ~/kalibr_calib_data/dynamic/dynamic.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --result-path ~/kalibr_calib_result/ --gyro-noise-std 0.005 --accel-noise-std 0.01 --gyro-bias-std 4.0e-06 --accel-bias-std 0.0002
+```
+![kalibr_imu_calib](/doc/img/kalibr_imu_calib.png)

doc/DevSetup.md

@@ -0,0 +1,71 @@
+
+
+### Clang-format
+We use clang-format to maintain a consistent formating of the code. Since there are small differences between different version of clang-format we use version 11 on all platforms.
+
+On **Ubuntu 20.04 or 18.04** run the following commands to install clang-format-11
+```
+wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add -
+sudo sh -c 'echo "deb http://apt.llvm.org/$(lsb_release -sc)/ llvm-toolchain-$(lsb_release -sc)-11 main" > /etc/apt/sources.list.d/llvm11.list'
+sudo apt-get update
+sudo apt-get install clang-format-11
+```
+
+On **MacOS** [Homebrew](https://brew.sh/) should install the right version of clang-format:
+```
+brew install clang-format
+```
+
+### Realsense Drivers (Optional)
+If you want to use the code with Realsense T265 cameras you should install the realsense library.
+
+On **Ubuntu 20.04 or 18.04** run the following commands:
+```
+sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE
+sudo sh -c 'echo "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" > /etc/apt/sources.list.d/realsense.list'
+sudo apt-get update
+sudo apt-get install librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils librealsense2-dkms
+```
+
+On **MacOS** run:
+```
+brew install librealsense
+```
+
+### Install and configure QtCreator
+Download and install QtCreator. On **Ubuntu 20.04 or 18.04** run:
+```
+wget https://download.qt.io/official_releases/qtcreator/4.10/4.10.0/qt-creator-opensource-linux-x86_64-4.10.0.run
+chmod +x qt-creator-opensource-linux-x86_64-4.10.0.run
+./qt-creator-opensource-linux-x86_64-4.10.0.run
+```
+
+On **MacOS** run:
+```
+brew cask install qt-creator
+```
+
+After installation, go to `Help` -> `About plugins...` in the menu and enable Beautifier plugin (formats the code automatically on save):
+
+![qt_creator_plugins](/doc/img/qt_creator_plugins.png)
+
+Go to `Tools` -> `Options` and select the Beautifier tab. There select ClangFormat as the tool in `General` tab.
+
+![qt_creator_beautifier_general](/doc/img/qt_creator_beautifier_general.png)
+
+Select file as predefined style in `Clang Format` tab. Also select `None` as the fallback style. For **Ubuntu 20.04 or 18.04** change the executable name to `/usr/bin/clang-format-11`.
+
+![qt_creator_beautifier_clang_format](/doc/img/qt_creator_beautifier_clang_format.png)
+
+### Build project
+First, clone the project repository.
+```
+git clone --recursive https://gitlab.com/VladyslavUsenko/basalt.git
+```
+
+After that, in QtCreator open to the `CMakeLists.txt` in the `basalt` folder and configure the project with `Release with Debug Info` configuration. The build directory should point to `/<your_installation_path>/basalt/build`.
+
+![qt_creator_configure_project](/doc/img/qt_creator_configure_project.png)
+
+Finally, you should be able to build and run the project.
+

doc/Realsense.md

@@ -0,0 +1,159 @@
+# Tutorial on Camera-IMU and Motion Capture Calibration with Realsense T265
+
+![Realsense](/doc/img/realsense_setup.jpg)
+
+In this tutorial we explain how to perform photometric and geometric calibration of the multi-camera setup and then calibrate the transformations between cameras, IMU and the motion capture marker setup. To make sure the calibration is successful we recommend to rigidly attach markers to the camera as shown on the figure above.
+
+## Dataset
+We provide a set of example datasets for the calibration. Even if you plan to record your own datasets (covered in the next section), we recommend to download and try the provided examples:
+```
+mkdir ~/t265_calib_data
+cd ~/t265_calib_data
+wget http://vision.in.tum.de/tumvi/t265/response_calib.zip
+wget http://vision.in.tum.de/tumvi/t265/cam_calib.zip
+wget http://vision.in.tum.de/tumvi/t265/imu_calib.zip
+wget http://vision.in.tum.de/tumvi/t265/sequence0.zip
+unzip response_calib
+unzip cam_calib
+unzip imu_calib
+unzip sequence0
+```
+
+## Recording Own Dataset
+You can record your own sequences using the `basalt_rs_t265_record` executable:
+```
+basalt_rs_t265_record --dataset-path ~/t265_calib_data/ --manual-exposure 
+```
+* `--dataset-path` specifies the location where the recorded dataset will be stored. In this case it will be stored in `~/t265_calib_data/<current_timestamp>/`.
+* `--manual-exposure` disables the autoexposure. In this tutorial the autoexposure is disabled for all calibration sequences, but for the VIO sequence (sequence0) we enable it.
+
+![t265_record](/doc/img/t265_record.png)
+
+The GUI elements have the following meaning:
+* `webp_quality` compression quality. The highest value (101) means lossless compression. For photometric calibration it is important not to have any compression artifacts, so we record these calibration sequences with lossless compression.
+* `skip_frames` reduces the framerate of the recorded dataset by skipping frames.
+* `exposure` controls the exposure time of the cameras.
+* `record` starts/stops the recoding of the dataset. If you run the system on the slow PC pay attention to the number of messages in the queue. If it goes beyond the limit the recorder will start dropping frames.
+* `export_calibration` exports factory calibration in the basalt format.
+
+After recoding the dataset it is a good practice to verify the quality of the dataset. You can do this by running:
+```
+basalt_verify_dataset.py -d ~/t265_calib_data/<dataset_path>/
+```
+It will report the actual frequencies of the recorded sensor messages and warn you if any files with image data are missing.
+
+Every sequence required for the calibration should have certain properties to enable successful calibration. Pay attention to the **Important for recording the dataset** subsections and inspect the provided examples before recoding your own sequences.
+
+## Response calibration
+In this project we assume a camera has a linear response function (intensity of the pixel is linearly proportional to the amount of light captured by the pixel). In this section we will verify this for the Realsense T265 cameras. We will need to record a static scene with different exposure times.
+
+**Important for recording the dataset:**
+* Keep the camera static and make sure that nothing in the scene moves during the recording.
+* Move `webp_quality` slider to the highest value to enable lossless compression.
+* Optionally set the `skip_frames` slider to 3-5 to speed up the dataset recoding.
+* Start recoding and slowly move the exposure slider up and down. Stop recoding.
+* Rename the dataset to `response_calib`.
+
+Run the response function calibration:
+```
+basalt_response_calib.py -d ~/t265_calib_data/response_calib
+```
+You should see the response function and the irradiance image similar to the one shown below. For the details of the algorithm see Section 2.3.1 of [[arXiv:1607.02555]](https://arxiv.org/abs/1607.02555). The results suggest that the response function used in the camera is linear. 
+![t265_inv_resp_irradiance](/doc/img/t265_inv_resp_irradiance.png)
+
+## Multi-Camera Geometric and Vignette Calibration
+For the camera calibration we need to record a dataset with a static aprilgrid pattern.
+
+**Important for recording the dataset:**
+* Move `webp_quality` slider to the highest value to enable lossless compression (important for vignette calibration).
+* Set the `skip_frames` slider to 5 to speed up the dataset recoding.
+* Move the camera slowly to reduce the motion blur.
+* Cover the entire field of view of the camera with the calibration pattern. Try to observe the pattern from different angles.
+* Make sure you do not cast shadows at the pattern (important for vignette calibration).
+* Rename the dataset to `cam_calib` 
+
+Run the calibration executable:
+```
+basalt_calibrate --dataset-path ~/t265_calib_data/cam_calib --dataset-type euroc --result-path ~/t265_calib_results/ --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --cam-types kb4 kb4
+```
+To perform the calibration follow these steps:
+* `load_dataset` load the dataset.
+* `detect_corners` detect corners. If the corners were detected before the cached detections will be loaded at the previous step so there is no need to re-run the detection.
+* `init_cam_intr` initialize camera intrinsics.
+* `init_cam_poses` initialize camera poses using the current intrinsics estimate.
+* `init_cam_extr` initialize transformations between multiple cameras.
+* `init_opt` initialize optimization.
+* `opt_until_converge` optimize until convergence.
+* `init_cam_poses` some initial poses computed from the initialized intrinsics can be far from optimum and not converge to the right minimum. To improve the final result we can re-initialize poses with optimized intrinsics.
+* `init_opt` initialize optimization with new initial poses.
+* `opt_until_converge` optimize until convergence.
+* `compute_vign` after optimizing geometric models compute the vignetting of the cameras.
+* `save_calib` save calibration file to the `~/t265_calib_results/calibration.json`.
+
+![t265_cam_calib](/doc/img/t265_cam_calib.png)
+
+
+## IMU and Motion Capture Calibration
+After calibrating cameras we can proceed to geometric and time calibration of the cameras, IMU and motion capture system. Setting up the motion capture system is specific for your setup. 
+
+For the motion capture recording we use [ros_vrpn_client](https://github.com/ethz-asl/ros_vrpn_client) with [basalt_capture_mocap.py](/scripts/basalt_capture_mocap.py). We record the data to the `mocap0` folder and then move it to the `mav0` directory of the camera dataset. This script is provided as an example. Motion capture setup is different in every particular case.
+
+**Important for recording the dataset:**
+* Set the `skip_frames` slider to 1 to use the full framerate.
+* Reduce the exposure time to reduce the motion blur.
+* Move the setup such that all axes of accelerometer and gyro are excited. This means moving with acceleration along X, Y and Z axes and rotating around those axes.
+* Do not forget to simultaneously record motion capture data.
+* Rename the dataset to `imu_calib`.
+
+Run the calibration executable:
+```
+basalt_calibrate_imu --dataset-path ~/t265_calib_data/imu_calib --dataset-type euroc --result-path ~/t265_calib_results/ --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --accel-noise-std 0.00818 --gyro-noise-std 0.00226 --accel-bias-std 0.01 --gyro-bias-std 0.0007
+```
+
+To perform the calibration follow these steps:
+* `load_dataset` load the dataset.
+* `detect_corners` detect corners. If the corners were detected before the cached detections will be loaded at the previous step so there is no need to re-run the detection.
+* `init_cam_poses` initialize camera poses.
+* `init_cam_imu` initialize the transformation between cameras and the IMU.
+* `init_opt` initialize optimization.
+* `opt_until_converge` optimize until convergence.
+* Enable `opt_cam_time_offset` and `opt_imu_scale` to optimize the time offset between cameras and the IMU and the IMU scale.
+* `opt_until_converge` optimize until convergence.
+* `init_mocap` initialize transformation between the motion capture marker frame and the IMU and the transformation between the aprilgrid pattern and the motion capture system origin.
+* Enable `opt_mocap`.
+* `opt_until_converge` optimize until convergence.
+* `save_calib` save calibration file to the `~/t265_calib_results/calibration.json`.
+* `save_mocap_calib` save motion capture system calibration file to the `~/t265_calib_results/mocap_calibration.json`.
+
+![t265_imu_calib](/doc/img/t265_imu_calib.png)
+
+
+## Generating Time-Aligned Ground Truth
+Since motion capture system and the PC where the dataset was recorded might not have the same clock we need to perform the time synchronization. Additionally we need to transform the coordinate frame of the GT data to the IMU frame (originally it is in the coordinate frame attached to the markers).
+
+The raw motion capture data is stored in the `mav/mocap0/` folder. We can find the time offset by minimizing the error between gyro measurements and rotational velocities computed from the motion capture data. If you press the `save_aligned_dataset` button the resulting trajectory (time aligned and transformed to the IMU frame) will be written to `mav/gt/data.csv` and automatically loaded when available.
+```
+basalt_time_alignment --dataset-path ~/t265_calib_data/sequence0 --dataset-type euroc --calibration ~/t265_calib_results/calibration.json --mocap-calibration ~/t265_calib_results/mocap_calibration.json
+```
+You should be able to see that, despite some noise, rotational velocity computed from the motion capture data aligns well with gyro measurements.
+![t265_time_align_gyro](/doc/img/t265_time_align_gyro.png)
+
+You can also switch to the error function plot and see that there is a clear minimum corresponding to the computed time offset.
+![t265_time_align_error](/doc/img/t265_time_align_error.png)
+
+**Note:** If you want to run the time alignment again you should delete the `~/t265_calib_data/sequence0/mav/gt` folder first. If GT data already exist you will see the `save_aligned_dataset(disabled)` button which will **NOT** overwrite it.
+
+## Running Visual-Inertial Odometry
+Now we can run the visual-inertial odometry on the recorded dataset:
+```
+basalt_vio --dataset-path ~/t265_calib_data/sequence0 --cam-calib ~/t265_calib_results/calibration.json --dataset-type euroc --config-path /usr/etc/basalt/euroc_config.json --show-gui 1
+```
+After the system processes the whole sequence you can use `align_se3` button to align trajectory to the ground-truth data and compute RMS ATE.
+![t265_vio](/doc/img/t265_vio.png)
+
+
+## Running Visual-Inertial Odometry Live
+It is also possible to run the odometry live with the camera. If no calibration files are provided the factory calibration will be used.
+```
+basalt_rs_t265_vio --cam-calib ~/t265_calib_results/calibration.json --config-path /usr/etc/basalt/euroc_config.json
+```

doc/Simulation.md

@@ -0,0 +1,51 @@
+## 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 of the ground-truth landmarks in the image view.
+* `show_obs_noisy` toggles the visibility of the noisy landmarks in the image view.
+* `show_obs_vio` toggles the visibility of 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 the ground-truth spline.
+* `show_gyro` shows noisy gyroscope measurements generated from the 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_se3` 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_se3` 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.
+* `align_se3` performs SE(3) alignment with ground-truth trajectory and prints the RMS ATE to the console.
+
+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.

doc/VioMapping.md

@@ -0,0 +1,121 @@
+## 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.
+* `--config-path` path to the configuration file.
+* `--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 the 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 of 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_gt` 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` visualizes every frame without skipping. If both options are disabled the system shows the frame that is selected with the `show_frame` slider and the 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_se3` is used. It aligns the GT trajectory with the current estimate using an SE(3) transformation and prints the transformation and the root-mean-squared absolute trajectory error (RMS ATE).
+
+The button `save_traj` saves the trajectory in one of two formats (`euroc_fmt` or `tum_rgbd_fmt`). In EuRoC format each pose is a line in the file and has the following format `timestamp[ns],tx,ty,tz,qw,qx,qy,qz`. TUM RBG-D can be used with [TUM RGB-D](https://vision.in.tum.de/data/datasets/rgbd-dataset/tools) or [UZH](https://github.com/uzh-rpg/rpg_trajectory_evaluation) trajectory evaluation tools and has the following format `timestamp[s] tx ty tz qx qy qz qw`. 
+
+
+### 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
+```
+Here `--marg-data` is the folder with the results from VIO.
+
+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_se3` align ground-truth trajectory in SE(3) and print the transformation and the error.
+
+The `num_opt_iter` slider controls the maximum number of iterations executed when pressing `optimize`.
+
+The button `save_traj` works similar to the VIO, but saves the keyframe trajectory (subset of frames).
+
+For more systematic evaluation see the evaluation scripts in the [scripts/eval_full](/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 the `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)
+
+
+## TUM-VI dataset
+
+We demonstrate the usage of the system with the `magistrale1` sequence of the [TUM-VI dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) as an example.
+
+Download the sequence from the dataset and extract it. 
+```
+mkdir tumvi_data
+cd tumvi_data
+wget http://vision.in.tum.de/tumvi/exported/euroc/512_16/dataset-magistrale1_512_16.tar
+tar -xvf dataset-magistrale1_512_16.tar
+```
+
+### Visual-inertial odometry
+To run the visual-inertial odometry execute the following command in `tumvi_data` folder where you downloaded the dataset.
+```
+basalt_vio --dataset-path dataset-magistrale1_512_16/ --cam-calib /usr/etc/basalt/tumvi_512_ds_calib.json --dataset-type euroc --config-path /usr/etc/basalt/tumvi_512_config.json --marg-data tumvi_marg_data --show-gui 1 
+```
+![magistrale1_vio](/doc/img/magistrale1_vio.png)
+
+### Visual-inertial mapping
+To run the mapping tool execute the following command:
+```
+basalt_mapper --cam-calib /usr/etc/basalt/tumvi_512_ds_calib.json --marg-data tumvi_marg_data
+```
+![magistrale1_mapping](/doc/img/magistrale1_mapping.png)

doc/Vo.md

@@ -0,0 +1,25 @@
+## KITTI dataset
+
+[![teaser](/doc/img/kitti_video.png)](https://www.youtube.com/watch?v=M_ZcNgExUNc)
+
+We demonstrate the usage of the system with the [KITTI dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) 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 sequences (`data_odometry_gray.zip`) from the dataset and extract it. 
+```
+# We assume you have extracted the sequences in ~/dataset_gray/sequences/
+# Convert calibration to the basalt format
+basalt_convert_kitti_calib.py ~/dataset_gray/sequences/00/
+
+# If you want to convert calibrations for all sequences use the following command
+for i in {00..21}; do basalt_convert_kitti_calib.py ~/dataset_gray/sequences/$i/; done
+```
+Optionally you can also copy the provided ground-truth poses to `poses.txt` in the corresponding sequence.
+
+### Visual odometry
+To run the visual odometry execute the following command.
+```
+basalt_vio --dataset-path ~/dataset_gray/sequences/00/ --cam-calib /work/kitti/dataset_gray/sequences/00/basalt_calib.json --dataset-type kitti --config-path /usr/etc/basalt/kitti_config.json --show-gui 1 --use-imu 0
+```
+![magistrale1_vio](/doc/img/kitti.png)

docker/b_image_bionic/Dockerfile

@@ -0,0 +1,19 @@
+FROM ubuntu:18.04
+
+RUN apt-get update && apt-get install -y wget gnupg lsb-release software-properties-common && apt-get clean all
+
+RUN wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key| apt-key add -
+RUN echo "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-11 main" > /etc/apt/sources.list.d/llvm11.list
+RUN echo "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-12 main" > /etc/apt/sources.list.d/llvm12.list
+
+RUN add-apt-repository ppa:ubuntu-toolchain-r/test
+
+RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
+RUN add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
+
+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 librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils doxygen graphviz libsuitesparse-dev clang-11 clang-format-11 clang-tidy-11 clang-12 clang-format-12 clang-tidy-12 g++-9 libfmt-dev && apt-get clean all
+
+RUN update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 60 \
+                         --slave /usr/bin/g++ g++ /usr/bin/g++-9
+RUN gcc --version
+RUN g++ --version

docker/b_image_focal/Dockerfile

@@ -0,0 +1,12 @@
+FROM ubuntu:20.04
+
+RUN apt-get update && apt-get install -y wget gnupg lsb-release software-properties-common && apt-get clean all
+
+RUN wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key| apt-key add -
+RUN echo "deb http://apt.llvm.org/focal/ llvm-toolchain-focal-11 main" > /etc/apt/sources.list.d/llvm11.list
+RUN echo "deb http://apt.llvm.org/focal/ llvm-toolchain-focal-12 main" > /etc/apt/sources.list.d/llvm12.list
+
+RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
+RUN add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
+
+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 librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils doxygen graphviz libsuitesparse-dev clang-11 clang-format-11 clang-tidy-11 clang-12 clang-format-12 clang-tidy-12 libfmt-dev && apt-get clean all

include/basalt/calibration/aprilgrid.h

@@ -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.
+*/
+#pragma once
+
+#include <basalt/utils/sophus_utils.hpp>
+
+namespace basalt {
+
+struct AprilGrid {
+  AprilGrid(const std::string &config_path);
+
+  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
+  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
+
+  inline int getTagCols() const { return tagCols; }
+  inline int getTagRows() const { return tagRows; }
+
+ private:
+  int tagCols;        // number of apriltags
+  int tagRows;        // number of apriltags
+  double tagSize;     // size of apriltag, edge to edge [m]
+  double tagSpacing;  // ratio of space between tags to tagSize
+};
+
+}  // namespace basalt

include/basalt/calibration/calibration_helper.h

@@ -0,0 +1,131 @@
+/**
+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/io/dataset_io.h>
+#include <basalt/utils/common_types.h>
+#include <basalt/calibration/calibration.hpp>
+
+#include <tbb/concurrent_unordered_map.h>
+
+namespace basalt {
+
+struct CalibCornerData {
+  Eigen::aligned_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 ProjectedCornerData {
+  Eigen::aligned_vector<Eigen::Vector2d> corners_proj;
+  std::vector<bool> corners_proj_success;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct CalibInitPoseData {
+  Sophus::SE3d T_a_c;
+  size_t num_inliers;
+
+  Eigen::aligned_vector<Eigen::Vector2d> reprojected_corners;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+using CalibCornerMap = tbb::concurrent_unordered_map<TimeCamId, CalibCornerData,
+                                                     std::hash<TimeCamId>>;
+
+using CalibInitPoseMap =
+    tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData,
+                                  std::hash<TimeCamId>>;
+
+class CalibHelper {
+ public:
+  static void detectCorners(const VioDatasetPtr& vio_data,
+                            CalibCornerMap& calib_corners,
+                            CalibCornerMap& calib_corners_rejected);
+
+  static void initCamPoses(
+      const Calibration<double>::Ptr& calib,
+      const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
+      CalibCornerMap& calib_corners, CalibInitPoseMap& calib_init_poses);
+
+  static bool initializeIntrinsics(
+      const Eigen::aligned_vector<Eigen::Vector2d>& corners,
+      const std::vector<int>& corner_ids, const AprilGrid& aprilgrid, int cols,
+      int rows, Eigen::Vector4d& init_intr);
+
+  static bool initializeIntrinsicsPinhole(
+      const std::vector<CalibCornerData*> pinhole_corners,
+      const AprilGrid& aprilgrid, 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::aligned_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::aligned_vector<Eigen::Vector2d>& corners,
+      const std::vector<int>& corner_ids,
+      const Eigen::aligned_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

include/basalt/calibration/cam_calib.h

@@ -0,0 +1,176 @@
+/**
+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/image/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 &aprilgrid_path, 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();
+
+  bool 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;
+
+  VioDatasetPtr vio_dataset;
+  // CalibrationPtr calib;
+
+  CalibCornerMap calib_corners;
+  CalibCornerMap calib_corners_rejected;
+  CalibInitPoseMap calib_init_poses;
+
+  std::shared_ptr<std::thread> processing_thread;
+
+  std::shared_ptr<PosesOptimization> calib_opt;
+
+  std::map<TimeCamId, ProjectedCornerData> reprojected_corners;
+  std::map<TimeCamId, ProjectedCornerData> reprojected_vignette;
+  std::map<TimeCamId, std::vector<double>> reprojected_vignette_error;
+
+  std::string dataset_path;
+  std::string dataset_type;
+
+  AprilGrid april_grid;
+
+  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;
+
+  pangolin::Var<bool> opt_until_convg;
+  pangolin::Var<double> stop_thresh;
+
+  std::shared_ptr<pangolin::Plotter> vign_plotter;
+  std::shared_ptr<pangolin::Plotter> polar_plotter;
+  std::shared_ptr<pangolin::Plotter> azimuth_plotter;
+
+  std::vector<pangolin::Colour> cam_colors;
+
+  pangolin::View *img_view_display;
+
+  std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+
+  pangolin::DataLog vign_data_log;
+
+  std::vector<std::shared_ptr<pangolin::DataLog>> polar_data_log,
+      azimuth_data_log;
+};
+
+}  // namespace basalt

include/basalt/calibration/cam_imu_calib.h

@@ -0,0 +1,182 @@
+/**
+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/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 &aprilgrid_path, 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();
+
+  bool 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;
+
+  VioDatasetPtr vio_dataset;
+
+  CalibCornerMap calib_corners;
+  CalibCornerMap calib_corners_rejected;
+  CalibInitPoseMap calib_init_poses;
+
+  std::shared_ptr<std::thread> processing_thread;
+
+  std::shared_ptr<SplineOptimization<5, double>> calib_opt;
+
+  std::map<TimeCamId, ProjectedCornerData> reprojected_corners;
+
+  std::string dataset_path;
+  std::string dataset_type;
+
+  AprilGrid april_grid;
+
+  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::Var<bool> opt_until_convg;
+  pangolin::Var<double> stop_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

include/basalt/calibration/vignette.h

@@ -0,0 +1,89 @@
+/**
+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>
+
+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::aligned_vector<Eigen::Vector2d> &optical_centers,
+      const Eigen::aligned_vector<Eigen::Vector2i> &resolutions,
+      const std::map<TimeCamId, Eigen::aligned_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(std::vector<std::vector<float>> &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::aligned_vector<Eigen::Vector2d> optical_centers;
+  Eigen::aligned_vector<Eigen::Vector2i> resolutions;
+  std::map<TimeCamId, Eigen::aligned_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;
+};
+}  // namespace basalt

include/basalt/device/rs_t265.h

@@ -0,0 +1,118 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer 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 <math.h>
+#include <atomic>
+#include <cstring>
+#include <fstream>
+#include <iomanip>
+#include <mutex>
+#include <thread>
+
+#include <librealsense2/rs.hpp>
+
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <tbb/concurrent_queue.h>
+
+#include <basalt/imu/imu_types.h>
+#include <basalt/optical_flow/optical_flow.h>
+#include <basalt/calibration/calibration.hpp>
+
+namespace basalt {
+
+struct RsIMUData {
+  double timestamp;
+  Eigen::Vector3d data;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct RsPoseData {
+  int64_t t_ns;
+  Sophus::SE3d data;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+class RsT265Device {
+ public:
+  using Ptr = std::shared_ptr<RsT265Device>;
+
+  static constexpr int IMU_RATE = 200;
+  static constexpr int NUM_CAMS = 2;
+
+  RsT265Device(bool manual_exposure, int skip_frames, int webp_quality,
+               double exposure_value = 10.0);
+
+  void start();
+  void stop();
+
+  bool setExposure(double exposure);  // in milliseconds
+  void setSkipFrames(int skip);
+  void setWebpQuality(int quality);
+
+  std::shared_ptr<basalt::Calibration<double>> exportCalibration();
+
+  OpticalFlowInput::Ptr last_img_data;
+  tbb::concurrent_bounded_queue<OpticalFlowInput::Ptr>* image_data_queue =
+      nullptr;
+  tbb::concurrent_bounded_queue<ImuData<double>::Ptr>* imu_data_queue = nullptr;
+  tbb::concurrent_bounded_queue<RsPoseData>* pose_data_queue = nullptr;
+
+ private:
+  bool manual_exposure;
+  int skip_frames;
+  int webp_quality;
+
+  int frame_counter = 0;
+
+  Eigen::aligned_deque<RsIMUData> gyro_data_queue;
+  std::shared_ptr<RsIMUData> prev_accel_data;
+
+  std::shared_ptr<basalt::Calibration<double>> calib;
+
+  rs2::context context;
+  rs2::config config;
+  rs2::pipeline pipe;
+  rs2::sensor sensor;
+
+  rs2::pipeline_profile profile;
+};
+
+}  // namespace basalt

include/basalt/hash_bow/hash_bow.h

@@ -0,0 +1,167 @@
+#pragma once
+
+#include <array>
+#include <bitset>
+#include <iostream>
+#include <unordered_map>
+#include <vector>
+
+#include <basalt/utils/common_types.h>
+
+#include <tbb/concurrent_unordered_map.h>
+#include <tbb/concurrent_vector.h>
+
+namespace basalt {
+
+template <size_t N>
+class HashBow {
+ public:
+  HashBow(size_t num_bits) : num_bits(num_bits < 32 ? num_bits : 32) {
+    static_assert(N < 512,
+                  "This implementation of HashBow only supports the descriptor "
+                  "length below 512.");
+  }
+
+  inline FeatureHash compute_hash(const std::bitset<N>& descriptor) const {
+    FeatureHash res;
+    for (size_t i = 0; i < num_bits; ++i) {
+      res[i] = descriptor[word_bit_permutation[i]];
+    }
+    return res;
+  }
+
+  inline void compute_bow(const std::vector<std::bitset<N>>& descriptors,
+                          std::vector<FeatureHash>& hashes,
+                          HashBowVector& bow_vector) const {
+    size_t descriptors_size = descriptors.size();
+    hashes.resize(descriptors_size);
+
+    std::unordered_map<FeatureHash, double> bow_map;
+    bow_map.clear();
+    bow_map.reserve(descriptors_size);
+
+    for (size_t i = 0; i < descriptors_size; i++) {
+      hashes[i] = compute_hash(descriptors[i]);
+      bow_map[hashes[i]] += 1.0;
+    }
+
+    bow_vector.clear();
+
+    double l1_sum = 0;
+    for (const auto& kv : bow_map) {
+      bow_vector.emplace_back(kv);
+      l1_sum += std::abs(kv.second);
+    }
+
+    for (auto& kv : bow_vector) {
+      kv.second /= l1_sum;
+    }
+  }
+
+  inline void add_to_database(const TimeCamId& tcid,
+                              const HashBowVector& bow_vector) {
+    for (const auto& kv : bow_vector) {
+      // std::pair<TimeCamId, double> p = std::make_pair(tcid, kv.second);
+      inverted_index[kv.first].emplace_back(tcid, kv.second);
+    }
+  }
+
+  inline void querry_database(
+      const HashBowVector& bow_vector, size_t num_results,
+      std::vector<std::pair<TimeCamId, double>>& results,
+      const int64_t* max_t_ns = nullptr) const {
+    results.clear();
+
+    std::unordered_map<TimeCamId, double> scores;
+
+    for (const auto& kv : bow_vector) {
+      const auto range_it = inverted_index.find(kv.first);
+
+      if (range_it != inverted_index.end())
+        for (const auto& v : range_it->second) {
+          // if there is a maximum query time select only the frames that have
+          // timestamp below max_t_ns
+          if (!max_t_ns || v.first.frame_id < (*max_t_ns))
+            scores[v.first] += std::abs(kv.second - v.second) -
+                               std::abs(kv.second) - std::abs(v.second);
+        }
+    }
+
+    results.reserve(scores.size());
+
+    for (const auto& kv : scores)
+      results.emplace_back(kv.first, -kv.second / 2.0);
+
+    if (results.size() > num_results) {
+      std::partial_sort(
+          results.begin(), results.begin() + num_results, results.end(),
+          [](const auto& a, const auto& b) { return a.second > b.second; });
+
+      results.resize(num_results);
+    }
+  }
+
+ protected:
+  constexpr static const size_t random_bit_permutation[512] = {
+      484, 458, 288, 170, 215, 424, 41,  38,  293, 96,  172, 428, 508, 52,  370,
+      1,   182, 472, 89,  339, 273, 234, 98,  217, 73,  195, 307, 306, 113, 429,
+      161, 443, 364, 439, 301, 247, 325, 24,  490, 366, 75,  7,   464, 232, 49,
+      196, 144, 69,  470, 387, 3,   86,  361, 313, 396, 356, 94,  201, 291, 360,
+      107, 251, 413, 393, 296, 124, 308, 146, 298, 160, 121, 302, 151, 345, 336,
+      26,  63,  238, 79,  267, 262, 437, 433, 350, 53,  134, 194, 452, 114, 54,
+      82,  214, 191, 242, 482, 37,  432, 311, 130, 460, 422, 221, 271, 192, 474,
+      46,  289, 34,  20,  95,  463, 499, 159, 272, 481, 129, 448, 173, 323, 258,
+      416, 229, 334, 510, 461, 263, 362, 346, 39,  500, 381, 401, 492, 299, 33,
+      169, 241, 11,  254, 449, 199, 486, 400, 365, 70,  436, 108, 19,  233, 505,
+      152, 6,   480, 468, 278, 426, 253, 471, 328, 327, 139, 29,  27,  488, 332,
+      290, 412, 164, 259, 352, 222, 186, 32,  319, 410, 211, 405, 187, 213, 507,
+      205, 395, 62,  178, 36,  140, 87,  491, 351, 450, 314, 77,  342, 132, 133,
+      477, 103, 389, 206, 197, 324, 485, 425, 297, 231, 123, 447, 126, 9,   64,
+      181, 40,  14,  5,   261, 431, 333, 223, 4,   138, 220, 76,  44,  300, 331,
+      78,  193, 497, 403, 435, 275, 147, 66,  368, 141, 451, 225, 250, 61,  18,
+      444, 208, 380, 109, 255, 337, 372, 212, 359, 457, 31,  398, 354, 219, 117,
+      248, 392, 203, 88,  479, 509, 149, 120, 145, 51,  15,  367, 190, 163, 417,
+      454, 329, 183, 390, 83,  404, 249, 81,  264, 445, 317, 179, 244, 473, 71,
+      111, 118, 209, 171, 224, 459, 446, 104, 13,  377, 200, 414, 198, 420, 226,
+      153, 384, 25,  441, 305, 338, 316, 483, 184, 402, 48,  131, 502, 252, 469,
+      12,  167, 243, 373, 35,  127, 341, 455, 379, 210, 340, 128, 430, 57,  434,
+      330, 415, 494, 142, 355, 282, 322, 65,  105, 421, 68,  409, 466, 245, 59,
+      269, 112, 386, 257, 256, 93,  174, 16,  60,  143, 343, 115, 506, 276, 10,
+      496, 489, 235, 47,  136, 22,  165, 204, 42,  465, 440, 498, 312, 504, 116,
+      419, 185, 303, 218, 353, 283, 374, 2,   177, 137, 240, 102, 309, 292, 85,
+      453, 388, 397, 438, 281, 279, 442, 110, 55,  101, 100, 150, 375, 406, 157,
+      23,  0,   237, 376, 236, 216, 8,   154, 91,  456, 423, 176, 427, 284, 30,
+      84,  349, 335, 56,  270, 227, 286, 168, 239, 122, 478, 162, 475, 166, 17,
+      348, 285, 175, 155, 266, 382, 304, 268, 180, 295, 125, 371, 467, 277, 294,
+      58,  347, 72,  280, 50,  287, 511, 80,  260, 326, 495, 45,  106, 399, 369,
+      503, 357, 315, 418, 487, 99,  43,  320, 188, 407, 246, 501, 119, 158, 274,
+      408, 230, 358, 90,  148, 363, 207, 344, 265, 462, 189, 310, 385, 67,  28,
+      383, 378, 156, 394, 97,  476, 493, 321, 411, 228, 21,  391, 202, 92,  318,
+      74,  135};
+
+  constexpr static std::array<size_t, FEATURE_HASH_MAX_SIZE>
+  compute_permutation() {
+    std::array<size_t, FEATURE_HASH_MAX_SIZE> res{};
+    size_t j = 0;
+    for (size_t i = 0; i < 512 && j < FEATURE_HASH_MAX_SIZE; ++i) {
+      if (random_bit_permutation[i] < N) {
+        res[j] = random_bit_permutation[i];
+        j++;
+      }
+    }
+
+    return res;
+  }
+
+  constexpr static const std::array<size_t, FEATURE_HASH_MAX_SIZE>
+      word_bit_permutation = compute_permutation();
+
+  size_t num_bits;
+
+  tbb::concurrent_unordered_map<
+      FeatureHash, tbb::concurrent_vector<std::pair<TimeCamId, double>>,
+      std::hash<FeatureHash>>
+      inverted_index;
+};
+
+}  // namespace basalt

include/basalt/io/dataset_io.h

@@ -0,0 +1,178 @@
+/**
+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 <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/image/image.h>
+#include <basalt/utils/assert.h>
+
+#include <basalt/camera/generic_camera.hpp>
+#include <basalt/camera/stereographic_param.hpp>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+struct ImageData {
+  ImageData() : exposure(0) {}
+
+  ManagedImage<uint16_t>::Ptr img;
+  double exposure;
+};
+
+struct Observations {
+  Eigen::aligned_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::aligned_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::aligned_vector<AccelData> &get_accel_data() const = 0;
+  virtual const Eigen::aligned_vector<GyroData> &get_gyro_data() const = 0;
+  virtual const std::vector<int64_t> &get_gt_timestamps() const = 0;
+  virtual const Eigen::aligned_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;
+  virtual std::vector<cv::Mat> get_image_data_cv_mat(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 load_mocap_as_gt = false);
+};
+
+}  // namespace basalt
+
+namespace cereal {
+
+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

include/basalt/io/dataset_io_euroc.h

@@ -0,0 +1,336 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef DATASET_IO_EUROC_H
+#define DATASET_IO_EUROC_H
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/utils/filesystem.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_pose_data;  // TODO: change to eigen aligned
+
+  int64_t mocap_to_imu_offset_ns = 0;
+
+  std::vector<std::unordered_map<int64_t, double>> exposure_times;
+
+ public:
+  ~EurocVioDataset(){};
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_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 (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
+
+        if (img.type() == CV_8UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else if (img.type() == CV_8UC3) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i * 3];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else if (img.type() == CV_16UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+          std::memcpy(res[i].img->ptr, img.ptr(),
+                      img.cols * img.rows * sizeof(uint16_t));
+
+        } else {
+          std::cerr << "img.fmt.bpp " << img.type() << std::endl;
+          std::abort();
+        }
+
+        auto exp_it = exposure_times[i].find(t_ns);
+        if (exp_it != exposure_times[i].end()) {
+          res[i].exposure = exp_it->second;
+        }
+      }
+    }
+
+    return res;
+  }
+
+  // returns a vector of 2 images: for the first and the second camera at the defined timestamp.
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    std::vector<cv::Mat> 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 (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_COLOR);
+
+        res.push_back(img);
+        // The rest of the body from get_image_data() was deleted.
+      }
+    }
+    return res;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class EurocIO;
+};
+
+class EurocIO : public DatasetIoInterface {
+ public:
+  EurocIO(bool load_mocap_as_gt) : load_mocap_as_gt(load_mocap_as_gt) {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new EurocVioDataset);
+
+    data->num_cams = 2;
+    data->path = path;
+
+    read_image_timestamps(path + "/mav0/cam0/");
+
+    read_imu_data(path + "/mav0/imu0/");
+
+    if (!load_mocap_as_gt &&
+        fs::exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) {
+      read_gt_data_state(path + "/mav0/state_groundtruth_estimate0/");
+    } else if (!load_mocap_as_gt && fs::exists(path + "/mav0/gt/data.csv")) {
+      read_gt_data_pose(path + "/mav0/gt/");
+    } else if (fs::exists(path + "/mav0/mocap0/data.csv")) {
+      read_gt_data_pose(path + "/mav0/mocap0/");
+    }
+
+    data->exposure_times.resize(data->num_cams);
+    if (fs::exists(path + "/mav0/cam0/exposure.csv")) {
+      std::cout << "Loading exposure times for cam0" << std::endl;
+      read_exposure(path + "/mav0/cam0/", data->exposure_times[0]);
+    }
+    if (fs::exists(path + "/mav0/cam1/exposure.csv")) {
+      std::cout << "Loading exposure times for cam1" << std::endl;
+      read_exposure(path + "/mav0/cam1/", data->exposure_times[1]);
+    }
+  }
+
+  void reset() { data.reset(); }
+
+  VioDatasetPtr get_data() { return data; }
+
+ private:
+  void read_exposure(const std::string &path,
+                     std::unordered_map<int64_t, double> &exposure_data) {
+    exposure_data.clear();
+
+    std::ifstream f(path + "exposure.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      char tmp;
+      int64_t timestamp, exposure_int;
+      Eigen::Vector3d gyro, accel;
+
+      ss >> timestamp >> tmp >> exposure_int;
+
+      exposure_data[timestamp] = exposure_int * 1e-9;
+    }
+  }
+
+  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;
+  bool load_mocap_as_gt;
+};  // namespace basalt
+
+}  // namespace basalt
+
+#endif  // DATASET_IO_H

include/basalt/io/dataset_io_kitti.h

@@ -0,0 +1,220 @@
+/**
+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/utils/filesystem.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+class KittiVioDataset : 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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_pose_data;  // TODO: change to eigen aligned
+
+  int64_t mocap_to_imu_offset_ns;
+
+ public:
+  ~KittiVioDataset(){};
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_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 = {"/image_0/", "/image_1/"};
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path = path + folder[i] + image_path[t_ns];
+
+      if (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
+
+        if (img.type() == CV_8UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else {
+          std::cerr << "img.fmt.bpp " << img.type() << std::endl;
+          std::abort();
+        }
+      }
+    }
+
+    return res;
+  }
+
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    std::vector<cv::Mat> res(num_cams);
+
+    const std::vector<std::string> folder = {"/image_0/", "/image_1/"};
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path =
+          path + folder[i] + image_path[t_ns];
+
+      if (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_GRAYSCALE);
+
+        res.push_back(img);
+        // The rest of the body from get_image_data() was deleted.
+      }
+    }
+    return res;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class KittiIO;
+};
+
+class KittiIO : public DatasetIoInterface {
+ public:
+  KittiIO() {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new KittiVioDataset);
+
+    data->num_cams = 2;
+    data->path = path;
+
+    read_image_timestamps(path + "/times.txt");
+
+    if (fs::exists(path + "/poses.txt")) {
+      read_gt_data_pose(path + "/poses.txt");
+    }
+  }
+
+  void reset() { data.reset(); }
+
+  VioDatasetPtr get_data() { return data; }
+
+ private:
+  void read_image_timestamps(const std::string &path) {
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+      std::stringstream ss(line);
+
+      double t_s;
+      ss >> t_s;
+
+      int64_t t_ns = t_s * 1e9;
+
+      std::stringstream ss1;
+      ss1 << std::setfill('0') << std::setw(6) << data->image_timestamps.size()
+          << ".png";
+
+      data->image_timestamps.emplace_back(t_ns);
+      data->image_path[t_ns] = ss1.str();
+    }
+  }
+
+  void read_gt_data_pose(const std::string &path) {
+    data->gt_timestamps.clear();
+    data->gt_pose_data.clear();
+
+    int i = 0;
+
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      Eigen::Matrix3d rot;
+      Eigen::Vector3d pos;
+
+      ss >> rot(0, 0) >> rot(0, 1) >> rot(0, 2) >> pos[0] >> rot(1, 0) >>
+          rot(1, 1) >> rot(1, 2) >> pos[1] >> rot(2, 0) >> rot(2, 1) >>
+          rot(2, 2) >> pos[2];
+
+      data->gt_timestamps.emplace_back(data->image_timestamps[i]);
+      data->gt_pose_data.emplace_back(Eigen::Quaterniond(rot), pos);
+      i++;
+    }
+  }
+
+  std::shared_ptr<KittiVioDataset> data;
+};
+
+}  // namespace basalt

include/basalt/io/dataset_io_rosbag.h

@@ -0,0 +1,406 @@
+/**
+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/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+
+#include <basalt/utils/filesystem.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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_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::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_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;
+  }
+
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    cv::Mat mat = cv::Mat(1, 10, CV_32F, 1);
+    return mat;
+  }
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class RosbagIO;
+};
+
+class RosbagIO : public DatasetIoInterface {
+ public:
+  RosbagIO() {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new RosbagVioDataset);
+
+    data->bag.reset(new rosbag::Bag);
+    data->bag->open(path, rosbag::bagmode::Read);
+
+    rosbag::View view(*data->bag);
+
+    // 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") &&
+                 info->topic.rfind("/fcu", 0) != 0) {
+        imu_topic = info->topic;
+      } else if (info->datatype ==
+                     std::string("geometry_msgs/TransformStamped") ||
+                 info->datatype == std::string("geometry_msgs/PoseStamped")) {
+        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 (const rosbag::MessageInstance &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>();
+
+        // Try different message type if instantiate did not work
+        if (!mocap_msg) {
+          geometry_msgs::PoseStampedConstPtr mocap_pose_msg =
+              m.instantiate<geometry_msgs::PoseStamped>();
+
+          geometry_msgs::TransformStampedPtr mocap_new_msg(
+              new geometry_msgs::TransformStamped);
+          mocap_new_msg->header = mocap_pose_msg->header;
+          mocap_new_msg->transform.rotation = mocap_pose_msg->pose.orientation;
+          mocap_new_msg->transform.translation.x =
+              mocap_pose_msg->pose.position.x;
+          mocap_new_msg->transform.translation.y =
+              mocap_pose_msg->pose.position.y;
+          mocap_new_msg->transform.translation.z =
+              mocap_pose_msg->pose.position.z;
+
+          mocap_msg = mocap_new_msg;
+        }
+
+        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;
+};
+
+}  // namespace basalt
+
+#endif  // DATASET_IO_ROSBAG_H

include/basalt/io/dataset_io_uzh.h

@@ -0,0 +1,316 @@
+/**
+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/utils/filesystem.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+class UzhVioDataset : public VioDataset {
+  size_t num_cams;
+
+  std::string path;
+
+  std::vector<int64_t> image_timestamps;
+  std::unordered_map<int64_t, std::string> left_image_path;
+  std::unordered_map<int64_t, std::string> right_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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_pose_data;  // TODO: change to eigen aligned
+
+  int64_t mocap_to_imu_offset_ns = 0;
+
+  std::vector<std::unordered_map<int64_t, double>> exposure_times;
+
+ public:
+  ~UzhVioDataset(){};
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_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);
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path =
+          path + "/" +
+          (i == 0 ? left_image_path.at(t_ns) : right_image_path.at(t_ns));
+
+      if (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
+
+        if (img.type() == CV_8UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else if (img.type() == CV_8UC3) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i * 3];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else if (img.type() == CV_16UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+          std::memcpy(res[i].img->ptr, img.ptr(),
+                      img.cols * img.rows * sizeof(uint16_t));
+
+        } else {
+          std::cerr << "img.fmt.bpp " << img.type() << std::endl;
+          std::abort();
+        }
+
+        auto exp_it = exposure_times[i].find(t_ns);
+        if (exp_it != exposure_times[i].end()) {
+          res[i].exposure = exp_it->second;
+        }
+      }
+    }
+
+    return res;
+  }
+
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    cv::Mat mat = cv::Mat(1, 10, CV_32F, 1);
+    return mat;
+  }
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class UzhIO;
+};
+
+class UzhIO : public DatasetIoInterface {
+ public:
+  UzhIO() {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new UzhVioDataset);
+
+    data->num_cams = 2;
+    data->path = path;
+
+    read_image_timestamps(path);
+
+    std::cout << "Loaded " << data->get_image_timestamps().size()
+              << " timestamps, " << data->left_image_path.size()
+              << " left images and " << data->right_image_path.size()
+              << std::endl;
+
+    //    {
+    //      int64_t t_ns = data->get_image_timestamps()[0];
+    //      std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " "
+    //                << data->right_image_path.at(t_ns) << std::endl;
+    //    }
+
+    read_imu_data(path + "/imu.txt");
+
+    std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs."
+              << std::endl;
+
+    if (fs::exists(path + "/groundtruth.txt")) {
+      read_gt_data_pose(path + "/groundtruth.txt");
+    }
+
+    data->exposure_times.resize(data->num_cams);
+  }
+
+  void reset() { data.reset(); }
+
+  VioDatasetPtr get_data() { return data; }
+
+ private:
+  void read_exposure(const std::string &path,
+                     std::unordered_map<int64_t, double> &exposure_data) {
+    exposure_data.clear();
+
+    std::ifstream f(path + "exposure.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      char tmp;
+      int64_t timestamp, exposure_int;
+      Eigen::Vector3d gyro, accel;
+
+      ss >> timestamp >> tmp >> exposure_int;
+
+      exposure_data[timestamp] = exposure_int * 1e-9;
+    }
+  }
+
+  void read_image_timestamps(const std::string &path) {
+    {
+      std::ifstream f(path + "/left_images.txt");
+      std::string line;
+      while (std::getline(f, line)) {
+        if (line[0] == '#') continue;
+        std::stringstream ss(line);
+        int tmp;
+        double t_s;
+        std::string path;
+        ss >> tmp >> t_s >> path;
+
+        int64_t t_ns = t_s * 1e9;
+
+        data->image_timestamps.emplace_back(t_ns);
+        data->left_image_path[t_ns] = path;
+      }
+    }
+
+    {
+      std::ifstream f(path + "/right_images.txt");
+      std::string line;
+      while (std::getline(f, line)) {
+        if (line[0] == '#') continue;
+        std::stringstream ss(line);
+        int tmp;
+        double t_s;
+        std::string path;
+        ss >> tmp >> t_s >> path;
+
+        int64_t t_ns = t_s * 1e9;
+
+        data->right_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);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      int tmp;
+      double timestamp;
+      Eigen::Vector3d gyro, accel;
+
+      ss >> tmp >> timestamp >> gyro[0] >> gyro[1] >> gyro[2] >> accel[0] >>
+          accel[1] >> accel[2];
+
+      int64_t t_ns = timestamp * 1e9;
+
+      data->accel_data.emplace_back();
+      data->accel_data.back().timestamp_ns = t_ns;
+      data->accel_data.back().data = accel;
+
+      data->gyro_data.emplace_back();
+      data->gyro_data.back().timestamp_ns = t_ns;
+      data->gyro_data.back().data = gyro;
+    }
+  }
+
+  void read_gt_data_pose(const std::string &path) {
+    data->gt_timestamps.clear();
+    data->gt_pose_data.clear();
+
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      int tmp;
+      double timestamp;
+      Eigen::Quaterniond q;
+      Eigen::Vector3d pos;
+
+      ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >>
+          q.z() >> q.w();
+
+      int64_t t_ns = timestamp * 1e9;
+
+      data->gt_timestamps.emplace_back(t_ns);
+      data->gt_pose_data.emplace_back(q, pos);
+    }
+  }
+
+  std::shared_ptr<UzhVioDataset> data;
+};
+
+}  // namespace basalt

include/basalt/io/marg_data_io.h

@@ -0,0 +1,78 @@
+/**
+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() {
+    if (processing_thread) processing_thread->join();
+  }
+
+  tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue;
+
+ private:
+  std::shared_ptr<std::thread> processing_thread;
+};
+}  // namespace basalt

include/basalt/linearization/block_diagonal.hpp

@@ -0,0 +1,89 @@
+#pragma once
+
+#include <unordered_map>
+
+#include <Eigen/Dense>
+
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+// TODO: expand IndexedBlocks to small class / struct that also holds info on
+// block size and number of blocks, so we don't have to pass it around all the
+// time and we can directly implement things link adding diagonal and matrix
+// vector products in this sparse block diagonal matrix.
+
+// map of camera index to block; used to represent sparse block diagonal matrix
+template <typename Scalar>
+using IndexedBlocks =
+    std::unordered_map<size_t,
+                       Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>;
+
+// scale dimensions of JTJ as you would do for jacobian scaling of J beforehand,
+// with diagonal scaling matrix D: For jacobain we would use JD, so for JTJ we
+// use DJTJD.
+template <typename Scalar>
+void scale_jacobians(
+    IndexedBlocks<Scalar>& block_diagonal, size_t num_blocks, size_t block_size,
+    const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& scaling_vector) {
+  BASALT_ASSERT(num_blocks * block_size ==
+                unsigned_cast(scaling_vector.size()));
+  for (auto& [cam_idx, block] : block_diagonal) {
+    auto D =
+        scaling_vector.segment(block_size * cam_idx, block_size).asDiagonal();
+    block = D * block * D;
+  }
+}
+
+// add diagonal to block-diagonal matrix; missing blocks are assumed to be 0
+template <typename Scalar>
+void add_diagonal(IndexedBlocks<Scalar>& block_diagonal, size_t num_blocks,
+                  size_t block_size,
+                  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& diagonal) {
+  BASALT_ASSERT(num_blocks * block_size == unsigned_cast(diagonal.size()));
+  for (size_t idx = 0; idx < num_blocks; ++idx) {
+    auto [it, is_new] = block_diagonal.try_emplace(idx);
+    if (is_new) {
+      it->second = diagonal.segment(block_size * idx, block_size).asDiagonal();
+    } else {
+      it->second += diagonal.segment(block_size * idx, block_size).asDiagonal();
+    }
+  }
+}
+
+// sum up diagonal blocks in hash map for parallel reduction
+template <typename Scalar>
+class BlockDiagonalAccumulator {
+ public:
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+  inline void add(size_t idx, MatX&& block) {
+    auto [it, is_new] = block_diagonal_.try_emplace(idx);
+    if (is_new) {
+      it->second = std::move(block);
+    } else {
+      it->second += block;
+    }
+  }
+
+  inline void add_diag(size_t num_blocks, size_t block_size,
+                       const VecX& diagonal) {
+    add_diagonal(block_diagonal_, num_blocks, block_size, diagonal);
+  }
+
+  inline void join(BlockDiagonalAccumulator& b) {
+    for (auto& [k, v] : b.block_diagonal_) {
+      auto [it, is_new] = block_diagonal_.try_emplace(k);
+      if (is_new) {
+        it->second = std::move(v);
+      } else {
+        it->second += v;
+      }
+    }
+  }
+
+  IndexedBlocks<Scalar> block_diagonal_;
+};
+
+}  // namespace basalt

include/basalt/linearization/imu_block.hpp

@@ -0,0 +1,235 @@
+#pragma once
+
+#include <basalt/imu/preintegration.h>
+#include <basalt/optimization/accumulator.h>
+#include <basalt/utils/imu_types.h>
+
+namespace basalt {
+
+template <class Scalar_>
+class ImuBlock {
+ public:
+  using Scalar = Scalar_;
+
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+  ImuBlock(const IntegratedImuMeasurement<Scalar>* meas,
+           const ImuLinData<Scalar>* imu_lin_data, const AbsOrderMap& aom)
+      : imu_meas(meas), imu_lin_data(imu_lin_data), aom(aom) {
+    Jp.resize(POSE_VEL_BIAS_SIZE, 2 * POSE_VEL_BIAS_SIZE);
+    r.resize(POSE_VEL_BIAS_SIZE);
+  }
+
+  Scalar linearizeImu(
+      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
+          frame_states) {
+    Jp.setZero();
+    r.setZero();
+
+    const int64_t start_t = imu_meas->get_start_t_ns();
+    const int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
+
+    const size_t start_idx = 0;
+    const size_t end_idx = POSE_VEL_BIAS_SIZE;
+
+    PoseVelBiasStateWithLin<Scalar> start_state = frame_states.at(start_t);
+    PoseVelBiasStateWithLin<Scalar> end_state = frame_states.at(end_t);
+
+    typename IntegratedImuMeasurement<Scalar>::MatNN d_res_d_start, d_res_d_end;
+    typename IntegratedImuMeasurement<Scalar>::MatN3 d_res_d_bg, d_res_d_ba;
+
+    typename PoseVelState<Scalar>::VecN res = imu_meas->residual(
+        start_state.getStateLin(), imu_lin_data->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 = imu_meas->residual(
+          start_state.getState(), imu_lin_data->g, end_state.getState(),
+          start_state.getState().bias_gyro, start_state.getState().bias_accel);
+    }
+
+    // error
+    Scalar imu_error =
+        Scalar(0.5) * (imu_meas->get_sqrt_cov_inv() * res).squaredNorm();
+
+    // imu residual linearization
+    Jp.template block<9, 9>(0, start_idx) =
+        imu_meas->get_sqrt_cov_inv() * d_res_d_start;
+    Jp.template block<9, 9>(0, end_idx) =
+        imu_meas->get_sqrt_cov_inv() * d_res_d_end;
+
+    Jp.template block<9, 3>(0, start_idx + 9) =
+        imu_meas->get_sqrt_cov_inv() * d_res_d_bg;
+    Jp.template block<9, 3>(0, start_idx + 12) =
+        imu_meas->get_sqrt_cov_inv() * d_res_d_ba;
+
+    r.template segment<9>(0) = imu_meas->get_sqrt_cov_inv() * res;
+
+    // difference between biases
+    Scalar dt = imu_meas->get_dt_ns() * Scalar(1e-9);
+
+    Vec3 gyro_bias_weight_dt =
+        imu_lin_data->gyro_bias_weight_sqrt / std::sqrt(dt);
+    Vec3 res_bg =
+        start_state.getState().bias_gyro - end_state.getState().bias_gyro;
+
+    Jp.template block<3, 3>(9, start_idx + 9) =
+        gyro_bias_weight_dt.asDiagonal();
+    Jp.template block<3, 3>(9, end_idx + 9) =
+        (-gyro_bias_weight_dt).asDiagonal();
+
+    r.template segment<3>(9) += gyro_bias_weight_dt.asDiagonal() * res_bg;
+
+    Scalar bg_error =
+        Scalar(0.5) * (gyro_bias_weight_dt.asDiagonal() * res_bg).squaredNorm();
+
+    Vec3 accel_bias_weight_dt =
+        imu_lin_data->accel_bias_weight_sqrt / std::sqrt(dt);
+    Vec3 res_ba =
+        start_state.getState().bias_accel - end_state.getState().bias_accel;
+
+    Jp.template block<3, 3>(12, start_idx + 12) =
+        accel_bias_weight_dt.asDiagonal();
+    Jp.template block<3, 3>(12, end_idx + 12) =
+        (-accel_bias_weight_dt).asDiagonal();
+
+    r.template segment<3>(12) += accel_bias_weight_dt.asDiagonal() * res_ba;
+
+    Scalar ba_error =
+        Scalar(0.5) *
+        (accel_bias_weight_dt.asDiagonal() * res_ba).squaredNorm();
+
+    return imu_error + bg_error + ba_error;
+  }
+
+  void add_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r, size_t row_start_idx) const {
+    int64_t start_t = imu_meas->get_start_t_ns();
+    int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
+
+    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;
+
+    Q2Jp.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(row_start_idx,
+                                                                start_idx) +=
+        Jp.template topLeftCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>();
+
+    Q2Jp.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(row_start_idx,
+                                                                end_idx) +=
+        Jp.template topRightCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>();
+
+    Q2r.template segment<POSE_VEL_BIAS_SIZE>(row_start_idx) += r;
+  }
+
+  void add_dense_H_b(DenseAccumulator<Scalar>& accum) const {
+    int64_t start_t = imu_meas->get_start_t_ns();
+    int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
+
+    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;
+
+    const MatX H = Jp.transpose() * Jp;
+    const VecX b = Jp.transpose() * r;
+
+    accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+        start_idx, start_idx,
+        H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(0, 0));
+
+    accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+        end_idx, start_idx,
+        H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+            POSE_VEL_BIAS_SIZE, 0));
+
+    accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+        start_idx, end_idx,
+        H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+            0, POSE_VEL_BIAS_SIZE));
+
+    accum.template addH<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+        end_idx, end_idx,
+        H.template block<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>(
+            POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE));
+
+    accum.template addB<POSE_VEL_BIAS_SIZE>(
+        start_idx, b.template segment<POSE_VEL_BIAS_SIZE>(0));
+    accum.template addB<POSE_VEL_BIAS_SIZE>(
+        end_idx, b.template segment<POSE_VEL_BIAS_SIZE>(POSE_VEL_BIAS_SIZE));
+  }
+
+  void scaleJp_cols(const VecX& jacobian_scaling) {
+    int64_t start_t = imu_meas->get_start_t_ns();
+    int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
+
+    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;
+
+    Jp.template topLeftCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>() *=
+        jacobian_scaling.template segment<POSE_VEL_BIAS_SIZE>(start_idx)
+            .asDiagonal();
+
+    Jp.template topRightCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>() *=
+        jacobian_scaling.template segment<POSE_VEL_BIAS_SIZE>(end_idx)
+            .asDiagonal();
+  }
+
+  void addJp_diag2(VecX& res) const {
+    int64_t start_t = imu_meas->get_start_t_ns();
+    int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
+
+    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;
+
+    res.template segment<POSE_VEL_BIAS_SIZE>(start_idx) +=
+        Jp.template topLeftCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>()
+            .colwise()
+            .squaredNorm();
+
+    res.template segment<POSE_VEL_BIAS_SIZE>(end_idx) +=
+        Jp.template topRightCorner<POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE>()
+            .colwise()
+            .squaredNorm();
+  }
+
+  void backSubstitute(const VecX& pose_inc, Scalar& l_diff) {
+    int64_t start_t = imu_meas->get_start_t_ns();
+    int64_t end_t = imu_meas->get_start_t_ns() + imu_meas->get_dt_ns();
+
+    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;
+
+    VecX pose_inc_reduced(2 * POSE_VEL_BIAS_SIZE);
+    pose_inc_reduced.template head<POSE_VEL_BIAS_SIZE>() =
+        pose_inc.template segment<POSE_VEL_BIAS_SIZE>(start_idx);
+    pose_inc_reduced.template tail<POSE_VEL_BIAS_SIZE>() =
+        pose_inc.template segment<POSE_VEL_BIAS_SIZE>(end_idx);
+
+    // We want to compute the model cost change. The model function is
+    //
+    //     L(inc) = F(x) + incT JT r + 0.5 incT JT J inc
+    //
+    // and thus the expect decrease in cost for the computed increment is
+    //
+    //     l_diff = L(0) - L(inc)
+    //            = - incT JT r - 0.5 incT JT J inc.
+    //            = - incT JT (r + 0.5 J inc)
+    //            = - (J inc)T (r + 0.5 (J inc))
+
+    VecX Jinc = Jp * pose_inc_reduced;
+    l_diff -= Jinc.transpose() * (Scalar(0.5) * Jinc + r);
+  }
+
+ protected:
+  std::array<FrameId, 2> frame_ids;
+  MatX Jp;
+  VecX r;
+
+  const IntegratedImuMeasurement<Scalar>* imu_meas;
+  const ImuLinData<Scalar>* imu_lin_data;
+  const AbsOrderMap& aom;
+};  // namespace basalt
+
+}  // namespace basalt

include/basalt/linearization/landmark_block.hpp

@@ -0,0 +1,140 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/vi_estimator/landmark_database.h>
+#include <basalt/linearization/block_diagonal.hpp>
+
+namespace basalt {
+
+template <class Scalar>
+struct RelPoseLin {
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
+
+  Mat4 T_t_h;
+  Mat6 d_rel_d_h;
+  Mat6 d_rel_d_t;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <typename Scalar>
+class LandmarkBlock {
+ public:
+  struct Options {
+    // use Householder instead of Givens for marginalization
+    bool use_householder = true;
+
+    // use_valid_projections_only: if true, set invalid projection's
+    // residual and jacobian to 0; invalid means z <= 0
+    bool use_valid_projections_only = true;
+
+    // if > 0, use huber norm with given threshold, else squared norm
+    Scalar huber_parameter = 0;
+
+    // Standard deviation of reprojection error to weight visual measurements
+    Scalar obs_std_dev = 1;
+
+    // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
+    // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
+    Scalar jacobi_scaling_eps = 1e-6;
+  };
+
+  enum State {
+    Uninitialized = 0,
+    Allocated,
+    NumericalFailure,
+    Linearized,
+    Marginalized
+  };
+
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using RowMatX =
+      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+
+  using RowMat3 = Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>;
+
+  virtual ~LandmarkBlock(){};
+
+  virtual bool isNumericalFailure() const = 0;
+  virtual void allocateLandmark(
+      Keypoint<Scalar>& lm,
+      const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
+                                         RelPoseLin<Scalar>>& relative_pose_lin,
+      const Calibration<Scalar>& calib, const AbsOrderMap& aom,
+      const Options& options,
+      const std::map<TimeCamId, size_t>* rel_order = nullptr) = 0;
+
+  // may set state to NumericalFailure --> linearization at this state is
+  // unusable. Numeric check is only performed for residuals that were
+  // considered to be used (valid), which depends on
+  // use_valid_projections_only setting.
+  virtual Scalar linearizeLandmark() = 0;
+  virtual void performQR() = 0;
+
+  // Sets damping and maintains upper triangular matrix for landmarks.
+  virtual void setLandmarkDamping(Scalar lambda) = 0;
+
+  // lambda < 0 means computing exact model cost change
+  virtual void backSubstitute(const VecX& pose_inc, Scalar& l_diff) = 0;
+
+  virtual void addQ2JpTQ2Jp_mult_x(VecX& res, const VecX& x_pose) const = 0;
+
+  virtual void addQ2JpTQ2r(VecX& res) const = 0;
+
+  virtual void addJp_diag2(VecX& res) const = 0;
+
+  virtual void addQ2JpTQ2Jp_blockdiag(
+      BlockDiagonalAccumulator<Scalar>& accu) const = 0;
+
+  virtual void scaleJl_cols() = 0;
+  virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0;
+  virtual void printStorage(const std::string& filename) const = 0;
+  virtual State getState() const = 0;
+
+  virtual size_t numReducedCams() const = 0;
+
+  virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
+                                  size_t start_idx) const = 0;
+
+  virtual void get_dense_Q2Jp_Q2r_rel(
+      MatX& Q2Jp, VecX& Q2r, size_t start_idx,
+      const std::map<TimeCamId, size_t>& rel_order) const = 0;
+
+  virtual void add_dense_H_b(DenseAccumulator<Scalar>& accum) const = 0;
+
+  virtual void add_dense_H_b(MatX& H, VecX& b) const = 0;
+
+  virtual void add_dense_H_b_rel(
+      MatX& H_rel, VecX& b_rel,
+      const std::map<TimeCamId, size_t>& rel_order) const = 0;
+
+  virtual const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
+      const = 0;
+
+  virtual Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
+      const std::map<TimeCamId, size_t>& rel_order) const = 0;
+
+  virtual void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const = 0;
+
+  virtual TimeCamId getHostKf() const = 0;
+
+  virtual size_t numQ2rows() const = 0;
+
+  // factory method
+  template <int POSE_SIZE>
+  static std::unique_ptr<LandmarkBlock<Scalar>> createLandmarkBlock();
+};
+
+}  // namespace basalt

include/basalt/linearization/landmark_block_abs_dynamic.hpp

@@ -0,0 +1,585 @@
+#pragma once
+
+#include <fstream>
+#include <mutex>
+
+#include <basalt/utils/ba_utils.h>
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/utils/sophus_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+class LandmarkBlockAbsDynamic : public LandmarkBlock<Scalar> {
+ public:
+  using Options = typename LandmarkBlock<Scalar>::Options;
+  using State = typename LandmarkBlock<Scalar>::State;
+
+  inline bool isNumericalFailure() const override {
+    return state == State::NumericalFailure;
+  }
+
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using RowMatX =
+      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+
+  virtual inline void allocateLandmark(
+      Keypoint<Scalar>& lm,
+      const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
+                                         RelPoseLin<Scalar>>& relative_pose_lin,
+      const Calibration<Scalar>& calib, const AbsOrderMap& aom,
+      const Options& options,
+      const std::map<TimeCamId, size_t>* rel_order = nullptr) override {
+    // some of the logic assumes the members are at their initial values
+    BASALT_ASSERT(state == State::Uninitialized);
+
+    UNUSED(rel_order);
+
+    lm_ptr = &lm;
+    options_ = &options;
+    calib_ = &calib;
+
+    // TODO: consider for VIO that we have a lot of 0 columns if we just use aom
+    // --> add option to use different AOM with reduced size and/or just
+    // involved poses --> when accumulating results, check which case we have;
+    // if both aom are identical, we don't have to do block-wise operations.
+    aom_ = &aom;
+
+    pose_lin_vec.clear();
+    pose_lin_vec.reserve(lm.obs.size());
+    pose_tcid_vec.clear();
+    pose_tcid_vec.reserve(lm.obs.size());
+
+    // LMBs without host frame should not be created
+    BASALT_ASSERT(aom.abs_order_map.count(lm.host_kf_id.frame_id) > 0);
+
+    for (const auto& [tcid_t, pos] : lm.obs) {
+      size_t i = pose_lin_vec.size();
+
+      auto it = relative_pose_lin.find(std::make_pair(lm.host_kf_id, tcid_t));
+      BASALT_ASSERT(it != relative_pose_lin.end());
+
+      if (aom.abs_order_map.count(tcid_t.frame_id) > 0) {
+        pose_lin_vec.push_back(&it->second);
+      } else {
+        // Observation droped for marginalization
+        pose_lin_vec.push_back(nullptr);
+      }
+      pose_tcid_vec.push_back(&it->first);
+
+      res_idx_by_abs_pose_[it->first.first.frame_id].insert(i);   // host
+      res_idx_by_abs_pose_[it->first.second.frame_id].insert(i);  // target
+    }
+
+    // number of pose-jacobian columns is determined by oam
+    padding_idx = aom_->total_size;
+
+    num_rows = pose_lin_vec.size() * 2 + 3;  // residuals and lm damping
+
+    size_t pad = padding_idx % 4;
+    if (pad != 0) {
+      padding_size = 4 - pad;
+    }
+
+    lm_idx = padding_idx + padding_size;
+    res_idx = lm_idx + 3;
+    num_cols = res_idx + 1;
+
+    // number of columns should now be multiple of 4 for good memory alignment
+    // TODO: test extending this to 8 --> 32byte alignment for float?
+    BASALT_ASSERT(num_cols % 4 == 0);
+
+    storage.resize(num_rows, num_cols);
+
+    damping_rotations.clear();
+    damping_rotations.reserve(6);
+
+    state = State::Allocated;
+  }
+
+  // may set state to NumericalFailure --> linearization at this state is
+  // unusable. Numeric check is only performed for residuals that were
+  // considered to be used (valid), which depends on
+  // use_valid_projections_only setting.
+  virtual inline Scalar linearizeLandmark() override {
+    BASALT_ASSERT(state == State::Allocated ||
+                  state == State::NumericalFailure ||
+                  state == State::Linearized || state == State::Marginalized);
+
+    // storage.setZero(num_rows, num_cols);
+    storage.setZero();
+    damping_rotations.clear();
+    damping_rotations.reserve(6);
+
+    bool numerically_valid = true;
+
+    Scalar error_sum = 0;
+
+    size_t i = 0;
+    for (const auto& [tcid_t, obs] : lm_ptr->obs) {
+      std::visit(
+          [&, obs = obs](const auto& cam) {
+            // TODO: The pose_lin_vec[i] == nullptr is intended to deal with
+            // dropped measurements during marginalization. However, dropped
+            // measurements should only occur for the remaining frames, not for
+            // the marginalized frames. Maybe these are observations bewtween
+            // two marginalized frames, if more than one is marginalized at the
+            // same time? But those we would not have to drop... Double check if
+            // and when this happens and possibly resolve by fixing handling
+            // here, or else updating the measurements in lmdb before calling
+            // linearization. Otherwise, check where else we need a `if
+            // (pose_lin_vec[i])` check or `pose_lin_vec[i] != nullptr` assert
+            // in this class.
+
+            if (pose_lin_vec[i]) {
+              size_t obs_idx = i * 2;
+              size_t abs_h_idx =
+                  aom_->abs_order_map.at(pose_tcid_vec[i]->first.frame_id)
+                      .first;
+              size_t abs_t_idx =
+                  aom_->abs_order_map.at(pose_tcid_vec[i]->second.frame_id)
+                      .first;
+
+              Vec2 res;
+              Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
+              Eigen::Matrix<Scalar, 2, 3> d_res_d_p;
+
+              using CamT = std::decay_t<decltype(cam)>;
+              bool valid = linearizePoint<Scalar, CamT>(
+                  obs, *lm_ptr, pose_lin_vec[i]->T_t_h, cam, res, &d_res_d_xi,
+                  &d_res_d_p);
+
+              if (!options_->use_valid_projections_only || valid) {
+                numerically_valid = numerically_valid &&
+                                    d_res_d_xi.array().isFinite().all() &&
+                                    d_res_d_p.array().isFinite().all();
+
+                const Scalar res_squared = res.squaredNorm();
+                const auto [weighted_error, weight] =
+                    compute_error_weight(res_squared);
+                const Scalar sqrt_weight =
+                    std::sqrt(weight) / options_->obs_std_dev;
+
+                error_sum += weighted_error /
+                             (options_->obs_std_dev * options_->obs_std_dev);
+
+                storage.template block<2, 3>(obs_idx, lm_idx) =
+                    sqrt_weight * d_res_d_p;
+                storage.template block<2, 1>(obs_idx, res_idx) =
+                    sqrt_weight * res;
+
+                d_res_d_xi *= sqrt_weight;
+                storage.template block<2, 6>(obs_idx, abs_h_idx) +=
+                    d_res_d_xi * pose_lin_vec[i]->d_rel_d_h;
+                storage.template block<2, 6>(obs_idx, abs_t_idx) +=
+                    d_res_d_xi * pose_lin_vec[i]->d_rel_d_t;
+              }
+            }
+
+            i++;
+          },
+          calib_->intrinsics[tcid_t.cam_id].variant);
+    }
+
+    if (numerically_valid) {
+      state = State::Linearized;
+    } else {
+      state = State::NumericalFailure;
+    }
+
+    return error_sum;
+  }
+
+  virtual inline void performQR() override {
+    BASALT_ASSERT(state == State::Linearized);
+
+    // Since we use dense matrices Householder QR might be better:
+    // https://mathoverflow.net/questions/227543/why-householder-reflection-is-better-than-givens-rotation-in-dense-linear-algebr
+
+    if (options_->use_householder) {
+      performQRHouseholder();
+    } else {
+      performQRGivens();
+    }
+
+    state = State::Marginalized;
+  }
+
+  // Sets damping and maintains upper triangular matrix for landmarks.
+  virtual inline void setLandmarkDamping(Scalar lambda) override {
+    BASALT_ASSERT(state == State::Marginalized);
+    BASALT_ASSERT(lambda >= 0);
+
+    if (hasLandmarkDamping()) {
+      BASALT_ASSERT(damping_rotations.size() == 6);
+
+      // undo dampening
+      for (int n = 2; n >= 0; n--) {
+        for (int m = n; m >= 0; m--) {
+          storage.applyOnTheLeft(num_rows - 3 + n - m, n,
+                                 damping_rotations.back().adjoint());
+          damping_rotations.pop_back();
+        }
+      }
+    }
+
+    if (lambda == 0) {
+      storage.template block<3, 3>(num_rows - 3, lm_idx).diagonal().setZero();
+    } else {
+      BASALT_ASSERT(Jl_col_scale.array().isFinite().all());
+
+      storage.template block<3, 3>(num_rows - 3, lm_idx)
+          .diagonal()
+          .setConstant(sqrt(lambda));
+
+      BASALT_ASSERT(damping_rotations.empty());
+
+      // apply dampening and remember rotations to undo
+      for (int n = 0; n < 3; n++) {
+        for (int m = 0; m <= n; m++) {
+          damping_rotations.emplace_back();
+          damping_rotations.back().makeGivens(
+              storage(n, lm_idx + n),
+              storage(num_rows - 3 + n - m, lm_idx + n));
+          storage.applyOnTheLeft(num_rows - 3 + n - m, n,
+                                 damping_rotations.back());
+        }
+      }
+    }
+  }
+
+  // lambda < 0 means computing exact model cost change
+  virtual inline void backSubstitute(const VecX& pose_inc,
+                                     Scalar& l_diff) override {
+    BASALT_ASSERT(state == State::Marginalized);
+
+    // For now we include all columns in LMB
+    BASALT_ASSERT(pose_inc.size() == signed_cast(padding_idx));
+
+    const auto Q1Jl = storage.template block<3, 3>(0, lm_idx)
+                          .template triangularView<Eigen::Upper>();
+
+    const auto Q1Jr = storage.col(res_idx).template head<3>();
+    const auto Q1Jp = storage.topLeftCorner(3, padding_idx);
+
+    Vec3 inc = -Q1Jl.solve(Q1Jr + Q1Jp * pose_inc);
+
+    // We want to compute the model cost change. The model function is
+    //
+    //     L(inc) = F(x) + inc^T J^T r + 0.5 inc^T J^T J inc
+    //
+    // and thus the expected decrease in cost for the computed increment is
+    //
+    //     l_diff = L(0) - L(inc)
+    //            = - inc^T J^T r - 0.5 inc^T J^T J inc
+    //            = - inc^T J^T (r + 0.5 J inc)
+    //            = - (J inc)^T (r + 0.5 (J inc)).
+    //
+    // Here we have J = [Jp, Jl] under the orthogonal projection Q = [Q1, Q2],
+    // i.e. the linearized system (model cost) is
+    //
+    //    L(inc) = 0.5 || J inc + r ||^2 = 0.5 || Q^T J inc + Q^T r ||^2
+    //
+    // and below we thus compute
+    //
+    //    l_diff = - (Q^T J inc)^T (Q^T r + 0.5 (Q^T J inc)).
+    //
+    // We have
+    //             | Q1^T |            | Q1^T Jp   Q1^T Jl |
+    //    Q^T J =  |      | [Jp, Jl] = |                   |
+    //             | Q2^T |            | Q2^T Jp      0    |.
+    //
+    // Note that Q2 is the nullspace of Jl, and Q1^T Jl == R. So with inc =
+    // [incp^T, incl^T]^T we have
+    //
+    //                | Q1^T Jp incp + Q1^T Jl incl |
+    //    Q^T J inc = |                             |
+    //                | Q2^T Jp incp                |
+    //
+
+    // undo damping before we compute the model cost difference
+    setLandmarkDamping(0);
+
+    // compute "Q^T J incp"
+    VecX QJinc = storage.topLeftCorner(num_rows - 3, padding_idx) * pose_inc;
+
+    // add "Q1^T Jl incl" to the first 3 rows
+    QJinc.template head<3>() += Q1Jl * inc;
+
+    auto Qr = storage.col(res_idx).head(num_rows - 3);
+    l_diff -= QJinc.transpose() * (Scalar(0.5) * QJinc + Qr);
+
+    // TODO: detect and handle case like ceres, allowing a few iterations but
+    // stopping eventually
+    if (!inc.array().isFinite().all() ||
+        !lm_ptr->direction.array().isFinite().all() ||
+        !std::isfinite(lm_ptr->inv_dist)) {
+      std::cerr << "Numerical failure in backsubstitution\n";
+    }
+
+    // Note: scale only after computing model cost change
+    inc.array() *= Jl_col_scale.array();
+
+    lm_ptr->direction += inc.template head<2>();
+    lm_ptr->inv_dist = std::max(Scalar(0), lm_ptr->inv_dist + inc[2]);
+  }
+
+  virtual inline size_t numReducedCams() const override {
+    BASALT_LOG_FATAL("check what we mean by numReducedCams for absolute poses");
+    return pose_lin_vec.size();
+  }
+
+  inline void addQ2JpTQ2Jp_mult_x(VecX& res,
+                                  const VecX& x_pose) const override {
+    UNUSED(res);
+    UNUSED(x_pose);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  virtual inline void addQ2JpTQ2r(VecX& res) const override {
+    UNUSED(res);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  virtual inline void addJp_diag2(VecX& res) const override {
+    BASALT_ASSERT(state == State::Linearized);
+
+    for (const auto& [frame_id, idx_set] : res_idx_by_abs_pose_) {
+      const int pose_idx = aom_->abs_order_map.at(frame_id).first;
+      for (const int i : idx_set) {
+        const auto block = storage.block(2 * i, pose_idx, 2, POSE_SIZE);
+
+        res.template segment<POSE_SIZE>(pose_idx) +=
+            block.colwise().squaredNorm();
+      }
+    }
+  }
+
+  virtual inline void addQ2JpTQ2Jp_blockdiag(
+      BlockDiagonalAccumulator<Scalar>& accu) const override {
+    UNUSED(accu);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  virtual inline void scaleJl_cols() override {
+    BASALT_ASSERT(state == State::Linearized);
+
+    // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
+    // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
+    Jl_col_scale =
+        (options_->jacobi_scaling_eps +
+         storage.block(0, lm_idx, num_rows - 3, 3).colwise().norm().array())
+            .inverse();
+
+    storage.block(0, lm_idx, num_rows - 3, 3) *= Jl_col_scale.asDiagonal();
+  }
+
+  virtual inline void scaleJp_cols(const VecX& jacobian_scaling) override {
+    BASALT_ASSERT(state == State::Marginalized);
+
+    // we assume we apply scaling before damping (we exclude the last 3 rows)
+    BASALT_ASSERT(!hasLandmarkDamping());
+
+    storage.topLeftCorner(num_rows - 3, padding_idx) *=
+        jacobian_scaling.asDiagonal();
+  }
+
+  inline bool hasLandmarkDamping() const { return !damping_rotations.empty(); }
+
+  virtual inline void printStorage(const std::string& filename) const override {
+    std::ofstream f(filename);
+
+    Eigen::IOFormat CleanFmt(4, 0, " ", "\n", "", "");
+
+    f << "Storage (state: " << state
+      << ", damping: " << (hasLandmarkDamping() ? "yes" : "no")
+      << " Jl_col_scale: " << Jl_col_scale.transpose() << "):\n"
+      << storage.format(CleanFmt) << std::endl;
+
+    f.close();
+  }
+#if 0
+  virtual inline void stage2(
+      Scalar lambda, const VecX* jacobian_scaling, VecX* precond_diagonal2,
+      BlockDiagonalAccumulator<Scalar>* precond_block_diagonal,
+      VecX& bref) override {
+    // 1. scale jacobian
+    if (jacobian_scaling) {
+      scaleJp_cols(*jacobian_scaling);
+    }
+
+    // 2. dampen landmarks
+    setLandmarkDamping(lambda);
+
+    // 3a. compute diagonal preconditioner (SCHUR_JACOBI_DIAGONAL)
+    if (precond_diagonal2) {
+      addQ2Jp_diag2(*precond_diagonal2);
+    }
+
+    // 3b. compute block diagonal preconditioner (SCHUR_JACOBI)
+    if (precond_block_diagonal) {
+      addQ2JpTQ2Jp_blockdiag(*precond_block_diagonal);
+    }
+
+    // 4. compute rhs of reduced camera normal equations
+    addQ2JpTQ2r(bref);
+  }
+#endif
+
+  inline State getState() const override { return state; }
+
+  virtual inline size_t numQ2rows() const override { return num_rows - 3; }
+
+ protected:
+  inline void performQRGivens() {
+    // Based on "Matrix Computations 4th Edition by Golub and Van Loan"
+    // See page 252, Algorithm 5.2.4 for how these two loops work
+    Eigen::JacobiRotation<Scalar> gr;
+    for (size_t n = 0; n < 3; n++) {
+      for (size_t m = num_rows - 4; m > n; m--) {
+        gr.makeGivens(storage(m - 1, lm_idx + n), storage(m, lm_idx + n));
+        storage.applyOnTheLeft(m, m - 1, gr);
+      }
+    }
+  }
+
+  inline void performQRHouseholder() {
+    VecX tempVector1(num_cols);
+    VecX tempVector2(num_rows - 3);
+
+    for (size_t k = 0; k < 3; ++k) {
+      size_t remainingRows = num_rows - k - 3;
+
+      Scalar beta;
+      Scalar tau;
+      storage.col(lm_idx + k)
+          .segment(k, remainingRows)
+          .makeHouseholder(tempVector2, tau, beta);
+
+      storage.block(k, 0, remainingRows, num_cols)
+          .applyHouseholderOnTheLeft(tempVector2, tau, tempVector1.data());
+    }
+  }
+
+  inline std::tuple<Scalar, Scalar> compute_error_weight(
+      Scalar res_squared) const {
+    // Note: Definition of cost is 0.5 ||r(x)||^2 to be in line with ceres
+
+    if (options_->huber_parameter > 0) {
+      // use huber norm
+      const Scalar huber_weight =
+          res_squared <= options_->huber_parameter * options_->huber_parameter
+              ? Scalar(1)
+              : options_->huber_parameter / std::sqrt(res_squared);
+      const Scalar error =
+          Scalar(0.5) * (2 - huber_weight) * huber_weight * res_squared;
+      return {error, huber_weight};
+    } else {
+      // use squared norm
+      return {Scalar(0.5) * res_squared, Scalar(1)};
+    }
+  }
+
+  void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
+                          size_t start_idx) const override {
+    Q2r.segment(start_idx, num_rows - 3) =
+        storage.col(res_idx).tail(num_rows - 3);
+
+    BASALT_ASSERT(Q2Jp.cols() == signed_cast(padding_idx));
+
+    Q2Jp.block(start_idx, 0, num_rows - 3, padding_idx) =
+        storage.block(3, 0, num_rows - 3, padding_idx);
+  }
+
+  void get_dense_Q2Jp_Q2r_rel(
+      MatX& Q2Jp, VecX& Q2r, size_t start_idx,
+      const std::map<TimeCamId, size_t>& rel_order) const override {
+    UNUSED(Q2Jp);
+    UNUSED(Q2r);
+    UNUSED(start_idx);
+    UNUSED(rel_order);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  void add_dense_H_b(DenseAccumulator<Scalar>& accum) const override {
+    UNUSED(accum);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  void add_dense_H_b(MatX& H, VecX& b) const override {
+    const auto r = storage.col(res_idx).tail(num_rows - 3);
+    const auto J = storage.block(3, 0, num_rows - 3, padding_idx);
+
+    H.noalias() += J.transpose() * J;
+    b.noalias() += J.transpose() * r;
+  }
+
+  void add_dense_H_b_rel(
+      MatX& H_rel, VecX& b_rel,
+      const std::map<TimeCamId, size_t>& rel_order) const override {
+    UNUSED(H_rel);
+    UNUSED(b_rel);
+    UNUSED(rel_order);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
+      const override {
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
+      const std::map<TimeCamId, size_t>& rel_order) const override {
+    UNUSED(rel_order);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const override {
+    UNUSED(H_rel);
+    UNUSED(b_rel);
+    BASALT_LOG_FATAL("not implemented");
+  }
+
+  virtual TimeCamId getHostKf() const override { return lm_ptr->host_kf_id; }
+
+ private:
+  // Dense storage for pose Jacobians, padding, landmark Jacobians and
+  // residuals [J_p | pad | J_l | res]
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
+      storage;
+
+  Vec3 Jl_col_scale = Vec3::Ones();
+  std::vector<Eigen::JacobiRotation<Scalar>> damping_rotations;
+
+  std::vector<const RelPoseLin<Scalar>*> pose_lin_vec;
+  std::vector<const std::pair<TimeCamId, TimeCamId>*> pose_tcid_vec;
+  size_t padding_idx = 0;
+  size_t padding_size = 0;
+  size_t lm_idx = 0;
+  size_t res_idx = 0;
+
+  size_t num_cols = 0;
+  size_t num_rows = 0;
+
+  const Options* options_ = nullptr;
+
+  State state = State::Uninitialized;
+
+  Keypoint<Scalar>* lm_ptr = nullptr;
+  const Calibration<Scalar>* calib_ = nullptr;
+  const AbsOrderMap* aom_ = nullptr;
+
+  std::map<int64_t, std::set<int>> res_idx_by_abs_pose_;
+};
+
+}  // namespace basalt

include/basalt/linearization/linearization_abs_qr.hpp

@@ -0,0 +1,141 @@
+#pragma once
+
+#include <basalt/linearization/linearization_base.hpp>
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/vi_estimator/landmark_database.h>
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+#include <basalt/utils/time_utils.hpp>
+
+namespace basalt {
+
+template <class Scalar>
+class ImuBlock;
+
+template <typename Scalar_, int POSE_SIZE_>
+class LinearizationAbsQR : public LinearizationBase<Scalar_, POSE_SIZE_> {
+ public:
+  using Scalar = Scalar_;
+  static constexpr int POSE_SIZE = POSE_SIZE_;
+  using Base = LinearizationBase<Scalar, POSE_SIZE>;
+
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+  using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+  using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;
+  using ImuBlockPtr = std::unique_ptr<ImuBlock<Scalar>>;
+
+  using typename Base::Options;
+
+  LinearizationAbsQR(
+      BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+      const Options& options,
+      const MargLinData<Scalar>* marg_lin_data = nullptr,
+      const ImuLinData<Scalar>* imu_lin_data = nullptr,
+      const std::set<FrameId>* used_frames = nullptr,
+      const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
+      int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
+
+  // destructor defined in cpp b/c of unique_ptr destructor (ImuBlock)
+  // copy/move constructor/assignment-operator for rule-of-five
+  ~LinearizationAbsQR();
+  LinearizationAbsQR(const LinearizationAbsQR&) = default;
+  LinearizationAbsQR(LinearizationAbsQR&&) = default;
+  LinearizationAbsQR& operator=(const LinearizationAbsQR&) = default;
+  LinearizationAbsQR& operator=(LinearizationAbsQR&&) = default;
+
+  void log_problem_stats(ExecutionStats& stats) const override;
+
+  Scalar linearizeProblem(bool* numerically_valid = nullptr) override;
+
+  void performQR() override;
+
+  void setPoseDamping(const Scalar lambda);
+
+  bool hasPoseDamping() const { return pose_damping_diagonal > 0; }
+
+  Scalar backSubstitute(const VecX& pose_inc) override;
+
+  VecX getJp_diag2() const;
+
+  void scaleJl_cols();
+
+  void scaleJp_cols(const VecX& jacobian_scaling);
+
+  void setLandmarkDamping(Scalar lambda);
+
+  void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const override;
+
+  void get_dense_H_b(MatX& H, VecX& b) const override;
+
+ protected:  // types
+  using PoseLinMapType =
+      Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
+                                   RelPoseLin<Scalar>>;
+  using PoseLinMapTypeConstIter = typename PoseLinMapType::const_iterator;
+
+  using HostLandmarkMapType =
+      std::unordered_map<TimeCamId, std::vector<const LandmarkBlock<Scalar>*>>;
+
+ protected:  //  helper
+  void get_dense_Q2Jp_Q2r_pose_damping(MatX& Q2Jp, size_t start_idx) const;
+
+  void get_dense_Q2Jp_Q2r_marg_prior(MatX& Q2Jp, VecX& Q2r,
+                                     size_t start_idx) const;
+
+  void add_dense_H_b_pose_damping(MatX& H) const;
+
+  void add_dense_H_b_marg_prior(MatX& H, VecX& b) const;
+
+  void add_dense_H_b_imu(DenseAccumulator<Scalar>& accum) const;
+
+  void add_dense_H_b_imu(MatX& H, VecX& b) const;
+
+ protected:
+  Options options_;
+
+  std::vector<KeypointId> landmark_ids;
+  std::vector<LandmarkBlockPtr> landmark_blocks;
+  std::vector<ImuBlockPtr> imu_blocks;
+
+  std::unordered_map<TimeCamId, size_t> host_to_idx_;
+  HostLandmarkMapType host_to_landmark_block;
+
+  std::vector<size_t> landmark_block_idx;
+  const BundleAdjustmentBase<Scalar>* estimator;
+
+  LandmarkDatabase<Scalar>& lmdb_;
+  const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses;
+
+  const Calibration<Scalar>& calib;
+
+  const AbsOrderMap& aom;
+  const std::set<FrameId>* used_frames;
+
+  const MargLinData<Scalar>* marg_lin_data;
+  const ImuLinData<Scalar>* imu_lin_data;
+
+  PoseLinMapType relative_pose_lin;
+  std::vector<std::vector<PoseLinMapTypeConstIter>> relative_pose_per_host;
+
+  Scalar pose_damping_diagonal;
+  Scalar pose_damping_diagonal_sqrt;
+
+  VecX marg_scaling;
+
+  size_t num_cameras;
+  size_t num_rows_Q2r;
+};
+
+}  // namespace basalt

include/basalt/linearization/linearization_abs_sc.hpp

@@ -0,0 +1,142 @@
+#pragma once
+
+#include <basalt/linearization/linearization_base.hpp>
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/vi_estimator/landmark_database.h>
+#include <basalt/vi_estimator/sqrt_ba_base.h>
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+#include <basalt/utils/time_utils.hpp>
+
+namespace basalt {
+
+template <class Scalar>
+class ImuBlock;
+
+template <typename Scalar_, int POSE_SIZE_>
+class LinearizationAbsSC : public LinearizationBase<Scalar_, POSE_SIZE_> {
+ public:
+  using Scalar = Scalar_;
+  static constexpr int POSE_SIZE = POSE_SIZE_;
+  using Base = LinearizationBase<Scalar, POSE_SIZE>;
+
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+  using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+  using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;
+  using ImuBlockPtr = std::unique_ptr<ImuBlock<Scalar>>;
+
+  using AbsLinData = typename ScBundleAdjustmentBase<Scalar>::AbsLinData;
+
+  using typename Base::Options;
+
+  LinearizationAbsSC(
+      BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+      const Options& options,
+      const MargLinData<Scalar>* marg_lin_data = nullptr,
+      const ImuLinData<Scalar>* imu_lin_data = nullptr,
+      const std::set<FrameId>* used_frames = nullptr,
+      const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
+      int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
+
+  // destructor defined in cpp b/c of unique_ptr destructor (ImuBlock)
+  // copy/move constructor/assignment-operator for rule-of-five
+  ~LinearizationAbsSC();
+  LinearizationAbsSC(const LinearizationAbsSC&) = default;
+  LinearizationAbsSC(LinearizationAbsSC&&) = default;
+  LinearizationAbsSC& operator=(const LinearizationAbsSC&) = default;
+  LinearizationAbsSC& operator=(LinearizationAbsSC&&) = default;
+
+  void log_problem_stats(ExecutionStats& stats) const override;
+
+  Scalar linearizeProblem(bool* numerically_valid = nullptr) override;
+
+  void performQR() override;
+
+  void setPoseDamping(const Scalar lambda);
+
+  bool hasPoseDamping() const { return pose_damping_diagonal > 0; }
+
+  Scalar backSubstitute(const VecX& pose_inc) override;
+
+  VecX getJp_diag2() const;
+
+  void scaleJl_cols();
+
+  void scaleJp_cols(const VecX& jacobian_scaling);
+
+  void setLandmarkDamping(Scalar lambda);
+
+  void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const override;
+
+  void get_dense_H_b(MatX& H, VecX& b) const override;
+
+ protected:  // types
+  using PoseLinMapType =
+      Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
+                                   RelPoseLin<Scalar>>;
+  using PoseLinMapTypeConstIter = typename PoseLinMapType::const_iterator;
+
+  using HostLandmarkMapType =
+      std::unordered_map<TimeCamId, std::vector<const LandmarkBlock<Scalar>*>>;
+
+ protected:  //  helper
+  void get_dense_Q2Jp_Q2r_pose_damping(MatX& Q2Jp, size_t start_idx) const;
+
+  void get_dense_Q2Jp_Q2r_marg_prior(MatX& Q2Jp, VecX& Q2r,
+                                     size_t start_idx) const;
+
+  void add_dense_H_b_pose_damping(MatX& H) const;
+
+  void add_dense_H_b_marg_prior(MatX& H, VecX& b) const;
+
+  void add_dense_H_b_imu(DenseAccumulator<Scalar>& accum) const;
+
+  void add_dense_H_b_imu(MatX& H, VecX& b) const;
+
+  // Transform to abs
+  static void accumulate_dense_H_b_rel_to_abs(
+      const MatX& rel_H, const VecX& rel_b,
+      const std::vector<PoseLinMapTypeConstIter>& rpph, const AbsOrderMap& aom,
+      DenseAccumulator<Scalar>& accum);
+
+ protected:
+  Options options_;
+
+  std::vector<ImuBlockPtr> imu_blocks;
+
+  const BundleAdjustmentBase<Scalar>* estimator;
+
+  LandmarkDatabase<Scalar>& lmdb_;
+  const Calibration<Scalar>& calib;
+
+  const AbsOrderMap& aom;
+  const std::set<FrameId>* used_frames;
+
+  const MargLinData<Scalar>* marg_lin_data;
+  const ImuLinData<Scalar>* imu_lin_data;
+  const std::unordered_set<KeypointId>* lost_landmarks;
+  int64_t last_state_to_marg;
+
+  Eigen::aligned_vector<AbsLinData> ald_vec;
+
+  Scalar pose_damping_diagonal;
+  Scalar pose_damping_diagonal_sqrt;
+
+  VecX marg_scaling;
+
+  size_t num_rows_Q2r;
+};
+
+}  // namespace basalt

include/basalt/linearization/linearization_base.hpp

@@ -0,0 +1,63 @@
+#pragma once
+
+#include <Eigen/Dense>
+
+#include <basalt/vi_estimator/ba_base.h>
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/utils/time_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar_, int POSE_SIZE_>
+class LinearizationBase {
+ public:
+  using Scalar = Scalar_;
+  static constexpr int POSE_SIZE = POSE_SIZE_;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+  struct Options {
+    typename LandmarkBlock<Scalar>::Options lb_options;
+    LinearizationType linearization_type;
+  };
+
+  virtual ~LinearizationBase() = default;
+
+  virtual void log_problem_stats(ExecutionStats& stats) const = 0;
+
+  virtual Scalar linearizeProblem(bool* numerically_valid = nullptr) = 0;
+
+  virtual void performQR() = 0;
+
+  // virtual void setPoseDamping(const Scalar lambda) = 0;
+
+  // virtual bool hasPoseDamping() const = 0;
+
+  virtual Scalar backSubstitute(const VecX& pose_inc) = 0;
+
+  // virtual VecX getJp_diag2() const = 0;
+
+  // virtual void scaleJl_cols() = 0;
+
+  // virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0;
+
+  // virtual void setLandmarkDamping(Scalar lambda) = 0;
+
+  virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const = 0;
+
+  virtual void get_dense_H_b(MatX& H, VecX& b) const = 0;
+
+  static std::unique_ptr<LinearizationBase> create(
+      BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+      const Options& options,
+      const MargLinData<Scalar>* marg_lin_data = nullptr,
+      const ImuLinData<Scalar>* imu_lin_data = nullptr,
+      const std::set<FrameId>* used_frames = nullptr,
+      const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
+      int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
+};
+
+bool isLinearizationSqrt(const LinearizationType& type);
+
+}  // namespace basalt

include/basalt/linearization/linearization_rel_sc.hpp

@@ -0,0 +1,143 @@
+#pragma once
+
+#include <basalt/linearization/linearization_base.hpp>
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/vi_estimator/landmark_database.h>
+#include <basalt/vi_estimator/sqrt_ba_base.h>
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+#include <basalt/utils/time_utils.hpp>
+
+namespace basalt {
+
+template <class Scalar>
+class ImuBlock;
+
+template <typename Scalar_, int POSE_SIZE_>
+class LinearizationRelSC : public LinearizationBase<Scalar_, POSE_SIZE_> {
+ public:
+  using Scalar = Scalar_;
+  static constexpr int POSE_SIZE = POSE_SIZE_;
+  using Base = LinearizationBase<Scalar, POSE_SIZE>;
+
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+  using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+  using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;
+  using ImuBlockPtr = std::unique_ptr<ImuBlock<Scalar>>;
+
+  using RelLinData = typename ScBundleAdjustmentBase<Scalar>::RelLinData;
+
+  using typename Base::Options;
+
+  LinearizationRelSC(
+      BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+      const Options& options,
+      const MargLinData<Scalar>* marg_lin_data = nullptr,
+      const ImuLinData<Scalar>* imu_lin_data = nullptr,
+      const std::set<FrameId>* used_frames = nullptr,
+      const std::unordered_set<KeypointId>* lost_landmarks = nullptr,
+      int64_t last_state_to_marg = std::numeric_limits<int64_t>::max());
+
+  // destructor defined in cpp b/c of unique_ptr destructor (ImuBlock)
+  // copy/move constructor/assignment-operator for rule-of-five
+  ~LinearizationRelSC();
+  LinearizationRelSC(const LinearizationRelSC&) = default;
+  LinearizationRelSC(LinearizationRelSC&&) = default;
+  LinearizationRelSC& operator=(const LinearizationRelSC&) = default;
+  LinearizationRelSC& operator=(LinearizationRelSC&&) = default;
+
+  void log_problem_stats(ExecutionStats& stats) const override;
+
+  Scalar linearizeProblem(bool* numerically_valid = nullptr) override;
+
+  void performQR() override;
+
+  void setPoseDamping(const Scalar lambda);
+
+  bool hasPoseDamping() const { return pose_damping_diagonal > 0; }
+
+  Scalar backSubstitute(const VecX& pose_inc) override;
+
+  VecX getJp_diag2() const;
+
+  void scaleJl_cols();
+
+  void scaleJp_cols(const VecX& jacobian_scaling);
+
+  void setLandmarkDamping(Scalar lambda);
+
+  void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const override;
+
+  void get_dense_H_b(MatX& H, VecX& b) const override;
+
+ protected:  // types
+  using PoseLinMapType =
+      Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
+                                   RelPoseLin<Scalar>>;
+  using PoseLinMapTypeConstIter = typename PoseLinMapType::const_iterator;
+
+  using HostLandmarkMapType =
+      std::unordered_map<TimeCamId, std::vector<const LandmarkBlock<Scalar>*>>;
+
+ protected:  //  helper
+  void get_dense_Q2Jp_Q2r_pose_damping(MatX& Q2Jp, size_t start_idx) const;
+
+  void get_dense_Q2Jp_Q2r_marg_prior(MatX& Q2Jp, VecX& Q2r,
+                                     size_t start_idx) const;
+
+  void add_dense_H_b_pose_damping(MatX& H) const;
+
+  void add_dense_H_b_marg_prior(MatX& H, VecX& b) const;
+
+  void add_dense_H_b_imu(DenseAccumulator<Scalar>& accum) const;
+
+  void add_dense_H_b_imu(MatX& H, VecX& b) const;
+
+  // Transform to abs
+  static void accumulate_dense_H_b_rel_to_abs(
+      const MatX& rel_H, const VecX& rel_b,
+      const std::vector<PoseLinMapTypeConstIter>& rpph, const AbsOrderMap& aom,
+      DenseAccumulator<Scalar>& accum);
+
+ protected:
+  Options options_;
+
+  std::vector<ImuBlockPtr> imu_blocks;
+
+  const BundleAdjustmentBase<Scalar>* estimator;
+
+  LandmarkDatabase<Scalar>& lmdb_;
+
+  const Calibration<Scalar>& calib;
+
+  const AbsOrderMap& aom;
+  const std::set<FrameId>* used_frames;
+
+  const MargLinData<Scalar>* marg_lin_data;
+  const ImuLinData<Scalar>* imu_lin_data;
+  const std::unordered_set<KeypointId>* lost_landmarks;
+  int64_t last_state_to_marg;
+
+  Eigen::aligned_vector<RelLinData> rld_vec;
+
+  Scalar pose_damping_diagonal;
+  Scalar pose_damping_diagonal_sqrt;
+
+  VecX marg_scaling;
+
+  size_t num_rows_Q2r;
+};
+
+}  // namespace basalt

include/basalt/optical_flow/frame_to_frame_optical_flow.h

@@ -0,0 +1,405 @@
+/**
+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 <sophus/se2.hpp>
+
+#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/image/image_pyr.h>
+#include <basalt/utils/keypoints.h>
+
+namespace basalt {
+
+/// Unlike PatchOpticalFlow, FrameToFrameOpticalFlow always tracks patches
+/// against the previous frame, not the initial frame where a track was created.
+/// While it might cause more drift of the patch location, it leads to longer
+/// tracks in practice.
+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), frame_counter(0), 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<uint16_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<uint16_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 && frame_counter % config.optical_flow_skip_frames == 0) {
+      output_queue->push(transforms);
+    }
+
+    frame_counter++;
+  }
+
+  void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
+                   const basalt::ManagedImagePyr<uint16_t>& pyr_2,
+                   const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
+                       transform_map_1,
+                   Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
+                       transform_map_2) const {
+    size_t num_points = transform_map_1.size();
+
+    std::vector<KeypointId> ids;
+    Eigen::aligned_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,
+                                  std::hash<KeypointId>>
+        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);
+
+      patch_valid &= p.valid;
+      if (patch_valid) {
+        // 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 uint16_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();
+
+      patch_valid &= dp.residual(img_2, transformed_pat, res);
+
+      if (patch_valid) {
+        const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
+
+        // avoid NaN in increment (leads to SE2::exp crashing)
+        patch_valid &= inc.array().isFinite().all();
+
+        // avoid very large increment
+        patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
+
+        if (patch_valid) {
+          transform *= SE2::exp(inc).matrix();
+
+          const int filter_margin = 2;
+
+          patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
+        }
+      }
+    }
+
+    return patch_valid;
+  }
+
+  void addPoints() {
+    Eigen::aligned_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::aligned_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::aligned_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::aligned_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;
+
+  size_t frame_counter;
+
+  KeypointId last_keypoint_id;
+
+  VioConfig config;
+  basalt::Calibration<Scalar> calib;
+
+  OpticalFlowResult::Ptr transforms;
+  std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
+      pyramid;
+
+  Matrix4 E;
+
+  std::shared_ptr<std::thread> processing_thread;
+};
+
+}  // namespace basalt

include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h

@@ -0,0 +1,468 @@
+/**
+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.
+*/
+
+// Original source for multi-scale implementation:
+// https://github.com/DLR-RM/granite (MIT license)
+
+#pragma once
+
+#include <thread>
+
+#include <sophus/se2.hpp>
+
+#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/image/image_pyr.h>
+#include <basalt/utils/keypoints.h>
+
+namespace basalt {
+
+/// MultiscaleFrameToFrameOpticalFlow is the same as FrameToFrameOpticalFlow,
+/// but patches can be created at all pyramid levels, not just the lowest
+/// pyramid.
+template <typename Scalar, template <typename> typename Pattern>
+class MultiscaleFrameToFrameOpticalFlow : 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;
+
+  MultiscaleFrameToFrameOpticalFlow(const VioConfig& config,
+                                    const basalt::Calibration<double>& calib)
+      : t_ns(-1), frame_counter(0), 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(
+        &MultiscaleFrameToFrameOpticalFlow::processingLoop, this));
+  }
+
+  ~MultiscaleFrameToFrameOpticalFlow() { 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);
+    }
+  }
+
+  bool processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
+    for (const auto& v : new_img_vec->img_data) {
+      if (!v.img.get()) {
+        std::cout << "Image for " << curr_t_ns << " not present!" << std::endl;
+        return true;
+      }
+    }
+    if (t_ns < 0) {
+      t_ns = curr_t_ns;
+
+      transforms.reset(new OpticalFlowResult);
+      transforms->observations.resize(calib.intrinsics.size());
+      transforms->pyramid_levels.resize(calib.intrinsics.size());
+      transforms->t_ns = t_ns;
+
+      pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_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<uint16_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->pyramid_levels.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], transforms->pyramid_levels[i],
+                    new_transforms->observations[i],
+                    new_transforms->pyramid_levels[i]);
+      }
+
+      // std::cout << t_ns << ": Could track "
+      //           << new_transforms->observations.at(0).size() << " points."
+      //           << std::endl;
+
+      transforms = new_transforms;
+      transforms->input_images = new_img_vec;
+
+      addPoints();
+      filterPoints();
+    }
+
+    if (frame_counter % config.optical_flow_skip_frames == 0) {
+      try {
+        output_queue->push(transforms);
+      } catch (const tbb::user_abort&) {
+        return false;
+      };
+    }
+
+    frame_counter++;
+    return true;
+  }
+
+  void trackPoints(
+      const basalt::ManagedImagePyr<uint16_t>& pyr_1,
+      const basalt::ManagedImagePyr<uint16_t>& pyr_2,
+      const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
+          transform_map_1,
+      const std::map<KeypointId, size_t>& pyramid_levels_1,
+      Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
+      std::map<KeypointId, size_t>& pyramid_levels_2) const {
+    size_t num_points = transform_map_1.size();
+
+    std::vector<KeypointId> ids;
+    Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
+    std::vector<size_t> pyramid_level;
+
+    ids.reserve(num_points);
+    init_vec.reserve(num_points);
+    pyramid_level.reserve(num_points);
+
+    for (const auto& kv : transform_map_1) {
+      ids.push_back(kv.first);
+      init_vec.push_back(kv.second);
+      pyramid_level.push_back(pyramid_levels_1.at(kv.first));
+    }
+
+    tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
+                                  std::hash<KeypointId>>
+        result_transforms;
+    tbb::concurrent_unordered_map<KeypointId, size_t, std::hash<KeypointId>>
+        result_pyramid_level;
+
+    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, pyramid_level[r],
+                                transform_2);
+
+        if (valid) {
+          Eigen::AffineCompact2f transform_1_recovered = transform_2;
+
+          valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r],
+                             transform_1_recovered);
+
+          if (valid) {
+            const Scalar scale = 1 << pyramid_level[r];
+            Scalar dist2 = (transform_1.translation() / scale -
+                            transform_1_recovered.translation() / scale)
+                               .squaredNorm();
+
+            if (dist2 < config.optical_flow_max_recovered_dist2) {
+              result_transforms[id] = transform_2;
+              result_pyramid_level[id] = pyramid_level[r];
+            }
+          }
+        }
+      }
+    };
+
+    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_transforms.begin(), result_transforms.end());
+    pyramid_levels_2.clear();
+    pyramid_levels_2.insert(result_pyramid_level.begin(),
+                            result_pyramid_level.end());
+  }
+
+  inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& old_pyr,
+                         const basalt::ManagedImagePyr<uint16_t>& pyr,
+                         const Eigen::AffineCompact2f& old_transform,
+                         const size_t pyramid_level,
+                         Eigen::AffineCompact2f& transform) const {
+    bool patch_valid = true;
+
+    transform.linear().setIdentity();
+
+    for (ssize_t level = config.optical_flow_levels;
+         level >= static_cast<ssize_t>(pyramid_level); level--) {
+      const Scalar scale = 1 << level;
+
+      Eigen::AffineCompact2f transform_tmp = transform;
+
+      transform_tmp.translation() /= scale;
+
+      PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
+
+      patch_valid &= p.valid;
+      if (patch_valid) {
+        // Perform tracking on current level
+        patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform_tmp);
+      }
+
+      if (level == static_cast<ssize_t>(pyramid_level) + 1 && !patch_valid) {
+        return false;
+      }
+
+      transform_tmp.translation() *= scale;
+
+      if (patch_valid) {
+        transform = transform_tmp;
+      }
+    }
+
+    transform.linear() = old_transform.linear() * transform.linear();
+
+    return patch_valid;
+  }
+
+  inline bool trackPointAtLevel(const Image<const uint16_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();
+
+      patch_valid &= dp.residual(img_2, transformed_pat, res);
+
+      if (patch_valid) {
+        const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
+
+        // avoid NaN in increment (leads to SE2::exp crashing)
+        patch_valid &= inc.array().isFinite().all();
+
+        // avoid very large increment
+        patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
+
+        if (patch_valid) {
+          transform *= SE2::exp(inc).matrix();
+
+          const int filter_margin = 2;
+
+          patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
+        }
+      }
+    }
+
+    return patch_valid;
+  }
+
+  void addPoints() {
+    KeypointsData kd;
+
+    Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses_main,
+        new_poses_stereo;
+    std::map<KeypointId, size_t> new_pyramid_levels_main,
+        new_pyramid_levels_stereo;
+
+    for (ssize_t level = 0;
+         level < static_cast<ssize_t>(config.optical_flow_levels) - 1;
+         level++) {
+      Eigen::aligned_vector<Eigen::Vector2d> pts;
+
+      for (const auto& kv : transforms->observations.at(0)) {
+        const ssize_t point_level =
+            transforms->pyramid_levels.at(0).at(kv.first);
+
+        // do not create points were already points at similar levels are
+        if (point_level <= level + 1 && point_level >= level - 1) {
+          // if (point_level == level) {
+          const Scalar scale = 1 << point_level;
+          pts.emplace_back(
+              (kv.second.translation() / scale).template cast<double>());
+        }
+      }
+
+      detectKeypoints(pyramid->at(0).lvl(level), kd,
+                      config.optical_flow_detection_grid_size, 1, pts);
+
+      const Scalar scale = 1 << level;
+
+      for (size_t i = 0; i < kd.corners.size(); i++) {
+        Eigen::AffineCompact2f transform;
+        transform.setIdentity();
+        transform.translation() =
+            kd.corners[i].cast<Scalar>() * scale;  // TODO cast float?
+
+        transforms->observations.at(0)[last_keypoint_id] = transform;
+        transforms->pyramid_levels.at(0)[last_keypoint_id] = level;
+        new_poses_main[last_keypoint_id] = transform;
+        new_pyramid_levels_main[last_keypoint_id] = level;
+
+        last_keypoint_id++;
+      }
+
+      trackPoints(pyramid->at(0), pyramid->at(1), new_poses_main,
+                  new_pyramid_levels_main, new_poses_stereo,
+                  new_pyramid_levels_stereo);
+
+      for (const auto& kv : new_poses_stereo) {
+        transforms->observations.at(1).emplace(kv);
+        transforms->pyramid_levels.at(1)[kv.first] =
+            new_pyramid_levels_stereo.at(kv.first);
+      }
+    }
+  }
+
+  void filterPoints() {
+    std::set<KeypointId> lm_to_remove;
+
+    std::vector<KeypointId> kpid;
+    Eigen::aligned_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::aligned_vector<Eigen::Vector4f> p3d_main, p3d_stereo;
+    std::vector<bool> p3d_main_success, p3d_stereo_success;
+
+    calib.intrinsics[0].unproject(proj0, p3d_main, p3d_main_success);
+    calib.intrinsics[1].unproject(proj1, p3d_stereo, p3d_stereo_success);
+
+    for (size_t i = 0; i < p3d_main_success.size(); i++) {
+      if (p3d_main_success[i] && p3d_stereo_success[i]) {
+        const double epipolar_error =
+            std::abs(p3d_main[i].transpose() * E * p3d_stereo[i]);
+
+        const Scalar scale = 1 << transforms->pyramid_levels.at(0).at(kpid[i]);
+
+        if (epipolar_error > config.optical_flow_epipolar_error * scale) {
+          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;
+
+  size_t frame_counter;
+
+  KeypointId last_keypoint_id;
+
+  VioConfig config;
+  basalt::Calibration<Scalar> calib;
+
+  OpticalFlowResult::Ptr transforms;
+  std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
+      pyramid;
+
+  // map from stereo pair -> essential matrix
+  Matrix4 E;
+
+  std::shared_ptr<std::thread> processing_thread;
+};
+
+}  // namespace basalt

include/basalt/optical_flow/optical_flow.h

@@ -0,0 +1,91 @@
+/**
+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>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+using KeypointId = size_t;
+
+struct OpticalFlowInput {
+  using Ptr = std::shared_ptr<OpticalFlowInput>;
+
+  int64_t t_ns;
+  std::vector<ImageData> img_data;
+  std::vector<cv::Mat> img_cv_data;
+};
+
+struct OpticalFlowResult {
+  using Ptr = std::shared_ptr<OpticalFlowResult>;
+
+  int64_t t_ns;
+  std::vector<Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>>
+      observations;
+
+  std::vector<std::map<KeypointId, size_t>> pyramid_levels;
+
+  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

include/basalt/optical_flow/patch.h

@@ -0,0 +1,226 @@
+/**
+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 <sophus/se2.hpp>
+
+#include <basalt/image/image.h>
+#include <basalt/optical_flow/patterns.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() = default;
+
+  OpticalFlowPatch(const Image<const uint16_t> &img, const Vector2 &pos) {
+    setFromImage(img, pos);
+  }
+
+  template <typename ImgT>
+  static void setData(const ImgT &img, const Vector2 &pos, Scalar &mean,
+                      VectorP &data, const Sophus::SE2<Scalar> *se2 = nullptr) {
+    int num_valid_points = 0;
+    Scalar sum = 0;
+
+    for (int i = 0; i < PATTERN_SIZE; i++) {
+      Vector2 p;
+      if (se2) {
+        p = pos + (*se2) * pattern2.col(i);
+      } else {
+        p = pos + pattern2.col(i);
+      };
+
+      if (img.InBounds(p, 2)) {
+        Scalar val = img.template interp<Scalar>(p);
+        data[i] = val;
+        sum += val;
+        num_valid_points++;
+      } else {
+        data[i] = -1;
+      }
+    }
+
+    mean = sum / num_valid_points;
+    data /= mean;
+  }
+
+  template <typename ImgT>
+  static void setDataJacSe2(const ImgT &img, const Vector2 &pos, Scalar &mean,
+                            VectorP &data, MatrixP3 &J_se2) {
+    int num_valid_points = 0;
+    Scalar sum = 0;
+    Vector3 grad_sum_se2(0, 0, 0);
+
+    Eigen::Matrix<Scalar, 2, 3> Jw_se2;
+    Jw_se2.template topLeftCorner<2, 2>().setIdentity();
+
+    for (int i = 0; i < PATTERN_SIZE; i++) {
+      Vector2 p = pos + pattern2.col(i);
+
+      // Fill jacobians with respect to SE2 warp
+      Jw_se2(0, 2) = -pattern2(1, i);
+      Jw_se2(1, 2) = pattern2(0, i);
+
+      if (img.InBounds(p, 2)) {
+        Vector3 valGrad = img.template interpGrad<Scalar>(p);
+        data[i] = valGrad[0];
+        sum += valGrad[0];
+        J_se2.row(i) = valGrad.template tail<2>().transpose() * Jw_se2;
+        grad_sum_se2 += J_se2.row(i);
+        num_valid_points++;
+      } else {
+        data[i] = -1;
+      }
+    }
+
+    mean = sum / num_valid_points;
+
+    const Scalar mean_inv = num_valid_points / sum;
+
+    for (int i = 0; i < PATTERN_SIZE; i++) {
+      if (data[i] >= 0) {
+        J_se2.row(i) -= grad_sum_se2.transpose() * data[i] / sum;
+        data[i] *= mean_inv;
+      } else {
+        J_se2.row(i).setZero();
+      }
+    }
+    J_se2 *= mean_inv;
+  }
+
+  void setFromImage(const Image<const uint16_t> &img, const Vector2 &pos) {
+    this->pos = pos;
+
+    MatrixP3 J_se2;
+
+    setDataJacSe2(img, pos, mean, data, J_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();
+
+    // NOTE: while it's very unlikely we get a source patch with all black
+    // pixels, since points are usually selected at corners, it doesn't cost
+    // much to be safe here.
+
+    // all-black patch cannot be normalized; will result in mean of "zero" and
+    // H_se2_inv_J_se2_T will contain "NaN" and data will contain "inf"
+    valid = mean > std::numeric_limits<Scalar>::epsilon() &&
+            H_se2_inv_J_se2_T.array().isFinite().all() &&
+            data.array().isFinite().all();
+  }
+
+  inline bool residual(const Image<const uint16_t> &img,
+                       const Matrix2P &transformed_pattern,
+                       VectorP &residual) const {
+    Scalar sum = 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;
+      }
+    }
+
+    // all-black patch cannot be normalized
+    if (sum < std::numeric_limits<Scalar>::epsilon()) {
+      residual.setZero();
+      return false;
+    }
+
+    int num_residuals = 0;
+
+    for (int i = 0; i < PATTERN_SIZE; i++) {
+      if (residual[i] >= 0 && data[i] >= 0) {
+        const 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 = Vector2::Zero();
+  VectorP data = VectorP::Zero();  // 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 = Matrix3P::Zero();
+
+  Scalar mean = 0;
+
+  bool valid = false;
+};
+
+template <typename Scalar, typename Pattern>
+const typename OpticalFlowPatch<Scalar, Pattern>::Matrix2P
+    OpticalFlowPatch<Scalar, Pattern>::pattern2 = Pattern::pattern2;
+
+}  // namespace basalt

include/basalt/optical_flow/patch_optical_flow.h

@@ -0,0 +1,410 @@
+/**
+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 <sophus/se2.hpp>
+
+#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/image/image_pyr.h>
+#include <basalt/utils/keypoints.h>
+
+namespace basalt {
+
+// TODO: patches are currently never erased, so over time memory consumption
+// increases
+// TODO: some changes from FrameToFrameOpticalFlow could be back-ported
+// (adjustments to Scalar=float, tbb parallelization, ...).
+
+/// PatchOpticalFlow keeps reference patches from the frame where the point was
+/// initially created. Should result in more consistent tracks (less drift over
+/// time) than frame-to-frame tracking, but it results in shorter tracks in
+/// practice.
+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),
+        frame_counter(0),
+        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<uint16_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<uint16_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 && frame_counter % config.optical_flow_skip_frames == 0) {
+      output_queue->push(transforms);
+    }
+    frame_counter++;
+  }
+
+  void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
+                   const basalt::ManagedImagePyr<uint16_t>& pyr_2,
+                   const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
+                       transform_map_1,
+                   Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
+                       transform_map_2) const {
+    size_t num_points = transform_map_1.size();
+
+    std::vector<KeypointId> ids;
+    Eigen::aligned_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,
+                                  std::hash<KeypointId>>
+        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::aligned_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::aligned_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;
+
+      // TODO: maybe we should better check patch validity when creating points
+      const auto& p = patch_vec[level];
+      patch_valid &= p.valid;
+      if (patch_valid) {
+        // Perform tracking on current level
+        patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
+      }
+
+      transform.translation() *= scale;
+    }
+
+    return patch_valid;
+  }
+
+  inline bool trackPointAtLevel(const Image<const uint16_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();
+
+      patch_valid &= dp.residual(img_2, transformed_pat, res);
+
+      if (patch_valid) {
+        const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
+
+        // avoid NaN in increment (leads to SE2::exp crashing)
+        patch_valid &= inc.array().isFinite().all();
+
+        // avoid very large increment
+        patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
+
+        if (patch_valid) {
+          transform *= SE2::exp(inc).matrix();
+
+          const int filter_margin = 2;
+
+          patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
+        }
+      }
+    }
+
+    return patch_valid;
+  }
+
+  void addPoints() {
+    Eigen::aligned_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::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
+        new_poses1;
+
+    for (size_t i = 0; i < kd.corners.size(); i++) {
+      Eigen::aligned_vector<PatchT>& p = patches[last_keypoint_id];
+
+      Vector2 pos = kd.corners[i].cast<Scalar>();
+
+      for (int l = 0; l <= config.optical_flow_levels; 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::aligned_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::aligned_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;
+
+  size_t frame_counter;
+
+  KeypointId last_keypoint_id;
+
+  VioConfig config;
+  basalt::Calibration<double> calib;
+
+  Eigen::aligned_unordered_map<KeypointId, Eigen::aligned_vector<PatchT>>
+      patches;
+
+  OpticalFlowResult::Ptr transforms;
+  std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
+      pyramid;
+
+  Eigen::Matrix4d E;
+
+  std::shared_ptr<std::thread> processing_thread;
+};
+
+}  // namespace basalt

include/basalt/optical_flow/patterns.h

@@ -0,0 +1,171 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <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

include/basalt/optimization/accumulator.h

@@ -0,0 +1,281 @@
+/**
+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 <Eigen/Sparse>
+
+#include <array>
+#include <chrono>
+#include <unordered_map>
+
+#include <basalt/utils/assert.h>
+#include <basalt/utils/hash.h>
+
+#if defined(BASALT_USE_CHOLMOD)
+
+#include <Eigen/CholmodSupport>
+
+template <class T>
+using SparseLLT = Eigen::CholmodSupernodalLLT<T>;
+
+#else
+
+template <class T>
+using SparseLLT = Eigen::SimplicialLDLT<T>;
+
+#endif
+
+namespace basalt {
+
+template <typename Scalar_ = double>
+class DenseAccumulator {
+ public:
+  using Scalar = Scalar_;
+
+  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 solve(const VectorX* diagonal) const {
+    if (diagonal == nullptr) {
+      return H.ldlt().solve(b);
+    } else {
+      MatrixX HH = H;
+      HH.diagonal() += *diagonal;
+      return HH.ldlt().solve(b);
+    }
+  }
+
+  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 void setup_solver(){};
+  inline VectorX Hdiagonal() const { return H.diagonal(); }
+
+  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 SparseHashAccumulator {
+ public:
+  using Scalar = Scalar_;
+
+  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 void setup_solver() {
+    std::vector<T> triplets;
+    triplets.reserve(hash_map.size() * 36 + b.rows());
+
+    for (const auto& kv : hash_map) {
+      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));
+        }
+      }
+    }
+
+    for (int i = 0; i < b.rows(); i++) {
+      triplets.emplace_back(i, i, std::numeric_limits<double>::min());
+    }
+
+    smm = SparseMatrix(b.rows(), b.rows());
+    smm.setFromTriplets(triplets.begin(), triplets.end());
+  }
+
+  inline VectorX Hdiagonal() const { return smm.diagonal(); }
+
+  inline VectorX& getB() { return b; }
+
+  inline VectorX solve(const VectorX* diagonal) const {
+    auto t2 = std::chrono::high_resolution_clock::now();
+
+    SparseMatrix sm = smm;
+    if (diagonal) sm.diagonal() += *diagonal;
+
+    VectorX res;
+
+    if (iterative_solver) {
+      // NOTE: since we have to disable Eigen's parallelization with OpenMP
+      // (interferes with TBB), the current CG is single-threaded, and we
+      // can expect a substantial speedup by switching to a parallel
+      // implementation of CG.
+      Eigen::ConjugateGradient<Eigen::SparseMatrix<double>,
+                               Eigen::Lower | Eigen::Upper>
+          cg;
+
+      cg.setTolerance(tolerance);
+      cg.compute(sm);
+      res = cg.solve(b);
+    } else {
+      SparseLLT<SparseMatrix> chol(sm);
+      res = chol.solve(b);
+    }
+
+    auto t3 = std::chrono::high_resolution_clock::now();
+
+    auto elapsed2 =
+        std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
+
+    if (print_info) {
+      std::cout << "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, c[i]);
+      }
+      return seed;
+    }
+  };
+
+  std::unordered_map<KeyT, MatrixX, KeyHash> hash_map;
+
+  VectorX b;
+
+  SparseMatrix smm;
+};
+
+}  // namespace basalt

include/basalt/optimization/linearize.h

@@ -0,0 +1,195 @@
+/**
+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>
+
+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::aligned_vector<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_vector<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_vector<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_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::aligned_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;
+    Vector3 gyro_var_inv;
+    Vector3 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;
+
+    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);
+
+    Eigen::Vector2d proj;
+    bool valid;
+    if (cph) {
+      valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
+    } else {
+      valid = cam.project(point3d, proj);
+    }
+    if (!valid || !proj.array().isFinite().all()) return;
+
+    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;
+
+    if (cph) {
+      Eigen::Matrix<double, 4, 6> d_point_d_xi;
+
+      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::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
+
+      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

include/basalt/optimization/poses_linearize.h

@@ -0,0 +1,264 @@
+/**
+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/blocked_range.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::aligned_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::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
+
+  LinearizePosesOpt(
+      size_t opt_size,
+      const Eigen::aligned_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;
+  }
+};
+
+template <typename Scalar>
+struct ComputeErrorPosesOpt : 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::aligned_vector<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
+
+  ComputeErrorPosesOpt(
+      size_t opt_size,
+      const Eigen::aligned_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;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  ComputeErrorPosesOpt(const ComputeErrorPosesOpt& other, tbb::split)
+      : opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
+    this->common_data = other.common_data;
+    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) {
+            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, nullptr, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void join(ComputeErrorPosesOpt& rhs) {
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+};  // namespace basalt
+
+}  // namespace basalt
+
+#endif

include/basalt/optimization/poses_optimize.h

@@ -0,0 +1,315 @@
+/**
+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/optimization/poses_linearize.h>
+
+#include <tbb/parallel_reduce.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::aligned_vector<AprilgridCornersData>::const_iterator;
+
+ public:
+  PosesOptimization()
+      : lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {}
+
+  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) 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; }
+
+  // Returns true when converged
+  bool optimize(bool opt_intrinsics, double huber_thresh, double stop_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;
+
+    std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
+              << lopt.num_points << std::endl;
+
+    lopt.accum.setup_solver();
+    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
+    bool converged = false;
+    bool step = false;
+    int max_iter = 10;
+
+    while (!step && max_iter > 0 && !converged) {
+      Eigen::aligned_unordered_map<int64_t, Sophus::SE3d>
+          timestam_to_pose_backup = timestam_to_pose;
+      Calibration<Scalar> calib_backup = *calib;
+
+      Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
+      for (int i = 0; i < Hdiag_lambda.size(); i++)
+        Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
+
+      Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc.array().abs().maxCoeff();
+      if (max_inc < stop_thresh) converged = true;
+
+      for (auto &kv : timestam_to_pose) {
+        kv.second *=
+            Sophus::se3_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::se3_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()));
+      }
+
+      ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
+      tbb::parallel_reduce(april_range, eopt);
+
+      double f_diff = (lopt.error - eopt.error);
+      double l_diff = 0.5 * inc.dot(inc * lambda - lopt.accum.getB());
+
+      // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
+
+      double step_quality = f_diff / l_diff;
+
+      if (step_quality < 0) {
+        std::cout << "\t[REJECTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
+                  << " num points " << eopt.num_points << std::endl;
+        lambda = std::min(max_lambda, lambda_vee * lambda);
+        lambda_vee *= 2;
+
+        timestam_to_pose = timestam_to_pose_backup;
+        *calib = calib_backup;
+      } else {
+        std::cout << "\t[ACCEPTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
+                  << " num points " << eopt.num_points << std::endl;
+
+        lambda = std::max(
+            min_lambda,
+            lambda *
+                std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+        lambda_vee = 2;
+
+        error = eopt.error;
+        num_points = eopt.num_points;
+        reprojection_error = eopt.reprojection_error;
+
+        step = true;
+      }
+      max_iter--;
+    }
+
+    if (converged) {
+      std::cout << "[CONVERGED]" << std::endl;
+    }
+
+    return converged;
+  }
+
+  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::aligned_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::aligned_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::aligned_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;
+
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
+
+  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::aligned_unordered_map<int64_t, Sophus::SE3d> timestam_to_pose;
+
+  Eigen::aligned_vector<AprilgridCornersData> aprilgrid_corners_measurements;
+
+  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
+};
+
+}  // namespace basalt

include/basalt/optimization/spline_linearize.h

@@ -0,0 +1,846 @@
+/**
+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/blocked_range.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::aligned_deque<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_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)
+      : 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 Vector3& accel_var_inv = this->common_data.accel_var_inv;
+
+      error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
+
+      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,
+              J.d_val_d_knot[i].transpose() * accel_var_inv.asDiagonal() *
+                  J.d_val_d_knot[j]);
+        }
+        accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
+            start_bias, start_i,
+            J_bias.transpose() * accel_var_inv.asDiagonal() *
+                J.d_val_d_knot[i]);
+
+        if (this->common_data.opt_g) {
+          accum.template addH<G_SIZE, POSE_SIZE>(
+              start_g, start_i,
+              J_g.transpose() * accel_var_inv.asDiagonal() * J.d_val_d_knot[i]);
+        }
+
+        accum.template addB<POSE_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
+                                                    accel_var_inv.asDiagonal() *
+                                                    residual);
+      }
+
+      accum.template addH<ACCEL_BIAS_SIZE, ACCEL_BIAS_SIZE>(
+          start_bias, start_bias,
+          J_bias.transpose() * accel_var_inv.asDiagonal() * J_bias);
+
+      if (this->common_data.opt_g) {
+        accum.template addH<G_SIZE, ACCEL_BIAS_SIZE>(
+            start_g, start_bias,
+            J_g.transpose() * accel_var_inv.asDiagonal() * J_bias);
+        accum.template addH<G_SIZE, G_SIZE>(
+            start_g, start_g,
+            J_g.transpose() * accel_var_inv.asDiagonal() * J_g);
+      }
+
+      accum.template addB<ACCEL_BIAS_SIZE>(
+          start_bias,
+          J_bias.transpose() * accel_var_inv.asDiagonal() * residual);
+
+      if (this->common_data.opt_g) {
+        accum.template addB<G_SIZE>(
+            start_g, J_g.transpose() * accel_var_inv.asDiagonal() * 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 Vector3& 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 += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
+
+      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,
+              J.d_val_d_knot[i].transpose() * gyro_var_inv.asDiagonal() *
+                  J.d_val_d_knot[j]);
+        }
+        accum.template addH<GYRO_BIAS_SIZE, ROT_SIZE>(
+            start_bias, start_i,
+            J_bias.transpose() * gyro_var_inv.asDiagonal() * J.d_val_d_knot[i]);
+        accum.template addB<ROT_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
+                                                   gyro_var_inv.asDiagonal() *
+                                                   residual);
+      }
+
+      accum.template addH<GYRO_BIAS_SIZE, GYRO_BIAS_SIZE>(
+          start_bias, start_bias,
+          J_bias.transpose() * gyro_var_inv.asDiagonal() * J_bias);
+      accum.template addB<GYRO_BIAS_SIZE>(
+          start_bias,
+          J_bias.transpose() * gyro_var_inv.asDiagonal() * 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::se3_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
+};
+
+template <int N, typename Scalar>
+struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
+  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::aligned_deque<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
+      MocapPoseDataIter;
+
+  // typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const SplineT* spline;
+
+  ComputeErrorSplineOpt(size_t opt_size, const SplineT* spl,
+                        const CalibCommonData& common_data)
+      : opt_size(opt_size), spline(spl) {
+    this->common_data = common_data;
+
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+
+    BASALT_ASSERT(spline);
+  }
+
+  ComputeErrorSplineOpt(const ComputeErrorSplineOpt& other, tbb::split)
+      : opt_size(other.opt_size), spline(other.spline) {
+    this->common_data = other.common_data;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  void operator()(const tbb::blocked_range<PoseDataIter>& r) {
+    for (const PoseData& pm : r) {
+      int64_t time_ns = pm.timestamp_ns;
+
+      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());
+      Vector3 residual_rot =
+          spline->orientationResidual(time_ns, pm.data.so3());
+
+      // 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());
+    }
+  }
+
+  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) {
+      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));
+
+      const Vector3& accel_var_inv = this->common_data.accel_var_inv;
+
+      error += residual.transpose() * accel_var_inv.asDiagonal() * 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) {
+      int64_t t_ns = pm.timestamp_ns;
+
+      BASALT_ASSERT(t_ns >= spline->minTimeNs());
+      BASALT_ASSERT(t_ns <= spline->maxTimeNs());
+
+      const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
+
+      Vector3 residual = spline->gyroResidual(
+          t_ns, pm.data, this->common_data.calibration->calib_gyro_bias);
+
+      error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
+    }
+  }
+
+  void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
+    for (const AprilgridCornersData& acd : r) {
+      std::visit(
+          [&](const auto& cam) {
+            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);
+            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, nullptr, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void operator()(const tbb::blocked_range<MocapPoseDataIter>& r) {
+    for (const MocapPoseData& pm : r) {
+      int64_t time_ns =
+          pm.timestamp_ns +
+          this->common_data.mocap_calibration->mocap_time_offset_ns;
+
+      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);
+      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::se3_logd(T_mark_moc_meas * T_moc_mark);
+
+      const Scalar& mocap_var_inv = this->common_data.mocap_var_inv;
+
+      error += mocap_var_inv * residual.squaredNorm();
+    }
+  }
+
+  void join(ComputeErrorSplineOpt& rhs) {
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace basalt
+
+#endif

include/basalt/optimization/spline_optimize.h

@@ -0,0 +1,612 @@
+/**
+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/image/image.h>
+
+#include <basalt/serialization/headers_serialization.h>
+
+#include <tbb/parallel_reduce.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, double init_lambda = 1e-12)
+      : pose_var(1e-4),
+        mocap_initialized(false),
+        lambda(init_lambda),
+        min_lambda(1e-18),
+        max_lambda(100),
+        lambda_vee(2),
+        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::aligned_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::aligned_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;
+
+    while (!mocap_measurements.empty() &&
+           mocap_measurements.front().timestamp_ns <=
+               spline.minTimeNs() + spline.getDtNs())
+      mocap_measurements.pop_front();
+
+    while (!mocap_measurements.empty() &&
+           mocap_measurements.back().timestamp_ns >=
+               spline.maxTimeNs() - spline.getDtNs())
+      mocap_measurements.pop_back();
+
+    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 =
+        calib->dicrete_time_gyro_noise_std().array().square().inverse();
+    ccd.accel_var_inv =
+        calib->dicrete_time_accel_noise_std().array().square().inverse();
+    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;
+  }
+
+  // Returns true when converged
+  bool 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 stop_thresh, double& error,
+                int& num_points, double& reprojection_error,
+                bool print_info = true) {
+    // std::cerr << "optimize num_knots " << num_knots << std::endl;
+
+    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();
+
+    tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(),
+                                                pose_measurements.end());
+    tbb::blocked_range<AprilgridCornersDataIter> april_range(
+        aprilgrid_corners_measurements.begin(),
+        aprilgrid_corners_measurements.end());
+
+    tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
+        mocap_measurements.begin(), mocap_measurements.end());
+
+    tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
+                                                  accel_measurements.end());
+
+    tbb::blocked_range<GyroDataIter> gyro_range(gyro_measurements.begin(),
+                                                gyro_measurements.end());
+
+    if (use_poses) {
+      tbb::parallel_reduce(pose_range, lopt);
+      // lopt(pose_range);
+    }
+
+    if (use_april_corners) {
+      tbb::parallel_reduce(april_range, lopt);
+      // lopt(april_range);
+    }
+
+    if (use_mocap && mocap_initialized) {
+      tbb::parallel_reduce(mocap_pose_range, lopt);
+      // lopt(mocap_pose_range);
+    } else if (use_mocap && !mocap_initialized) {
+      std::cout << "Mocap residuals are not used. Initialize Mocap first!"
+                << std::endl;
+    }
+
+    tbb::parallel_reduce(accel_range, lopt);
+    tbb::parallel_reduce(gyro_range, lopt);
+
+    error = lopt.error;
+    num_points = lopt.num_points;
+    reprojection_error = lopt.reprojection_error;
+
+    if (print_info)
+      std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
+                << lopt.num_points << std::endl;
+
+    lopt.accum.setup_solver();
+    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
+    bool converged = false;
+    bool step = false;
+    int max_iter = 10;
+
+    while (!step && max_iter > 0 && !converged) {
+      Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
+      for (int i = 0; i < Hdiag_lambda.size(); i++)
+        Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
+
+      VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc_full.array().abs().maxCoeff();
+
+      if (max_inc < stop_thresh) converged = true;
+
+      Calibration<Scalar> calib_backup = *calib;
+      MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;
+      SplineT spline_backup = spline;
+      Vector3 g_backup = g;
+
+      applyInc(inc_full, offset_cam_intrinsics);
+
+      ComputeErrorSplineOpt eopt(opt_size, &spline, ccd);
+      if (use_poses) {
+        tbb::parallel_reduce(pose_range, eopt);
+      }
+
+      if (use_april_corners) {
+        tbb::parallel_reduce(april_range, eopt);
+      }
+
+      if (use_mocap && mocap_initialized) {
+        tbb::parallel_reduce(mocap_pose_range, eopt);
+      } else if (use_mocap && !mocap_initialized) {
+        std::cout << "Mocap residuals are not used. Initialize Mocap first!"
+                  << std::endl;
+      }
+
+      tbb::parallel_reduce(accel_range, eopt);
+      tbb::parallel_reduce(gyro_range, eopt);
+
+      double f_diff = (lopt.error - eopt.error);
+      double l_diff = 0.5 * inc_full.dot(inc_full * lambda - lopt.accum.getB());
+
+      // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
+
+      double step_quality = f_diff / l_diff;
+
+      if (step_quality < 0) {
+        if (print_info)
+          std::cout << "\t[REJECTED] lambda:" << lambda
+                    << " step_quality: " << step_quality
+                    << " max_inc: " << max_inc << " Error: " << eopt.error
+                    << " num points " << eopt.num_points << std::endl;
+        lambda = std::min(max_lambda, lambda_vee * lambda);
+        lambda_vee *= 2;
+
+        spline = spline_backup;
+        *calib = calib_backup;
+        *mocap_calib = mocap_calib_backup;
+        g = g_backup;
+
+      } else {
+        if (print_info)
+          std::cout << "\t[ACCEPTED] lambda:" << lambda
+                    << " step_quality: " << step_quality
+                    << " max_inc: " << max_inc << " Error: " << eopt.error
+                    << " num points " << eopt.num_points << std::endl;
+
+        lambda = std::max(
+            min_lambda,
+            lambda *
+                std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+        lambda_vee = 2;
+
+        error = eopt.error;
+        num_points = eopt.num_points;
+        reprojection_error = eopt.reprojection_error;
+
+        step = true;
+      }
+      max_iter--;
+    }
+
+    if (converged && print_info) {
+      std::cout << "[CONVERGED]" << std::endl;
+    }
+
+    return converged;
+  }
+
+  typename Calibration<Scalar>::Ptr calib;
+  typename MocapCalibration<Scalar>::Ptr mocap_calib;
+  bool mocap_initialized;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+  typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_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::se3_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::se3_expd(
+        inc_full.template segment<POSE_SIZE>(mocap_block_offset));
+    mocap_calib->T_i_mark *= Sophus::se3_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];
+
+    //    std::cout << "bias_block_offset " << bias_block_offset << std::endl;
+    //    std::cout << "mocap_block_offset " << mocap_block_offset << std::endl;
+  }
+
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
+
+  int64_t min_time_us, max_time_us;
+
+  Eigen::aligned_deque<PoseData> pose_measurements;
+  Eigen::aligned_deque<GyroData> gyro_measurements;
+  Eigen::aligned_deque<AccelData> accel_measurements;
+  Eigen::aligned_deque<AprilgridCornersData> aprilgrid_corners_measurements;
+  Eigen::aligned_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::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
+
+  int64_t dt_ns;
+};  // namespace basalt
+
+}  // namespace basalt

include/basalt/utils/ba_utils.h

@@ -0,0 +1,146 @@
+/**
+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/vi_estimator/landmark_database.h>
+
+namespace basalt {
+
+template <class Scalar>
+Sophus::SE3<Scalar> computeRelPose(
+    const Sophus::SE3<Scalar>& T_w_i_h, const Sophus::SE3<Scalar>& T_i_c_h,
+    const Sophus::SE3<Scalar>& T_w_i_t, const Sophus::SE3<Scalar>& T_i_c_t,
+    Sophus::Matrix6<Scalar>* d_rel_d_h = nullptr,
+    Sophus::Matrix6<Scalar>* d_rel_d_t = nullptr) {
+  Sophus::SE3<Scalar> tmp2 = (T_i_c_t).inverse();
+
+  Sophus::SE3<Scalar> 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::SE3<Scalar> tmp = tmp2 * T_t_i_h_i;
+  Sophus::SE3<Scalar> res = tmp * T_i_c_h;
+
+  if (d_rel_d_h) {
+    Sophus::Matrix3<Scalar> R = T_w_i_h.so3().inverse().matrix();
+
+    Sophus::Matrix6<Scalar> RR;
+    RR.setZero();
+    RR.template topLeftCorner<3, 3>() = R;
+    RR.template bottomRightCorner<3, 3>() = R;
+
+    *d_rel_d_h = tmp.Adj() * RR;
+  }
+
+  if (d_rel_d_t) {
+    Sophus::Matrix3<Scalar> R = T_w_i_t.so3().inverse().matrix();
+
+    Sophus::Matrix6<Scalar> RR;
+    RR.setZero();
+    RR.template topLeftCorner<3, 3>() = R;
+    RR.template bottomRightCorner<3, 3>() = R;
+
+    *d_rel_d_t = -tmp2.Adj() * RR;
+  }
+
+  return res;
+}
+
+template <class Scalar, class CamT>
+inline bool linearizePoint(
+    const Eigen::Matrix<Scalar, 2, 1>& kpt_obs, const Keypoint<Scalar>& kpt_pos,
+    const Eigen::Matrix<Scalar, 4, 4>& T_t_h, const CamT& cam,
+    Eigen::Matrix<Scalar, 2, 1>& res,
+    Eigen::Matrix<Scalar, 2, POSE_SIZE>* d_res_d_xi = nullptr,
+    Eigen::Matrix<Scalar, 2, 3>* d_res_d_p = nullptr,
+    Eigen::Matrix<Scalar, 4, 1>* proj = nullptr) {
+  static_assert(std::is_same_v<typename CamT::Scalar, Scalar>);
+
+  // Todo implement without jacobians
+  Eigen::Matrix<Scalar, 4, 2> Jup;
+  Eigen::Matrix<Scalar, 4, 1> p_h_3d;
+  p_h_3d = StereographicParam<Scalar>::unproject(kpt_pos.direction, &Jup);
+  p_h_3d[3] = kpt_pos.inv_dist;
+
+  const Eigen::Matrix<Scalar, 4, 1> p_t_3d = T_t_h * p_h_3d;
+
+  Eigen::Matrix<Scalar, 2, 4> Jp;
+  bool valid = cam.project(p_t_3d, res, &Jp);
+  valid &= res.array().isFinite().all();
+
+  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->template head<2>() = res;
+    (*proj)[2] = p_t_3d[3] / p_t_3d.template head<3>().norm();
+  }
+  res -= kpt_obs;
+
+  if (d_res_d_xi) {
+    Eigen::Matrix<Scalar, 4, POSE_SIZE> d_point_d_xi;
+    d_point_d_xi.template topLeftCorner<3, 3>() =
+        Eigen::Matrix<Scalar, 3, 3>::Identity() * kpt_pos.inv_dist;
+    d_point_d_xi.template topRightCorner<3, 3>() =
+        -Sophus::SO3<Scalar>::hat(p_t_3d.template head<3>());
+    d_point_d_xi.row(3).setZero();
+
+    *d_res_d_xi = Jp * d_point_d_xi;
+  }
+
+  if (d_res_d_p) {
+    Eigen::Matrix<Scalar, 4, 3> Jpp;
+    Jpp.setZero();
+    Jpp.template block<3, 2>(0, 0) = T_t_h.template topLeftCorner<3, 4>() * Jup;
+    Jpp.col(2) = T_t_h.col(3);
+
+    *d_res_d_p = Jp * Jpp;
+  }
+
+  return true;
+}
+
+}  // namespace basalt

include/basalt/utils/cast_utils.hpp

@@ -0,0 +1,64 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2021, 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 <type_traits>
+
+namespace basalt {
+
+template <class I>
+typename std::make_signed_t<I> signed_cast(I v) {
+  static_assert(std::is_unsigned_v<I>, "no unsigned");
+  return static_cast<typename std::make_signed_t<I>>(v);
+}
+
+template <class I>
+typename std::make_unsigned_t<I> unsigned_cast(I v) {
+  static_assert(std::is_signed_v<I>, "no signed");
+  return static_cast<typename std::make_unsigned_t<I>>(v);
+}
+
+// copy-assign map while casting Scalar type of values
+template <class M1, class M2>
+void assign_cast_map_values(M1& a, const M2& b) {
+  using Scalar = typename M1::mapped_type::Scalar;
+  a.clear();
+  for (const auto& [k, v] : b) {
+    a.try_emplace(k, v.template cast<Scalar>());
+  }
+}
+
+}  // namespace basalt