Merge branch 'root_vo_squashed' into 'master'

ICCV'21 square root marginalization paper code release

See merge request basalt/basalt!47

.gitignore

@@ -4,3 +4,5 @@ CMakeLists.txt.user
 build*
 scripts/eval/eval_*
 scripts/eval_full/eval_*
+stats_*.*json
+__pycache__

.gitlab-ci.yml

@@ -123,7 +123,7 @@ stages:
     - ./run_evaluations_tumvi.sh
 
 
-.eval_euroc_template: &eval_kitti_definition
+.eval_kitti_template: &eval_kitti_definition
   stage: eval
   parallel: 10
   tags: [docker, dataset-eval]
@@ -145,7 +145,7 @@ bionic-release-compile:
   only:
     - master
   variables:
-    CXX_MARCH: 'sandybridge'
+    CXX_MARCH: 'haswell'
     DEB_DIR: deb_bionic
   artifacts:
     paths:
@@ -157,7 +157,7 @@ focal-release-compile:
   <<: *compile_test_package_definition
   image: vladyslavusenko/b_image_focal:latest
   variables:
-    CXX_MARCH: 'sandybridge'
+    CXX_MARCH: 'haswell'
     DEB_DIR: deb_focal
   artifacts:
     paths:
@@ -168,6 +168,7 @@ focal-release-compile:
 focal-debug-compile:
   <<: *prepare_docker_definition
   <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_focal:latest
   only:
     - master
   variables:
@@ -176,11 +177,51 @@ focal-debug-compile:
 focal-relwithdebinfo-compile:
   <<: *prepare_docker_definition
   <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_focal:latest
   variables:
     BUILD_TYPE: CiRelWithDebInfo
   only:
     - master
 
+# test bionic (lab machines are still on bionic); but we need to do it with GCC9, since GCC7 has issues with root_vo additions
+bionic-relwithdebinfo-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_bionic:latest
+  ### Workaround to install GCC9 in this image, b/c root_vo doesn't compile with GCC7
+  stage: build
+  script:
+    - apt-get update
+    - apt-get install -y software-properties-common
+    - add-apt-repository -y ppa:ubuntu-toolchain-r/test
+    - apt-get update
+    - apt-get install -y g++-9
+    - 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
+  variables:
+    BUILD_TYPE: CiRelWithDebInfo
+    CC: gcc-9
+    CXX: g++-9
+#  only:
+#    - master
+
 # elcapitan-relwithdebinfo-compile:
 #  <<: *compile_test_definition
 #  tags: [macos, "10.11"]
@@ -201,7 +242,7 @@ catalina-relwithdebinfo-compile:
   <<: *compile_test_definition
   tags: [macos, "10.15"]
   only:
-    - master
+#    - master
   variables:
     BUILD_TYPE: CiRelWithDebInfo
 

.gitmodules

@@ -19,3 +19,9 @@
 [submodule "thirdparty/basalt-headers"]
 	path = thirdparty/basalt-headers
 	url = https://gitlab.com/VladyslavUsenko/basalt-headers.git
+[submodule "thirdparty/fmt"]
+	path = thirdparty/fmt
+	url = https://github.com/fmtlib/fmt.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

@@ -123,6 +123,11 @@ else()
       # 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()
 
 
@@ -218,13 +223,25 @@ set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE")
 #  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} -march=${CXX_MARCH} ${BASALT_PASSED_CXX_FLAGS}")
 
 
 set(EIGEN_INCLUDE_DIR_HINTS ${EIGEN_ROOT})
-find_package(Eigen3 3.3.7 EXACT REQUIRED MODULE)
+find_package(Eigen3 3.4.0 EXACT REQUIRED MODULE)
 include_directories(${EIGEN3_INCLUDE_DIR})
 message(STATUS "Found Eigen headers in: ${EIGEN3_INCLUDE_DIR}")
 if(NOT EIGEN3_INCLUDE_DIR MATCHES "^${EIGEN_ROOT}")
@@ -252,7 +269,7 @@ include_directories(thirdparty/basalt-headers/thirdparty/Sophus)
 include_directories(thirdparty/basalt-headers/thirdparty/cereal/include)
 include_directories(thirdparty/basalt-headers/include)
 include_directories(thirdparty/CLI11/include)
-include_directories(thirdparty/fast/include)
+include_directories(thirdparty/magic_enum/include)
 
 include_directories(include)
 
@@ -263,19 +280,32 @@ add_library(basalt SHARED
   src/calibration/aprilgrid.cpp
   src/calibration/calibraiton_helper.cpp
   src/calibration/vignette.cpp
-  src/utils/vio_config.cpp
   src/optical_flow/optical_flow.cpp
-  src/vi_estimator/keypoint_vio.cpp
+  src/linearization/landmark_block.cpp
-  src/vi_estimator/keypoint_vio_linearize.cpp
+  src/linearization/linearization_base.cpp
-  src/vi_estimator/keypoint_vo.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
+  src/vi_estimator/landmark_database.cpp)
-  src/utils/keypoints.cpp)
 
 
-target_link_libraries(basalt PUBLIC ${TBB_LIBRARIES} ${STD_CXX_FS} ${OpenCV_LIBS} PRIVATE rosbag apriltag opengv)
+target_link_libraries(basalt
+  PUBLIC ${TBB_LIBRARIES} ${STD_CXX_FS} ${OpenCV_LIBS} fmt::fmt
+  PRIVATE rosbag apriltag opengv nlohmann::json)
+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)
@@ -291,8 +321,11 @@ target_link_libraries(basalt_vio_sim basalt pangolin)
 add_executable(basalt_mapper_sim src/mapper_sim.cpp)
 target_link_libraries(basalt_mapper_sim basalt pangolin)
 
-add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp)
+# mapper sim native doesn't use template free interface
-target_link_libraries(basalt_mapper_sim_naive basalt pangolin)
+if(BASALT_INSTANTIATIONS_DOUBLE)
+  add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp)
+  target_link_libraries(basalt_mapper_sim_naive basalt pangolin)
+endif()
 
 add_executable(basalt_mapper src/mapper.cpp)
 target_link_libraries(basalt_mapper basalt pangolin)
@@ -321,12 +354,20 @@ endif()
 
 
 
-install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper_sim_naive basalt_mapper basalt_opt_flow basalt_vio basalt_kitti_eval basalt_time_alignment basalt
+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

README.md

@@ -27,6 +27,9 @@ Calibration (demonstrates how these tools can be used for dataset calibration):
 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)
@@ -55,6 +58,7 @@ make -j8
 * [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)
@@ -65,3 +69,6 @@ make -j8
 ## 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).

data/euroc_config.json

@@ -8,11 +8,14 @@
         "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,
@@ -20,13 +23,18 @@
         "config.vio_filter_iteration": 4,
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
-        "config.vio_use_lm": false,
+        "config.vio_use_lm": true,
-        "config.vio_lm_lambda_min": 1e-32,
+        "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,

data/euroc_config_no_factors.json

@@ -8,11 +8,14 @@
         "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,
@@ -21,12 +24,17 @@
         "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,

data/euroc_config_no_weights.json

@@ -8,11 +8,14 @@
         "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,
@@ -21,12 +24,17 @@
         "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,

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

@@ -8,11 +8,14 @@
         "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,
@@ -21,12 +24,17 @@
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
         "config.vio_use_lm": false,
-        "config.vio_lm_lambda_min": 1e-32,
+        "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,

data/tumvi_512_config.json

@@ -8,12 +8,14 @@
         "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,
@@ -22,12 +24,17 @@
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
         "config.vio_use_lm": false,
-        "config.vio_lm_lambda_min": 1e-32,
+        "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,

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
+```

docker/b_image_bionic/Dockerfile

@@ -10,3 +10,4 @@ RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F
 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 && apt-get clean all
+

include/basalt/io/marg_data_io.h

@@ -66,7 +66,9 @@ class MargDataLoader {
   MargDataLoader();
 
   void start(const std::string& path);
-  ~MargDataLoader() { processing_thread->join(); }
+  ~MargDataLoader() {
+    if (processing_thread) processing_thread->join();
+  }
 
   tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue;
 

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/multiscale_frame_to_frame_optical_flow.h

@@ -0,0 +1,456 @@
+/**
+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 {
+
+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<u_int16_t>>);
+      pyramid->resize(calib.intrinsics.size());
+
+      tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
+                        [&](const tbb::blocked_range<size_t>& r) {
+                          for (size_t i = r.begin(); i != r.end(); ++i) {
+                            pyramid->at(i).setFromImage(
+                                *new_img_vec->img_data[i].img,
+                                config.optical_flow_levels);
+                          }
+                        });
+
+      transforms->input_images = new_img_vec;
+
+      addPoints();
+      filterPoints();
+    } else {
+      t_ns = curr_t_ns;
+
+      old_pyramid = pyramid;
+
+      pyramid.reset(new std::vector<basalt::ManagedImagePyr<u_int16_t>>);
+      pyramid->resize(calib.intrinsics.size());
+      tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
+                        [&](const tbb::blocked_range<size_t>& r) {
+                          for (size_t i = r.begin(); i != r.end(); ++i) {
+                            pyramid->at(i).setFromImage(
+                                *new_img_vec->img_data[i].img,
+                                config.optical_flow_levels);
+                          }
+                        });
+
+      OpticalFlowResult::Ptr new_transforms;
+      new_transforms.reset(new OpticalFlowResult);
+      new_transforms->observations.resize(calib.intrinsics.size());
+      new_transforms->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<u_int16_t>& pyr_1,
+      const basalt::ManagedImagePyr<u_int16_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);
+
+      // 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 u_int16_t>& img_2,
+                                const PatchT& dp,
+                                Eigen::AffineCompact2f& transform) const {
+    bool patch_valid = true;
+
+    for (int iteration = 0;
+         patch_valid && iteration < config.optical_flow_max_iterations;
+         iteration++) {
+      typename PatchT::VectorP res;
+
+      typename PatchT::Matrix2P transformed_pat =
+          transform.linear().matrix() * PatchT::pattern2;
+      transformed_pat.colwise() += transform.translation();
+
+      bool valid = dp.residual(img_2, transformed_pat, res);
+
+      if (valid) {
+        Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
+        transform *= SE2::exp(inc).matrix();
+
+        const int filter_margin = 2;
+
+        if (!img_2.InBounds(transform.translation(), filter_margin))
+          patch_valid = false;
+      } else {
+        patch_valid = false;
+      }
+    }
+
+    return patch_valid;
+  }
+
+  void addPoints() {
+    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<u_int16_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

@@ -49,7 +49,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace basalt {
 
-using KeypointId = uint32_t;
+using KeypointId = size_t;
 
 struct OpticalFlowInput {
   using Ptr = std::shared_ptr<OpticalFlowInput>;
@@ -65,6 +65,8 @@ struct OpticalFlowResult {
   std::vector<Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>>
       observations;
 
+  std::vector<std::map<KeypointId, size_t>> pyramid_levels;
+
   OpticalFlowInput::Ptr input_images;
 };
 

include/basalt/optimization/accumulator.h

@@ -61,9 +61,11 @@ using SparseLLT = Eigen::SimplicialLDLT<T>;
 
 namespace basalt {
 
-template <typename Scalar = double>
+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;
 
@@ -135,9 +137,11 @@ class DenseAccumulator {
   VectorX b;
 };
 
-template <typename Scalar = double>
+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;

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

include/basalt/utils/imu_types.h

@@ -52,40 +52,55 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace basalt {
 
+template <class Scalar_>
+class IntegratedImuMeasurement;
+
+template <class Scalar_>
+struct PoseStateWithLin;
+
 namespace constants {
 static const Eigen::Vector3d g(0, 0, -9.81);
 static const Eigen::Vector3d g_dir(0, 0, -1);
 }  // namespace constants
 
-template <class Scalar>
-struct PoseStateWithLin;
-
 template <class Scalar_>
 struct PoseVelBiasStateWithLin {
   using Scalar = Scalar_;
+
   using VecN = typename PoseVelBiasState<Scalar>::VecN;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using SE3 = Sophus::SE3<Scalar>;
 
   PoseVelBiasStateWithLin() {
     linearized = false;
     delta.setZero();
   };
 
-  PoseVelBiasStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i,
+  PoseVelBiasStateWithLin(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i,
-                          const Eigen::Vector3d& vel_w_i,
+                          const Vec3& bias_gyro, const Vec3& bias_accel,
-                          const Eigen::Vector3d& bias_gyro,
+                          bool linearized)
-                          const Eigen::Vector3d& bias_accel, bool linearized)
       : linearized(linearized),
         state_linearized(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel) {
     delta.setZero();
     state_current = state_linearized;
   }
 
-  PoseVelBiasStateWithLin(const PoseVelBiasState<Scalar>& other)
+  explicit PoseVelBiasStateWithLin(const PoseVelBiasState<Scalar>& other)
       : linearized(false), state_linearized(other) {
     delta.setZero();
     state_current = other;
   }
 
+  template <class Scalar2>
+  PoseVelBiasStateWithLin<Scalar2> cast() const {
+    PoseVelBiasStateWithLin<Scalar2> a;
+    a.linearized = linearized;
+    a.delta = delta.template cast<Scalar2>();
+    a.state_linearized = state_linearized.template cast<Scalar2>();
+    a.state_current = state_current.template cast<Scalar2>();
+    return a;
+  }
+
   void setLinFalse() {
     linearized = false;
     delta.setZero();
@@ -123,8 +138,6 @@ struct PoseVelBiasStateWithLin {
   inline const VecN& getDelta() const { return delta; }
   inline int64_t getT_ns() const { return state_linearized.t_ns; }
 
-  friend struct PoseStateWithLin<Scalar>;
-
   inline void backup() {
     backup_delta = delta;
     backup_state_linearized = state_linearized;
@@ -146,6 +159,13 @@ struct PoseVelBiasStateWithLin {
   VecN backup_delta;
   PoseVelBiasState<Scalar> backup_state_linearized, backup_state_current;
 
+  // give access to private members for constructor of PoseStateWithLin
+  friend struct PoseStateWithLin<Scalar>;
+
+  // give access to private members for cast() implementation
+  template <class>
+  friend struct PoseVelBiasStateWithLin;
+
   friend class cereal::access;
 
   template <class Archive>
@@ -164,25 +184,25 @@ struct PoseVelBiasStateWithLin {
   }
 };
 
-template <typename Scalar_>
+template <class Scalar_>
 struct PoseStateWithLin {
   using Scalar = Scalar_;
   using VecN = typename PoseState<Scalar>::VecN;
-  using SE3 = typename PoseState<Scalar>::SE3;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using SE3 = Sophus::SE3<Scalar>;
 
   PoseStateWithLin() {
     linearized = false;
     delta.setZero();
   };
 
-  PoseStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i,
+  PoseStateWithLin(int64_t t_ns, const SE3& T_w_i, bool linearized = false)
-                   bool linearized = false)
       : linearized(linearized), pose_linearized(t_ns, T_w_i) {
     delta.setZero();
     T_w_i_current = T_w_i;
   }
 
-  PoseStateWithLin(const PoseVelBiasStateWithLin<Scalar>& other)
+  explicit PoseStateWithLin(const PoseVelBiasStateWithLin<Scalar>& other)
       : linearized(other.linearized),
         delta(other.delta.template head<6>()),
         pose_linearized(other.state_linearized.t_ns,
@@ -191,13 +211,23 @@ struct PoseStateWithLin {
     PoseState<Scalar>::incPose(delta, T_w_i_current);
   }
 
+  template <class Scalar2>
+  PoseStateWithLin<Scalar2> cast() const {
+    PoseStateWithLin<Scalar2> a;
+    a.linearized = linearized;
+    a.delta = delta.template cast<Scalar2>();
+    a.pose_linearized = pose_linearized.template cast<Scalar2>();
+    a.T_w_i_current = T_w_i_current.template cast<Scalar2>();
+    return a;
+  }
+
   void setLinTrue() {
     linearized = true;
     BASALT_ASSERT(delta.isApproxToConstant(0));
     T_w_i_current = pose_linearized.T_w_i;
   }
 
-  inline const Sophus::SE3d& getPose() const {
+  inline const SE3& getPose() const {
     if (!linearized) {
       return pose_linearized.T_w_i;
     } else {
@@ -205,9 +235,7 @@ struct PoseStateWithLin {
     }
   }
 
-  inline const Sophus::SE3d& getPoseLin() const {
+  inline const SE3& getPoseLin() const { return pose_linearized.T_w_i; }
-    return pose_linearized.T_w_i;
-  }
 
   inline void applyInc(const VecN& inc) {
     if (!linearized) {
@@ -246,6 +274,10 @@ struct PoseStateWithLin {
   PoseState<Scalar> backup_pose_linearized;
   SE3 backup_T_w_i_current;
 
+  // give access to private members for cast() implementation
+  template <class>
+  friend struct PoseStateWithLin;
+
   friend class cereal::access;
 
   template <class Archive>
@@ -272,6 +304,28 @@ struct AbsOrderMap {
   }
 };
 
+template <class Scalar_>
+struct ImuLinData {
+  using Scalar = Scalar_;
+
+  const Eigen::Matrix<Scalar, 3, 1>& g;
+  const Eigen::Matrix<Scalar, 3, 1>& gyro_bias_weight_sqrt;
+  const Eigen::Matrix<Scalar, 3, 1>& accel_bias_weight_sqrt;
+
+  std::map<int64_t, const IntegratedImuMeasurement<Scalar>*> imu_meas;
+};
+
+template <class Scalar_>
+struct MargLinData {
+  using Scalar = Scalar_;
+
+  bool is_sqrt = true;
+
+  AbsOrderMap order;
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> H;
+  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> b;
+};
+
 struct MargData {
   typedef std::shared_ptr<MargData> Ptr;
 

include/basalt/utils/system_utils.h

@@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef BASALT_SYSTEM_UTILS_H
 #define BASALT_SYSTEM_UTILS_H
 
+#include <cstdint>
 #include <string>
 
 namespace basalt {
@@ -47,6 +48,13 @@ inline std::string ensure_trailing_slash(const std::string& path) {
   }
 }
 
+struct MemoryInfo {
+  uint64_t resident_memory = 0;       //!< in bytes
+  uint64_t resident_memory_peak = 0;  //!< in bytes
+};
+
+bool get_memory_info(MemoryInfo& info);
+
 }  // namespace basalt
 
 #endif  // include guard

include/basalt/utils/time_utils.hpp

@@ -0,0 +1,141 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2020-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 <chrono>
+#include <string>
+#include <unordered_map>
+#include <variant>
+#include <vector>
+
+#include <Eigen/Dense>
+
+namespace basalt {
+
+template <class Clock = std::chrono::high_resolution_clock>
+class Timer {
+ public:
+  /// start timer
+  Timer() : start_(Clock::now()) {}
+
+  /// return elapsed time in seconds
+  double elapsed() const {
+    return std::chrono::duration<double>(Clock::now() - start_).count();
+  }
+
+  /// return elapsed time in seconds and reset timer
+  double reset() {
+    const auto now = Clock::now();
+    const double elapsed = std::chrono::duration<double>(now - start_).count();
+    start_ = now;
+    return elapsed;
+  }
+
+ private:
+  std::chrono::time_point<Clock> start_;
+};
+
+template <class Scalar = double>
+struct assign_op {
+  void operator()(Scalar& lhs, const Scalar& rhs) { lhs = rhs; }
+};
+
+template <class Scalar = double>
+struct plus_assign_op {
+  void operator()(Scalar& lhs, const Scalar& rhs) { lhs += rhs; }
+};
+
+template <class T = Timer<>, class Assign = assign_op<>>
+class ScopedTimer {
+ public:
+  explicit ScopedTimer(double& dest) : dest_(dest) {}
+  ~ScopedTimer() { Assign()(dest_, timer_.elapsed()); }
+
+ private:
+  double& dest_;
+  T timer_;
+};
+
+using ScopedTimerAdd = ScopedTimer<Timer<>, plus_assign_op<>>;
+
+template <class F>
+void log_timing(double& time, F fun) {
+  Timer timer;
+  fun();
+  time = timer.elapsed();
+}
+
+template <class F>
+void log_timing_add(double& time, F fun) {
+  Timer timer;
+  fun();
+  time += timer.elapsed();
+}
+
+class ExecutionStats {
+ public:
+  struct Meta {
+    inline Meta& format(const std::string& s) {
+      format_ = s;
+      return *this;
+    }
+
+    // overwrite the meta data from another object
+    void set_meta(const Meta& other) { format_ = other.format_; }
+
+    std::variant<std::vector<double>, std::vector<Eigen::VectorXd>> data_;
+
+    std::string format_;
+  };
+
+  Meta& add(const std::string& name, double value);
+  Meta& add(const std::string& name, const Eigen::VectorXd& value);
+  Meta& add(const std::string& name, const Eigen::VectorXf& value);
+
+  void merge_all(const ExecutionStats& other);
+
+  void merge_sums(const ExecutionStats& other);
+
+  void print() const;
+
+  bool save_json(const std::string& path) const;
+
+ private:
+  std::unordered_map<std::string, Meta> stats_;
+  std::vector<std::string> order_;
+};
+
+}  // namespace basalt

include/basalt/utils/vio_config.h

@@ -38,6 +38,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace basalt {
 
+enum class LinearizationType { ABS_QR, ABS_SC, REL_SC };
+
 struct VioConfig {
   VioConfig();
   void load(const std::string& filename);
@@ -52,14 +54,18 @@ struct VioConfig {
   float optical_flow_epipolar_error;
   int optical_flow_skip_frames;
 
+  LinearizationType vio_linearization_type;
+  bool vio_sqrt_marg;
+
   int vio_max_states;
   int vio_max_kfs;
   int vio_min_frames_after_kf;
   float vio_new_kf_keypoints_thresh;
   bool vio_debug;
+  bool vio_extended_logging;
 
-  double vio_outlier_threshold;
+  //  double vio_outlier_threshold;
-  int vio_filter_iteration;
+  //  int vio_filter_iteration;
   int vio_max_iterations;
 
   double vio_obs_std_dev;
@@ -69,13 +75,21 @@ struct VioConfig {
   bool vio_enforce_realtime;
 
   bool vio_use_lm;
+  double vio_lm_lambda_initial;
   double vio_lm_lambda_min;
   double vio_lm_lambda_max;
+  int vio_lm_landmark_damping_variant;
+  int vio_lm_pose_damping_variant;
+
+  bool vio_scale_jacobian;
 
   double vio_init_pose_weight;
   double vio_init_ba_weight;
   double vio_init_bg_weight;
 
+  bool vio_marg_lost_landmarks;
+  double vio_kf_marg_feature_ratio;
+
   double mapper_obs_std_dev;
   double mapper_obs_huber_thresh;
   int mapper_detection_num_points;
@@ -95,4 +109,5 @@ struct VioConfig {
   double mapper_lm_lambda_min;
   double mapper_lm_lambda_max;
 };
+
 }  // namespace basalt

include/basalt/vi_estimator/ba_base.h

@@ -36,230 +36,61 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <basalt/vi_estimator/landmark_database.h>
 
-#include <tbb/blocked_range.h>
-
 namespace basalt {
 
+template <class Scalar_>
 class BundleAdjustmentBase {
  public:
-  struct RelLinDataBase {
+  using Scalar = Scalar_;
-    std::vector<std::pair<TimeCamId, TimeCamId>> order;
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+  using Vec6 = Eigen::Matrix<Scalar, 6, 1>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
 
-    Eigen::aligned_vector<Sophus::Matrix6d> d_rel_d_h;
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
-    Eigen::aligned_vector<Sophus::Matrix6d> d_rel_d_t;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
-  };
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
 
-  struct FrameRelLinData {
+  using SE3 = Sophus::SE3<Scalar>;
-    Sophus::Matrix6d Hpp;
-    Sophus::Vector6d bp;
 
-    std::vector<int> lm_id;
+  void computeError(Scalar& error,
-    Eigen::aligned_vector<Eigen::Matrix<double, 6, 3>> Hpl;
+                    std::map<int, std::vector<std::pair<TimeCamId, Scalar>>>*
-
-    FrameRelLinData() {
-      Hpp.setZero();
-      bp.setZero();
-    }
-
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  };
-
-  struct RelLinData : public RelLinDataBase {
-    RelLinData(size_t num_keypoints, size_t num_rel_poses) {
-      Hll.reserve(num_keypoints);
-      bl.reserve(num_keypoints);
-      lm_to_obs.reserve(num_keypoints);
-
-      Hpppl.reserve(num_rel_poses);
-      order.reserve(num_rel_poses);
-
-      d_rel_d_h.reserve(num_rel_poses);
-      d_rel_d_t.reserve(num_rel_poses);
-
-      error = 0;
-    }
-
-    void invert_keypoint_hessians() {
-      for (auto& kv : Hll) {
-        Eigen::Matrix3d Hll_inv;
-        Hll_inv.setIdentity();
-        kv.second.ldlt().solveInPlace(Hll_inv);
-        kv.second = Hll_inv;
-      }
-    }
-
-    Eigen::aligned_unordered_map<int, Eigen::Matrix3d> Hll;
-    Eigen::aligned_unordered_map<int, Eigen::Vector3d> bl;
-    Eigen::aligned_unordered_map<int, std::vector<std::pair<size_t, size_t>>>
-        lm_to_obs;
-
-    Eigen::aligned_vector<FrameRelLinData> Hpppl;
-
-    double error;
-  };
-
-  void computeError(double& error,
-                    std::map<int, std::vector<std::pair<TimeCamId, double>>>*
                         outliers = nullptr,
-                    double outlier_threshold = 0) const;
+                    Scalar outlier_threshold = 0) const;
 
-  void linearizeHelper(
+  void filterOutliers(Scalar outlier_threshold, int min_num_obs);
-      Eigen::aligned_vector<RelLinData>& rld_vec,
-      const Eigen::aligned_map<
-          TimeCamId,
-          Eigen::aligned_map<TimeCamId,
-                             Eigen::aligned_vector<KeypointObservation>>>&
-          obs_to_lin,
-      double& error) const;
 
-  static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H,
+  void optimize_single_frame_pose(
-                           Eigen::VectorXd& b);
+      PoseStateWithLin<Scalar>& state_t,
+      const std::vector<std::vector<int>>& connected_obs) const;
 
-  void filterOutliers(double outlier_threshold, int min_num_obs);
+  template <class Scalar2>
+  void get_current_points(
+      Eigen::aligned_vector<Eigen::Matrix<Scalar2, 3, 1>>& points,
+      std::vector<int>& ids) const;
 
-  template <class CamT>
+  void computeDelta(const AbsOrderMap& marg_order, VecX& delta) const;
-  static bool linearizePoint(
-      const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
-      const Eigen::Matrix4d& T_t_h, const CamT& cam, Eigen::Vector2d& res,
-      Eigen::Matrix<double, 2, POSE_SIZE>* d_res_d_xi = nullptr,
-      Eigen::Matrix<double, 2, 3>* d_res_d_p = nullptr,
-      Eigen::Vector4d* proj = nullptr) {
-    // Todo implement without jacobians
-    Eigen::Matrix<double, 4, 2> Jup;
-    Eigen::Vector4d p_h_3d;
-    p_h_3d = StereographicParam<double>::unproject(kpt_pos.dir, &Jup);
-    p_h_3d[3] = kpt_pos.id;
 
-    Eigen::Vector4d p_t_3d = T_t_h * p_h_3d;
+  void linearizeMargPrior(const MargLinData<Scalar>& mld,
+                          const AbsOrderMap& aom, MatX& abs_H, VecX& abs_b,
+                          Scalar& marg_prior_error) const;
 
-    Eigen::Matrix<double, 4, POSE_SIZE> d_point_d_xi;
+  void computeMargPriorError(const MargLinData<Scalar>& mld,
-    d_point_d_xi.topLeftCorner<3, 3>() =
+                             Scalar& marg_prior_error) const;
-        Eigen::Matrix3d::Identity() * kpt_pos.id;
-    d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(p_t_3d.head<3>());
-    d_point_d_xi.row(3).setZero();
 
-    Eigen::Matrix<double, 2, 4> Jp;
+  Scalar computeMargPriorModelCostChange(const MargLinData<Scalar>& mld,
-    bool valid = cam.project(p_t_3d, res, &Jp);
+                                         const VecX& marg_scaling,
-    valid &= res.array().isFinite().all();
+                                         const VecX& marg_pose_inc) const;
 
-    if (!valid) {
+  // TODO: Old version for squared H and b. Remove when no longer needed.
-      //      std::cerr << " Invalid projection! kpt_pos.dir "
+  Scalar computeModelCostChange(const MatX& H, const VecX& b,
-      //                << kpt_pos.dir.transpose() << " kpt_pos.id " <<
+                                const VecX& inc) const;
-      //                kpt_pos.id
-      //                << " idx " << kpt_obs.kpt_id << std::endl;
 
-      //      std::cerr << "T_t_h\n" << T_t_h << std::endl;
+  template <class Scalar2>
-      //      std::cerr << "p_h_3d\n" << p_h_3d.transpose() << std::endl;
+  void computeProjections(
-      //      std::cerr << "p_t_3d\n" << p_t_3d.transpose() << std::endl;
+      std::vector<Eigen::aligned_vector<Eigen::Matrix<Scalar2, 4, 1>>>& data,
-
+      FrameId last_state_t_ns) const;
-      return false;
-    }
-
-    if (proj) {
-      proj->head<2>() = res;
-      (*proj)[2] = p_t_3d[3] / p_t_3d.head<3>().norm();
-    }
-    res -= kpt_obs.pos;
-
-    if (d_res_d_xi) {
-      *d_res_d_xi = Jp * d_point_d_xi;
-    }
-
-    if (d_res_d_p) {
-      Eigen::Matrix<double, 4, 3> Jpp;
-      Jpp.setZero();
-      Jpp.block<3, 2>(0, 0) = T_t_h.topLeftCorner<3, 4>() * Jup;
-      Jpp.col(2) = T_t_h.col(3);
-
-      *d_res_d_p = Jp * Jpp;
-    }
-
-    return true;
-  }
-
-  template <class CamT>
-  inline static bool linearizePoint(
-      const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
-      const CamT& cam, Eigen::Vector2d& res,
-      Eigen::Matrix<double, 2, 3>* d_res_d_p = nullptr,
-      Eigen::Vector4d* proj = nullptr) {
-    // Todo implement without jacobians
-    Eigen::Matrix<double, 4, 2> Jup;
-    Eigen::Vector4d p_h_3d;
-    p_h_3d = StereographicParam<double>::unproject(kpt_pos.dir, &Jup);
-
-    Eigen::Matrix<double, 2, 4> Jp;
-    bool valid = cam.project(p_h_3d, res, &Jp);
-    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 << "p_h_3d\n" << p_h_3d.transpose() << std::endl;
-
-      return false;
-    }
-
-    if (proj) {
-      proj->head<2>() = res;
-      (*proj)[2] = kpt_pos.id;
-    }
-    res -= kpt_obs.pos;
-
-    if (d_res_d_p) {
-      Eigen::Matrix<double, 4, 3> Jpp;
-      Jpp.setZero();
-      Jpp.block<4, 2>(0, 0) = Jup;
-      Jpp.col(2).setZero();
-
-      *d_res_d_p = Jp * Jpp;
-    }
-
-    return true;
-  }
-
-  void updatePoints(const AbsOrderMap& aom, const RelLinData& rld,
-                    const Eigen::VectorXd& inc);
-
-  static Sophus::SE3d computeRelPose(const Sophus::SE3d& T_w_i_h,
-                                     const Sophus::SE3d& T_w_i_t,
-                                     const Sophus::SE3d& T_i_c_h,
-                                     const Sophus::SE3d& T_i_c_t,
-                                     Sophus::Matrix6d* d_rel_d_h = nullptr,
-                                     Sophus::Matrix6d* d_rel_d_t = nullptr);
-
-  void get_current_points(Eigen::aligned_vector<Eigen::Vector3d>& points,
-                          std::vector<int>& ids) const;
-
-  // Modifies abs_H and abs_b as a side effect.
-  static void marginalizeHelper(Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
-                                const std::set<int>& idx_to_keep,
-                                const std::set<int>& idx_to_marg,
-                                Eigen::MatrixXd& marg_H,
-                                Eigen::VectorXd& marg_b);
-
-  void computeDelta(const AbsOrderMap& marg_order,
-                    Eigen::VectorXd& delta) const;
-
-  void linearizeMargPrior(const AbsOrderMap& marg_order,
-                          const Eigen::MatrixXd& marg_H,
-                          const Eigen::VectorXd& marg_b, const AbsOrderMap& aom,
-                          Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
-                          double& marg_prior_error) const;
-
-  void computeMargPriorError(const AbsOrderMap& marg_order,
-                             const Eigen::MatrixXd& marg_H,
-                             const Eigen::VectorXd& marg_b,
-                             double& marg_prior_error) const;
-
-  static Eigen::VectorXd checkNullspace(
-      const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
-      const AbsOrderMap& marg_order,
-      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
-          frame_states,
-      const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& frame_poses);
 
   /// Triangulates the point and returns homogenous representation. First 3
   /// components - unit-length direction vector. Last component inverse
@@ -271,21 +102,23 @@ class BundleAdjustmentBase {
       const Sophus::SE3<typename Derived::Scalar>& T_0_1) {
     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
 
-    using Scalar = typename Derived::Scalar;
+    // suffix "2" to avoid name clash with class-wide typedefs
-    using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+    using Scalar_2 = typename Derived::Scalar;
+    using Vec4_2 = Eigen::Matrix<Scalar_2, 4, 1>;
 
-    Eigen::Matrix<Scalar, 3, 4> P1, P2;
+    Eigen::Matrix<Scalar_2, 3, 4> P1, P2;
     P1.setIdentity();
     P2 = T_0_1.inverse().matrix3x4();
 
-    Eigen::Matrix<Scalar, 4, 4> A(4, 4);
+    Eigen::Matrix<Scalar_2, 4, 4> A(4, 4);
     A.row(0) = f0[0] * P1.row(2) - f0[2] * P1.row(0);
     A.row(1) = f0[1] * P1.row(2) - f0[2] * P1.row(1);
     A.row(2) = f1[0] * P2.row(2) - f1[2] * P2.row(0);
     A.row(3) = f1[1] * P2.row(2) - f1[2] * P2.row(1);
 
-    Eigen::JacobiSVD<Eigen::Matrix<Scalar, 4, 4>> mySVD(A, Eigen::ComputeFullV);
+    Eigen::JacobiSVD<Eigen::Matrix<Scalar_2, 4, 4>> mySVD(A,
-    Vec4 worldPoint = mySVD.matrixV().col(3);
+                                                          Eigen::ComputeFullV);
+    Vec4_2 worldPoint = mySVD.matrixV().col(3);
     worldPoint /= worldPoint.template head<3>().norm();
 
     // Enforce same direction of bearing vector and initial point
@@ -294,104 +127,6 @@ class BundleAdjustmentBase {
     return worldPoint;
   }
 
-  template <class AccumT>
-  static void linearizeAbs(const Eigen::MatrixXd& rel_H,
-                           const Eigen::VectorXd& rel_b,
-                           const RelLinDataBase& rld, const AbsOrderMap& aom,
-                           AccumT& accum) {
-    // int asize = aom.total_size;
-
-    //  BASALT_ASSERT(abs_H.cols() == asize);
-    //  BASALT_ASSERT(abs_H.rows() == asize);
-    //  BASALT_ASSERT(abs_b.rows() == asize);
-
-    for (size_t i = 0; i < rld.order.size(); i++) {
-      const TimeCamId& tcid_h = rld.order[i].first;
-      const TimeCamId& tcid_ti = rld.order[i].second;
-
-      int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
-      int abs_ti_idx = aom.abs_order_map.at(tcid_ti.frame_id).first;
-
-      accum.template addB<POSE_SIZE>(
-          abs_h_idx, rld.d_rel_d_h[i].transpose() *
-                         rel_b.segment<POSE_SIZE>(i * POSE_SIZE));
-      accum.template addB<POSE_SIZE>(
-          abs_ti_idx, rld.d_rel_d_t[i].transpose() *
-                          rel_b.segment<POSE_SIZE>(i * POSE_SIZE));
-
-      for (size_t j = 0; j < rld.order.size(); j++) {
-        BASALT_ASSERT(rld.order[i].first == rld.order[j].first);
-
-        const TimeCamId& tcid_tj = rld.order[j].second;
-
-        int abs_tj_idx = aom.abs_order_map.at(tcid_tj.frame_id).first;
-
-        if (tcid_h.frame_id == tcid_ti.frame_id ||
-            tcid_h.frame_id == tcid_tj.frame_id)
-          continue;
-
-        accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_h_idx, abs_h_idx,
-            rld.d_rel_d_h[i].transpose() *
-                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
-                                                  POSE_SIZE * j) *
-                rld.d_rel_d_h[j]);
-
-        accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_ti_idx, abs_h_idx,
-            rld.d_rel_d_t[i].transpose() *
-                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
-                                                  POSE_SIZE * j) *
-                rld.d_rel_d_h[j]);
-
-        accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_h_idx, abs_tj_idx,
-            rld.d_rel_d_h[i].transpose() *
-                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
-                                                  POSE_SIZE * j) *
-                rld.d_rel_d_t[j]);
-
-        accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_ti_idx, abs_tj_idx,
-            rld.d_rel_d_t[i].transpose() *
-                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
-                                                  POSE_SIZE * j) *
-                rld.d_rel_d_t[j]);
-      }
-    }
-  }
-
-  template <class AccumT>
-  struct LinearizeAbsReduce {
-    using RelLinDataIter = Eigen::aligned_vector<RelLinData>::iterator;
-
-    LinearizeAbsReduce(AbsOrderMap& aom) : aom(aom) {
-      accum.reset(aom.total_size);
-    }
-
-    LinearizeAbsReduce(const LinearizeAbsReduce& other, tbb::split)
-        : aom(other.aom) {
-      accum.reset(aom.total_size);
-    }
-
-    void operator()(const tbb::blocked_range<RelLinDataIter>& range) {
-      for (RelLinData& rld : range) {
-        rld.invert_keypoint_hessians();
-
-        Eigen::MatrixXd rel_H;
-        Eigen::VectorXd rel_b;
-        linearizeRel(rld, rel_H, rel_b);
-
-        linearizeAbs(rel_H, rel_b, rld, aom, accum);
-      }
-    }
-
-    void join(LinearizeAbsReduce& rhs) { accum.join(rhs.accum); }
-
-    AbsOrderMap& aom;
-    AccumT accum;
-  };
-
   inline void backup() {
     for (auto& kv : frame_states) kv.second.backup();
     for (auto& kv : frame_poses) kv.second.backup();
@@ -405,7 +140,7 @@ class BundleAdjustmentBase {
   }
 
   // protected:
-  PoseStateWithLin<double> getPoseStateWithLin(int64_t t_ns) const {
+  PoseStateWithLin<Scalar> getPoseStateWithLin(int64_t t_ns) const {
     auto it = frame_poses.find(t_ns);
     if (it != frame_poses.end()) return it->second;
 
@@ -415,18 +150,18 @@ class BundleAdjustmentBase {
       std::abort();
     }
 
-    return PoseStateWithLin(it2->second);
+    return PoseStateWithLin<Scalar>(it2->second);
   }
 
-  Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>> frame_states;
+  Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>> frame_states;
-  Eigen::aligned_map<int64_t, PoseStateWithLin<double>> frame_poses;
+  Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>> frame_poses;
 
   // Point management
-  LandmarkDatabase lmdb;
+  LandmarkDatabase<Scalar> lmdb;
 
-  double obs_std_dev;
+  Scalar obs_std_dev;
-  double huber_thresh;
+  Scalar huber_thresh;
 
-  basalt::Calibration<double> calib;
+  basalt::Calibration<Scalar> calib;
 };
 }  // namespace basalt

include/basalt/vi_estimator/keypoint_vio.h

@@ -1,227 +0,0 @@
-/**
-BSD 3-Clause License
-
-This file is part of the Basalt project.
-https://gitlab.com/VladyslavUsenko/basalt.git
-
-Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
-* Redistributions of source code must retain the above copyright notice, this
-  list of conditions and the following disclaimer.
-
-* Redistributions in binary form must reproduce the above copyright notice,
-  this list of conditions and the following disclaimer in the documentation
-  and/or other materials provided with the distribution.
-
-* Neither the name of the copyright holder nor the names of its
-  contributors may be used to endorse or promote products derived from
-  this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-#pragma once
-
-#include <memory>
-#include <thread>
-
-#include <Eigen/Dense>
-#include <sophus/se3.hpp>
-
-#include <basalt/imu/preintegration.h>
-#include <basalt/io/dataset_io.h>
-#include <basalt/utils/assert.h>
-#include <basalt/utils/test_utils.h>
-#include <basalt/camera/generic_camera.hpp>
-#include <basalt/camera/stereographic_param.hpp>
-#include <basalt/utils/sophus_utils.hpp>
-
-#include <basalt/vi_estimator/ba_base.h>
-#include <basalt/vi_estimator/vio_estimator.h>
-
-namespace basalt {
-
-class KeypointVioEstimator : public VioEstimatorBase,
-                             public BundleAdjustmentBase {
- public:
-  typedef std::shared_ptr<KeypointVioEstimator> Ptr;
-
-  static const int N = 9;
-  typedef Eigen::Matrix<double, N, 1> VecN;
-  typedef Eigen::Matrix<double, N, N> MatNN;
-  typedef Eigen::Matrix<double, N, 3> MatN3;
-
-  KeypointVioEstimator(const Eigen::Vector3d& g,
-                       const basalt::Calibration<double>& calib,
-                       const VioConfig& config);
-
-  void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
-                  const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg,
-                  const Eigen::Vector3d& ba);
-
-  void initialize(const Eigen::Vector3d& bg, const Eigen::Vector3d& ba);
-
-  virtual ~KeypointVioEstimator() { processing_thread->join(); }
-
-  void addIMUToQueue(const ImuData<double>::Ptr& data);
-  void addVisionToQueue(const OpticalFlowResult::Ptr& data);
-
-  bool measure(const OpticalFlowResult::Ptr& data,
-               const IntegratedImuMeasurement<double>::Ptr& meas);
-
-  static void linearizeAbsIMU(
-      const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
-      double& imu_error, double& bg_error, double& ba_error,
-      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
-          states,
-      const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>>&
-          imu_meas,
-      const Eigen::Vector3d& gyro_bias_weight,
-      const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g);
-
-  static void computeImuError(
-      const AbsOrderMap& aom, double& imu_error, double& bg_error,
-      double& ba_error,
-      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
-          states,
-      const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>>&
-          imu_meas,
-      const Eigen::Vector3d& gyro_bias_weight,
-      const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g);
-
-  // int64_t propagate();
-  // void addNewState(int64_t data_t_ns);
-
-  void marginalize(const std::map<int64_t, int>& num_points_connected);
-
-  void optimize();
-
-  void checkMargNullspace() const;
-
-  int64_t get_t_ns() const {
-    return frame_states.at(last_state_t_ns).getState().t_ns;
-  }
-  const Sophus::SE3d& get_T_w_i() const {
-    return frame_states.at(last_state_t_ns).getState().T_w_i;
-  }
-  const Eigen::Vector3d& get_vel_w_i() const {
-    return frame_states.at(last_state_t_ns).getState().vel_w_i;
-  }
-
-  const PoseVelBiasState<double>& get_state() const {
-    return frame_states.at(last_state_t_ns).getState();
-  }
-  PoseVelBiasState<double> get_state(int64_t t_ns) const {
-    PoseVelBiasState<double> state;
-
-    auto it = frame_states.find(t_ns);
-
-    if (it != frame_states.end()) {
-      return it->second.getState();
-    }
-
-    auto it2 = frame_poses.find(t_ns);
-    if (it2 != frame_poses.end()) {
-      state.T_w_i = it2->second.getPose();
-    }
-
-    return state;
-  }
-  // const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); }
-
-  void computeProjections(
-      std::vector<Eigen::aligned_vector<Eigen::Vector4d>>& res) const;
-
-  inline void setMaxStates(size_t val) { max_states = val; }
-  inline void setMaxKfs(size_t val) { max_kfs = val; }
-
-  Eigen::aligned_vector<Sophus::SE3d> getFrameStates() const {
-    Eigen::aligned_vector<Sophus::SE3d> res;
-
-    for (const auto& kv : frame_states) {
-      res.push_back(kv.second.getState().T_w_i);
-    }
-
-    return res;
-  }
-
-  Eigen::aligned_vector<Sophus::SE3d> getFramePoses() const {
-    Eigen::aligned_vector<Sophus::SE3d> res;
-
-    for (const auto& kv : frame_poses) {
-      res.push_back(kv.second.getPose());
-    }
-
-    return res;
-  }
-
-  Eigen::aligned_map<int64_t, Sophus::SE3d> getAllPosesMap() const {
-    Eigen::aligned_map<int64_t, Sophus::SE3d> res;
-
-    for (const auto& kv : frame_poses) {
-      res[kv.first] = kv.second.getPose();
-    }
-
-    for (const auto& kv : frame_states) {
-      res[kv.first] = kv.second.getState().T_w_i;
-    }
-
-    return res;
-  }
-
-  const Sophus::SE3d& getT_w_i_init() { return T_w_i_init; }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- private:
-  bool take_kf;
-  int frames_after_kf;
-  std::set<int64_t> kf_ids;
-
-  int64_t last_state_t_ns;
-  Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>> imu_meas;
-
-  const Eigen::Vector3d g;
-
-  // Input
-
-  Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
-
-  std::map<int64_t, int> num_points_kf;
-
-  // Marginalization
-  AbsOrderMap marg_order;
-  Eigen::MatrixXd marg_H;
-  Eigen::VectorXd marg_b;
-
-  Eigen::Vector3d gyro_bias_weight, accel_bias_weight;
-
-  size_t max_states;
-  size_t max_kfs;
-
-  Sophus::SE3d T_w_i_init;
-
-  bool initialized;
-  bool opt_started;
-
-  VioConfig config;
-
-  double lambda, min_lambda, max_lambda, lambda_vee;
-
-  int64_t msckf_kf_id;
-
-  std::shared_ptr<std::thread> processing_thread;
-};
-}  // namespace basalt

include/basalt/vi_estimator/keypoint_vo.h

@@ -1,201 +0,0 @@
-/**
-BSD 3-Clause License
-
-This file is part of the Basalt project.
-https://gitlab.com/VladyslavUsenko/basalt.git
-
-Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
-* Redistributions of source code must retain the above copyright notice, this
-  list of conditions and the following disclaimer.
-
-* Redistributions in binary form must reproduce the above copyright notice,
-  this list of conditions and the following disclaimer in the documentation
-  and/or other materials provided with the distribution.
-
-* Neither the name of the copyright holder nor the names of its
-  contributors may be used to endorse or promote products derived from
-  this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-#pragma once
-
-#include <memory>
-#include <thread>
-
-#include <Eigen/Dense>
-#include <sophus/se3.hpp>
-
-#include <basalt/imu/preintegration.h>
-#include <basalt/io/dataset_io.h>
-#include <basalt/utils/assert.h>
-#include <basalt/utils/test_utils.h>
-#include <basalt/camera/generic_camera.hpp>
-#include <basalt/camera/stereographic_param.hpp>
-#include <basalt/utils/sophus_utils.hpp>
-
-#include <basalt/vi_estimator/ba_base.h>
-#include <basalt/vi_estimator/vio_estimator.h>
-
-namespace basalt {
-
-class KeypointVoEstimator : public VioEstimatorBase,
-                            public BundleAdjustmentBase {
- public:
-  typedef std::shared_ptr<KeypointVoEstimator> Ptr;
-
-  static const int N = 9;
-  typedef Eigen::Matrix<double, N, 1> VecN;
-  typedef Eigen::Matrix<double, N, N> MatNN;
-  typedef Eigen::Matrix<double, N, 3> MatN3;
-
-  KeypointVoEstimator(const basalt::Calibration<double>& calib,
-                      const VioConfig& config);
-
-  void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
-                  const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg,
-                  const Eigen::Vector3d& ba);
-
-  void initialize(const Eigen::Vector3d& bg, const Eigen::Vector3d& ba);
-
-  virtual ~KeypointVoEstimator() { processing_thread->join(); }
-
-  void addIMUToQueue(const ImuData<double>::Ptr& data);
-  void addVisionToQueue(const OpticalFlowResult::Ptr& data);
-
-  bool measure(const OpticalFlowResult::Ptr& data, bool add_frame);
-
-  // int64_t propagate();
-  // void addNewState(int64_t data_t_ns);
-
-  void marginalize(const std::map<int64_t, int>& num_points_connected);
-
-  void optimize();
-
-  void checkMargNullspace() const;
-
-  int64_t get_t_ns() const {
-    return frame_states.at(last_state_t_ns).getState().t_ns;
-  }
-  const Sophus::SE3d& get_T_w_i() const {
-    return frame_states.at(last_state_t_ns).getState().T_w_i;
-  }
-  const Eigen::Vector3d& get_vel_w_i() const {
-    return frame_states.at(last_state_t_ns).getState().vel_w_i;
-  }
-
-  const PoseVelBiasState<double>& get_state() const {
-    return frame_states.at(last_state_t_ns).getState();
-  }
-  PoseVelBiasState<double> get_state(int64_t t_ns) const {
-    PoseVelBiasState<double> state;
-
-    auto it = frame_states.find(t_ns);
-
-    if (it != frame_states.end()) {
-      return it->second.getState();
-    }
-
-    auto it2 = frame_poses.find(t_ns);
-    if (it2 != frame_poses.end()) {
-      state.T_w_i = it2->second.getPose();
-    }
-
-    return state;
-  }
-  // const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); }
-
-  void computeProjections(
-      std::vector<Eigen::aligned_vector<Eigen::Vector4d>>& res) const;
-
-  inline void setMaxStates(size_t val) { max_states = val; }
-  inline void setMaxKfs(size_t val) { max_kfs = val; }
-
-  Eigen::aligned_vector<Sophus::SE3d> getFrameStates() const {
-    Eigen::aligned_vector<Sophus::SE3d> res;
-
-    for (const auto& kv : frame_states) {
-      res.push_back(kv.second.getState().T_w_i);
-    }
-
-    return res;
-  }
-
-  Eigen::aligned_vector<Sophus::SE3d> getFramePoses() const {
-    Eigen::aligned_vector<Sophus::SE3d> res;
-
-    for (const auto& kv : frame_poses) {
-      res.push_back(kv.second.getPose());
-    }
-
-    return res;
-  }
-
-  Eigen::aligned_map<int64_t, Sophus::SE3d> getAllPosesMap() const {
-    Eigen::aligned_map<int64_t, Sophus::SE3d> res;
-
-    for (const auto& kv : frame_poses) {
-      res[kv.first] = kv.second.getPose();
-    }
-
-    for (const auto& kv : frame_states) {
-      res[kv.first] = kv.second.getState().T_w_i;
-    }
-
-    return res;
-  }
-
-  const Sophus::SE3d& getT_w_i_init() { return T_w_i_init; }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- private:
-  bool take_kf;
-  int frames_after_kf;
-  std::set<int64_t> kf_ids;
-
-  int64_t last_state_t_ns;
-
-  // Input
-
-  Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
-
-  std::map<int64_t, int> num_points_kf;
-
-  // Marginalization
-  AbsOrderMap marg_order;
-  Eigen::MatrixXd marg_H;
-  Eigen::VectorXd marg_b;
-
-  Eigen::Vector3d gyro_bias_weight, accel_bias_weight;
-
-  size_t max_states;
-  size_t max_kfs;
-
-  Sophus::SE3d T_w_i_init;
-
-  bool initialized;
-
-  VioConfig config;
-
-  double lambda, min_lambda, max_lambda, lambda_vee;
-
-  int64_t msckf_kf_id;
-
-  std::shared_ptr<std::thread> processing_thread;
-};
-}  // namespace basalt

include/basalt/vi_estimator/landmark_database.h

@@ -35,43 +35,61 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
 #include <basalt/utils/imu_types.h>
+#include <basalt/utils/eigen_utils.hpp>
 
 namespace basalt {
 
+template <class Scalar_>
+struct KeypointObservation {
+  using Scalar = Scalar_;
+
+  int kpt_id;
+  Eigen::Matrix<Scalar, 2, 1> pos;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
 // keypoint position defined relative to some frame
-struct KeypointPosition {
+template <class Scalar_>
-  TimeCamId kf_id;
+struct Keypoint {
-  Eigen::Vector2d dir;
+  using Scalar = Scalar_;
-  double id;
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+
+  using ObsMap = Eigen::aligned_map<TimeCamId, Vec2>;
+  using MapIter = typename ObsMap::iterator;
+
+  // 3D position parameters
+  Vec2 direction;
+  Scalar inv_dist;
+
+  // Observations
+  TimeCamId host_kf_id;
+  ObsMap obs;
 
   inline void backup() {
-    backup_dir = dir;
+    backup_direction = direction;
-    backup_id = id;
+    backup_inv_dist = inv_dist;
   }
 
   inline void restore() {
-    dir = backup_dir;
+    direction = backup_direction;
-    id = backup_id;
+    inv_dist = backup_inv_dist;
   }
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  private:
-  Eigen::Vector2d backup_dir;
+  Vec2 backup_direction;
-  double backup_id;
+  Scalar backup_inv_dist;
-};
-
-struct KeypointObservation {
-  int kpt_id;
-  Eigen::Vector2d pos;
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
+template <class Scalar_>
 class LandmarkDatabase {
  public:
+  using Scalar = Scalar_;
+
   // Non-const
-  void addLandmark(int lm_id, const KeypointPosition& pos);
+  void addLandmark(KeypointId lm_id, const Keypoint<Scalar>& pos);
 
   void removeFrame(const FrameId& frame);
 
@@ -80,34 +98,36 @@ class LandmarkDatabase {
                        const std::set<FrameId>& states_to_marg_all);
 
   void addObservation(const TimeCamId& tcid_target,
-                      const KeypointObservation& o);
+                      const KeypointObservation<Scalar>& o);
 
-  KeypointPosition& getLandmark(int lm_id);
+  Keypoint<Scalar>& getLandmark(KeypointId lm_id);
 
   // Const
-  const KeypointPosition& getLandmark(int lm_id) const;
+  const Keypoint<Scalar>& getLandmark(KeypointId lm_id) const;
 
   std::vector<TimeCamId> getHostKfs() const;
 
-  std::vector<KeypointPosition> getLandmarksForHost(
+  std::vector<const Keypoint<Scalar>*> getLandmarksForHost(
       const TimeCamId& tcid) const;
 
-  const Eigen::aligned_map<
+  const std::unordered_map<TimeCamId,
-      TimeCamId, Eigen::aligned_map<
+                           std::map<TimeCamId, std::set<KeypointId>>>&
-                     TimeCamId, Eigen::aligned_vector<KeypointObservation>>>&
   getObservations() const;
 
+  const Eigen::aligned_unordered_map<KeypointId, Keypoint<Scalar>>&
+  getLandmarks() const;
+
   bool landmarkExists(int lm_id) const;
 
   size_t numLandmarks() const;
 
   int numObservations() const;
 
-  int numObservations(int lm_id) const;
+  int numObservations(KeypointId lm_id) const;
 
-  void removeLandmark(int lm_id);
+  void removeLandmark(KeypointId lm_id);
 
-  void removeObservations(int lm_id, const std::set<TimeCamId>& obs);
+  void removeObservations(KeypointId lm_id, const std::set<TimeCamId>& obs);
 
   inline void backup() {
     for (auto& kv : kpts) kv.second.backup();
@@ -118,16 +138,19 @@ class LandmarkDatabase {
   }
 
  private:
-  Eigen::aligned_unordered_map<int, KeypointPosition> kpts;
+  using MapIter =
-  Eigen::aligned_map<
+      typename Eigen::aligned_unordered_map<KeypointId,
-      TimeCamId,
+                                            Keypoint<Scalar>>::iterator;
-      Eigen::aligned_map<TimeCamId, Eigen::aligned_vector<KeypointObservation>>>
+  MapIter removeLandmarkHelper(MapIter it);
-      obs;
+  typename Keypoint<Scalar>::MapIter removeLandmarkObservationHelper(
+      MapIter it, typename Keypoint<Scalar>::MapIter it2);
 
-  std::unordered_map<TimeCamId, std::set<int>> host_to_kpts;
+  Eigen::aligned_unordered_map<KeypointId, Keypoint<Scalar>> kpts;
 
-  int num_observations = 0;
+  std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
-  Eigen::aligned_unordered_map<int, int> kpts_num_obs;
+      observations;
+
+  static constexpr int min_num_obs = 2;
 };
 
 }  // namespace basalt

include/basalt/vi_estimator/marg_helper.h

@@ -0,0 +1,68 @@
+/**
+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 <set>
+
+namespace basalt {
+
+template <class Scalar_>
+class MargHelper {
+ public:
+  using Scalar = Scalar_;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  // Modifies abs_H and abs_b as a side effect.
+  static void marginalizeHelperSqToSq(MatX& abs_H, VecX& abs_b,
+                                      const std::set<int>& idx_to_keep,
+                                      const std::set<int>& idx_to_marg,
+                                      MatX& marg_H, VecX& marg_b);
+
+  // Modifies abs_H and abs_b as a side effect.
+  static void marginalizeHelperSqToSqrt(MatX& abs_H, VecX& abs_b,
+                                        const std::set<int>& idx_to_keep,
+                                        const std::set<int>& idx_to_marg,
+                                        MatX& marg_sqrt_H, VecX& marg_sqrt_b);
+
+  // Modifies Q2Jp and Q2r as a side effect.
+  static void marginalizeHelperSqrtToSqrt(MatX& Q2Jp, VecX& Q2r,
+                                          const std::set<int>& idx_to_keep,
+                                          const std::set<int>& idx_to_marg,
+                                          MatX& marg_sqrt_H, VecX& marg_sqrt_b);
+};
+}  // namespace basalt

include/basalt/vi_estimator/nfr_mapper.h

@@ -41,7 +41,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sophus/se3.hpp>
 
 #include <basalt/utils/common_types.h>
-#include <basalt/vi_estimator/ba_base.h>
+#include <basalt/utils/nfr.h>
+#include <basalt/vi_estimator/sc_ba_base.h>
 #include <basalt/vi_estimator/vio_estimator.h>
 
 #include <tbb/concurrent_unordered_map.h>
@@ -54,24 +55,27 @@ namespace basalt {
 template <size_t N>
 class HashBow;
 
-class NfrMapper : public BundleAdjustmentBase {
+class NfrMapper : public ScBundleAdjustmentBase<double> {
  public:
+  using Scalar = double;
+
   using Ptr = std::shared_ptr<NfrMapper>;
 
   template <class AccumT>
   struct MapperLinearizeAbsReduce
-      : public BundleAdjustmentBase::LinearizeAbsReduce<AccumT> {
+      : public ScBundleAdjustmentBase<Scalar>::LinearizeAbsReduce<AccumT> {
     using RollPitchFactorConstIter =
         Eigen::aligned_vector<RollPitchFactor>::const_iterator;
     using RelPoseFactorConstIter =
         Eigen::aligned_vector<RelPoseFactor>::const_iterator;
-    using RelLinDataIter = Eigen::aligned_vector<RelLinData>::iterator;
+    using RelLinDataConstIter =
+        Eigen::aligned_vector<RelLinData>::const_iterator;
 
     MapperLinearizeAbsReduce(
         AbsOrderMap& aom,
-        const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>*
+        const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>*
             frame_poses)
-        : BundleAdjustmentBase::LinearizeAbsReduce<AccumT>(aom),
+        : ScBundleAdjustmentBase<Scalar>::LinearizeAbsReduce<AccumT>(aom),
           frame_poses(frame_poses) {
       this->accum.reset(aom.total_size);
       roll_pitch_error = 0;
@@ -79,7 +83,7 @@ class NfrMapper : public BundleAdjustmentBase {
     }
 
     MapperLinearizeAbsReduce(const MapperLinearizeAbsReduce& other, tbb::split)
-        : BundleAdjustmentBase::LinearizeAbsReduce<AccumT>(other.aom),
+        : ScBundleAdjustmentBase<Scalar>::LinearizeAbsReduce<AccumT>(other.aom),
           frame_poses(other.frame_poses) {
       this->accum.reset(this->aom.total_size);
       roll_pitch_error = 0;
@@ -92,10 +96,8 @@ class NfrMapper : public BundleAdjustmentBase {
       rel_error += rhs.rel_error;
     }
 
-    void operator()(const tbb::blocked_range<RelLinDataIter>& range) {
+    void operator()(const tbb::blocked_range<RelLinDataConstIter>& range) {
-      for (RelLinData& rld : range) {
+      for (const RelLinData& rld : range) {
-        rld.invert_keypoint_hessians();
-
         Eigen::MatrixXd rel_H;
         Eigen::VectorXd rel_b;
         linearizeRel(rld, rel_H, rel_b);
@@ -155,7 +157,7 @@ class NfrMapper : public BundleAdjustmentBase {
     double roll_pitch_error;
     double rel_error;
 
-    const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>* frame_poses;
+    const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>* frame_poses;
   };
 
   NfrMapper(const basalt::Calibration<double>& calib, const VioConfig& config);
@@ -168,7 +170,7 @@ class NfrMapper : public BundleAdjustmentBase {
 
   void optimize(int num_iterations = 10);
 
-  Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& getFramePoses();
+  Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& getFramePoses();
 
   void computeRelPose(double& rel_error);
 

include/basalt/vi_estimator/sc_ba_base.h

@@ -0,0 +1,452 @@
+/**
+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/ba_base.h>
+
+#include <tbb/blocked_range.h>
+
+namespace basalt {
+
+template <class Scalar_>
+class ScBundleAdjustmentBase : public BundleAdjustmentBase<Scalar_> {
+ public:
+  using Scalar = Scalar_;
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec6 = Eigen::Matrix<Scalar, 6, 1>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
+  using Mat63 = Eigen::Matrix<Scalar, 6, 3>;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using SO3 = Sophus::SO3<Scalar>;
+  using SE3 = Sophus::SE3<Scalar>;
+
+  struct RelLinDataBase {
+    std::vector<std::pair<TimeCamId, TimeCamId>> order;
+
+    Eigen::aligned_vector<Mat6> d_rel_d_h;
+    Eigen::aligned_vector<Mat6> d_rel_d_t;
+  };
+
+  struct FrameRelLinData {
+    Mat6 Hpp;
+    Vec6 bp;
+
+    std::vector<int> lm_id;
+    Eigen::aligned_vector<Mat63> Hpl;
+
+    FrameRelLinData() {
+      Hpp.setZero();
+      bp.setZero();
+    }
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  struct RelLinData : public RelLinDataBase {
+    RelLinData(size_t num_keypoints, size_t num_rel_poses) {
+      Hll.reserve(num_keypoints);
+      Hllinv.reserve(num_keypoints);
+      bl.reserve(num_keypoints);
+      lm_to_obs.reserve(num_keypoints);
+
+      Hpppl.reserve(num_rel_poses);
+      order.reserve(num_rel_poses);
+
+      d_rel_d_h.reserve(num_rel_poses);
+      d_rel_d_t.reserve(num_rel_poses);
+
+      error = 0;
+    }
+
+    void invert_keypoint_hessians() {
+      for (const auto& [kpt_idx, hll] : Hll) {
+        Mat3 Hll_inv;
+        Hll_inv.setIdentity();
+        // Use ldlt b/c it has good speed (no numeric indefiniteness of this 3x3
+        // matrix expected), and compared ot a direct inverse (which may be even
+        // faster), it can handle degenerate cases where Hll is not invertible.
+        hll.ldlt().solveInPlace(Hll_inv);
+        Hllinv[kpt_idx] = Hll_inv;
+      }
+    }
+
+    using RelLinDataBase::d_rel_d_h;
+    using RelLinDataBase::d_rel_d_t;
+    using RelLinDataBase::order;
+
+    Eigen::aligned_unordered_map<int, Mat3> Hll;
+    Eigen::aligned_unordered_map<int, Mat3> Hllinv;
+    Eigen::aligned_unordered_map<int, Vec3> bl;
+    Eigen::aligned_unordered_map<int, std::vector<std::pair<size_t, size_t>>>
+        lm_to_obs;
+
+    Eigen::aligned_vector<FrameRelLinData> Hpppl;
+
+    Scalar error;
+  };
+
+  struct FrameAbsLinData {
+    Mat6 Hphph;
+    Vec6 bph;
+
+    Mat6 Hptpt;
+    Vec6 bpt;
+
+    Mat6 Hphpt;
+
+    std::vector<int> lm_id;
+    Eigen::aligned_vector<Mat63> Hphl;
+    Eigen::aligned_vector<Mat63> Hptl;
+
+    FrameAbsLinData() {
+      Hphph.setZero();
+      Hptpt.setZero();
+      Hphpt.setZero();
+
+      bph.setZero();
+      bpt.setZero();
+    }
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  struct AbsLinData {
+    AbsLinData(size_t num_keypoints, size_t num_rel_poses) {
+      Hll.reserve(num_keypoints);
+      Hllinv.reserve(num_keypoints);
+      bl.reserve(num_keypoints);
+      lm_to_obs.reserve(num_keypoints);
+
+      Hpppl.reserve(num_rel_poses);
+      order.reserve(num_rel_poses);
+
+      error = 0;
+    }
+
+    void invert_keypoint_hessians() {
+      for (const auto& [kpt_idx, hll] : Hll) {
+        Mat3 Hll_inv;
+        Hll_inv.setIdentity();
+        // Use ldlt b/c it has good speed (no numeric indefiniteness of this 3x3
+        // matrix expected), and compared ot a direct inverse (which may be even
+        // faster), it can handle degenerate cases where Hll is not invertible.
+        hll.ldlt().solveInPlace(Hll_inv);
+        Hllinv[kpt_idx] = Hll_inv;
+      }
+    }
+
+    std::vector<std::pair<TimeCamId, TimeCamId>> order;
+
+    Eigen::aligned_unordered_map<int, Mat3> Hll;
+    Eigen::aligned_unordered_map<int, Mat3> Hllinv;
+    Eigen::aligned_unordered_map<int, Vec3> bl;
+    Eigen::aligned_unordered_map<int, std::vector<std::pair<size_t, size_t>>>
+        lm_to_obs;
+
+    Eigen::aligned_vector<FrameAbsLinData> Hpppl;
+
+    Scalar error;
+  };
+
+  using BundleAdjustmentBase<Scalar>::computeDelta;
+
+  void linearizeHelper(
+      Eigen::aligned_vector<RelLinData>& rld_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      Scalar& error) const {
+    linearizeHelperStatic(rld_vec, obs_to_lin, this, error);
+  }
+
+  void linearizeAbsHelper(
+      Eigen::aligned_vector<AbsLinData>& ald_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      Scalar& error) const {
+    linearizeHelperAbsStatic(ald_vec, obs_to_lin, this, error);
+  }
+
+  static void linearizeHelperStatic(
+      Eigen::aligned_vector<RelLinData>& rld_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error);
+
+  void linearizeHelperAbs(
+      Eigen::aligned_vector<AbsLinData>& ald_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      Scalar& error) const {
+    linearizeHelperAbsStatic(ald_vec, obs_to_lin, this, error);
+  }
+
+  static void linearizeHelperAbsStatic(
+      Eigen::aligned_vector<AbsLinData>& ald_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error);
+
+  static void linearizeRel(const RelLinData& rld, MatX& H, VecX& b);
+
+  static void updatePoints(const AbsOrderMap& aom, const RelLinData& rld,
+                           const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
+                           Scalar* l_diff = nullptr);
+
+  static void updatePointsAbs(const AbsOrderMap& aom, const AbsLinData& ald,
+                              const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
+                              Scalar* l_diff = nullptr);
+
+  static Eigen::VectorXd checkNullspace(
+      const MatX& H, const VecX& b, const AbsOrderMap& marg_order,
+      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
+          frame_states,
+      const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
+      bool verbose = true);
+
+  static Eigen::VectorXd checkEigenvalues(const MatX& H, bool verbose = true);
+
+  static void computeImuError(
+      const AbsOrderMap& aom, Scalar& imu_error, Scalar& bg_error,
+      Scalar& ba_error,
+      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
+          states,
+      const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<Scalar>>&
+          imu_meas,
+      const Vec3& gyro_bias_weight, const Vec3& accel_bias_weight,
+      const Vec3& g);
+
+  template <class AccumT>
+  static void linearizeAbs(const MatX& rel_H, const VecX& rel_b,
+                           const RelLinDataBase& rld, const AbsOrderMap& aom,
+                           AccumT& accum) {
+    // int asize = aom.total_size;
+
+    //  BASALT_ASSERT(abs_H.cols() == asize);
+    //  BASALT_ASSERT(abs_H.rows() == asize);
+    //  BASALT_ASSERT(abs_b.rows() == asize);
+
+    for (size_t i = 0; i < rld.order.size(); i++) {
+      const TimeCamId& tcid_h = rld.order[i].first;
+      const TimeCamId& tcid_ti = rld.order[i].second;
+
+      int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
+      int abs_ti_idx = aom.abs_order_map.at(tcid_ti.frame_id).first;
+
+      accum.template addB<POSE_SIZE>(
+          abs_h_idx, rld.d_rel_d_h[i].transpose() *
+                         rel_b.template segment<POSE_SIZE>(i * POSE_SIZE));
+      accum.template addB<POSE_SIZE>(
+          abs_ti_idx, rld.d_rel_d_t[i].transpose() *
+                          rel_b.template segment<POSE_SIZE>(i * POSE_SIZE));
+
+      for (size_t j = 0; j < rld.order.size(); j++) {
+        BASALT_ASSERT(rld.order[i].first == rld.order[j].first);
+
+        const TimeCamId& tcid_tj = rld.order[j].second;
+
+        int abs_tj_idx = aom.abs_order_map.at(tcid_tj.frame_id).first;
+
+        if (tcid_h.frame_id == tcid_ti.frame_id ||
+            tcid_h.frame_id == tcid_tj.frame_id)
+          continue;
+
+        accum.template addH<POSE_SIZE, POSE_SIZE>(
+            abs_h_idx, abs_h_idx,
+            rld.d_rel_d_h[i].transpose() *
+                rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                           POSE_SIZE * j) *
+                rld.d_rel_d_h[j]);
+
+        accum.template addH<POSE_SIZE, POSE_SIZE>(
+            abs_ti_idx, abs_h_idx,
+            rld.d_rel_d_t[i].transpose() *
+                rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                           POSE_SIZE * j) *
+                rld.d_rel_d_h[j]);
+
+        accum.template addH<POSE_SIZE, POSE_SIZE>(
+            abs_h_idx, abs_tj_idx,
+            rld.d_rel_d_h[i].transpose() *
+                rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                           POSE_SIZE * j) *
+                rld.d_rel_d_t[j]);
+
+        accum.template addH<POSE_SIZE, POSE_SIZE>(
+            abs_ti_idx, abs_tj_idx,
+            rld.d_rel_d_t[i].transpose() *
+                rel_H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                           POSE_SIZE * j) *
+                rld.d_rel_d_t[j]);
+      }
+    }
+  }
+
+  template <class AccumT>
+  static void linearizeAbs(const AbsLinData& ald, const AbsOrderMap& aom,
+                           AccumT& accum) {
+    for (size_t i = 0; i < ald.order.size(); i++) {
+      const TimeCamId& tcid_h = ald.order[i].first;
+      const TimeCamId& tcid_ti = ald.order[i].second;
+
+      int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
+      int abs_ti_idx = aom.abs_order_map.at(tcid_ti.frame_id).first;
+
+      const FrameAbsLinData& fald = ald.Hpppl.at(i);
+
+      // Pose H and b part
+      accum.template addH<POSE_SIZE, POSE_SIZE>(abs_h_idx, abs_h_idx,
+                                                fald.Hphph);
+      accum.template addH<POSE_SIZE, POSE_SIZE>(abs_ti_idx, abs_ti_idx,
+                                                fald.Hptpt);
+      accum.template addH<POSE_SIZE, POSE_SIZE>(abs_h_idx, abs_ti_idx,
+                                                fald.Hphpt);
+      accum.template addH<POSE_SIZE, POSE_SIZE>(abs_ti_idx, abs_h_idx,
+                                                fald.Hphpt.transpose());
+
+      accum.template addB<POSE_SIZE>(abs_h_idx, fald.bph);
+      accum.template addB<POSE_SIZE>(abs_ti_idx, fald.bpt);
+
+      // Schur complement for landmark part
+      for (size_t j = 0; j < fald.lm_id.size(); j++) {
+        Eigen::Matrix<Scalar, POSE_SIZE, 3> H_phl_H_ll_inv, H_ptl_H_ll_inv;
+        int lm_id = fald.lm_id.at(j);
+
+        H_phl_H_ll_inv = fald.Hphl[j] * ald.Hllinv.at(lm_id);
+        H_ptl_H_ll_inv = fald.Hptl[j] * ald.Hllinv.at(lm_id);
+
+        accum.template addB<POSE_SIZE>(abs_h_idx,
+                                       -H_phl_H_ll_inv * ald.bl.at(lm_id));
+        accum.template addB<POSE_SIZE>(abs_ti_idx,
+                                       -H_ptl_H_ll_inv * ald.bl.at(lm_id));
+
+        const auto& other_obs = ald.lm_to_obs.at(lm_id);
+
+        for (size_t k = 0; k < other_obs.size(); k++) {
+          int other_frame_idx = other_obs[k].first;
+          int other_lm_idx = other_obs[k].second;
+          const FrameAbsLinData& fald_other = ald.Hpppl.at(other_frame_idx);
+          const TimeCamId& tcid_hk = ald.order.at(other_frame_idx).first;
+          const TimeCamId& tcid_tk = ald.order.at(other_frame_idx).second;
+
+          // Assume same host frame
+          BASALT_ASSERT(tcid_hk.frame_id == tcid_h.frame_id &&
+                        tcid_hk.cam_id == tcid_h.cam_id);
+
+          int abs_tk_idx = aom.abs_order_map.at(tcid_tk.frame_id).first;
+
+          Eigen::Matrix<Scalar, 3, POSE_SIZE> H_l_ph_other =
+              fald_other.Hphl[other_lm_idx].transpose();
+
+          Eigen::Matrix<Scalar, 3, POSE_SIZE> H_l_pt_other =
+              fald_other.Hptl[other_lm_idx].transpose();
+
+          accum.template addH<POSE_SIZE, POSE_SIZE>(
+              abs_h_idx, abs_h_idx, -H_phl_H_ll_inv * H_l_ph_other);
+          accum.template addH<POSE_SIZE, POSE_SIZE>(
+              abs_ti_idx, abs_h_idx, -H_ptl_H_ll_inv * H_l_ph_other);
+          accum.template addH<POSE_SIZE, POSE_SIZE>(
+              abs_h_idx, abs_tk_idx, -H_phl_H_ll_inv * H_l_pt_other);
+          accum.template addH<POSE_SIZE, POSE_SIZE>(
+              abs_ti_idx, abs_tk_idx, -H_ptl_H_ll_inv * H_l_pt_other);
+        }
+      }
+    }
+  }
+
+  template <class AccumT>
+  struct LinearizeAbsReduce {
+    static_assert(std::is_same_v<typename AccumT::Scalar, Scalar>);
+
+    using RelLinConstDataIter =
+        typename Eigen::aligned_vector<RelLinData>::const_iterator;
+
+    LinearizeAbsReduce(const AbsOrderMap& aom) : aom(aom) {
+      accum.reset(aom.total_size);
+    }
+
+    LinearizeAbsReduce(const LinearizeAbsReduce& other, tbb::split)
+        : aom(other.aom) {
+      accum.reset(aom.total_size);
+    }
+
+    void operator()(const tbb::blocked_range<RelLinConstDataIter>& range) {
+      for (const RelLinData& rld : range) {
+        MatX rel_H;
+        VecX rel_b;
+        linearizeRel(rld, rel_H, rel_b);
+
+        linearizeAbs(rel_H, rel_b, rld, aom, accum);
+      }
+    }
+
+    void join(LinearizeAbsReduce& rhs) { accum.join(rhs.accum); }
+
+    const AbsOrderMap& aom;
+    AccumT accum;
+  };
+
+  template <class AccumT>
+  struct LinearizeAbsReduce2 {
+    static_assert(std::is_same_v<typename AccumT::Scalar, Scalar>);
+
+    using AbsLinDataConstIter =
+        typename Eigen::aligned_vector<AbsLinData>::const_iterator;
+
+    LinearizeAbsReduce2(const AbsOrderMap& aom) : aom(aom) {
+      accum.reset(aom.total_size);
+    }
+
+    LinearizeAbsReduce2(const LinearizeAbsReduce2& other, tbb::split)
+        : aom(other.aom) {
+      accum.reset(aom.total_size);
+    }
+
+    void operator()(const tbb::blocked_range<AbsLinDataConstIter>& range) {
+      for (const AbsLinData& ald : range) {
+        linearizeAbs(ald, aom, accum);
+      }
+    }
+
+    void join(LinearizeAbsReduce2& rhs) { accum.join(rhs.accum); }
+
+    const AbsOrderMap& aom;
+    AccumT accum;
+  };
+};
+}  // namespace basalt

include/basalt/vi_estimator/sqrt_ba_base.h

@@ -0,0 +1,124 @@
+/**
+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/ba_base.h>
+#include <basalt/vi_estimator/sc_ba_base.h>
+
+namespace basalt {
+
+template <class Scalar_>
+class SqrtBundleAdjustmentBase : public BundleAdjustmentBase<Scalar_> {
+ public:
+  using Scalar = Scalar_;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using SO3 = Sophus::SO3<Scalar>;
+
+  using RelLinDataBase =
+      typename ScBundleAdjustmentBase<Scalar>::RelLinDataBase;
+  using FrameRelLinData =
+      typename ScBundleAdjustmentBase<Scalar>::FrameRelLinData;
+  using RelLinData = typename ScBundleAdjustmentBase<Scalar>::RelLinData;
+
+  using FrameAbsLinData =
+      typename ScBundleAdjustmentBase<Scalar>::FrameAbsLinData;
+  using AbsLinData = typename ScBundleAdjustmentBase<Scalar>::AbsLinData;
+
+  using BundleAdjustmentBase<Scalar>::computeDelta;
+
+  void linearizeHelper(
+      Eigen::aligned_vector<RelLinData>& rld_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      Scalar& error) const {
+    ScBundleAdjustmentBase<Scalar>::linearizeHelperStatic(rld_vec, obs_to_lin,
+                                                          this, error);
+  }
+
+  void linearizeAbsHelper(
+      Eigen::aligned_vector<AbsLinData>& ald_vec,
+      const std::unordered_map<
+          TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+      Scalar& error) const {
+    ScBundleAdjustmentBase<Scalar>::linearizeHelperAbsStatic(
+        ald_vec, obs_to_lin, this, error);
+  }
+
+  static void linearizeRel(const RelLinData& rld, MatX& H, VecX& b) {
+    ScBundleAdjustmentBase<Scalar>::linearizeRel(rld, H, b);
+  }
+
+  static void updatePoints(const AbsOrderMap& aom, const RelLinData& rld,
+                           const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
+                           Scalar* l_diff = nullptr) {
+    ScBundleAdjustmentBase<Scalar>::updatePoints(aom, rld, inc, lmdb, l_diff);
+  }
+
+  static void updatePointsAbs(const AbsOrderMap& aom, const AbsLinData& ald,
+                              const VecX& inc, LandmarkDatabase<Scalar>& lmdb,
+                              Scalar* l_diff = nullptr) {
+    ScBundleAdjustmentBase<Scalar>::updatePointsAbs(aom, ald, inc, lmdb,
+                                                    l_diff);
+  }
+
+  static Eigen::VectorXd checkNullspace(
+      const MargLinData<Scalar_>& mld,
+      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
+          frame_states,
+      const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
+      bool verbose = true);
+
+  static Eigen::VectorXd checkEigenvalues(const MargLinData<Scalar_>& mld,
+                                          bool verbose = true);
+
+  template <class AccumT>
+  static void linearizeAbs(const MatX& rel_H, const VecX& rel_b,
+                           const RelLinDataBase& rld, const AbsOrderMap& aom,
+                           AccumT& accum) {
+    return ScBundleAdjustmentBase<Scalar>::template linearizeAbs<AccumT>(
+        rel_H, rel_b, rld, aom, accum);
+  }
+
+  template <class AccumT>
+  static void linearizeAbs(const AbsLinData& ald, const AbsOrderMap& aom,
+                           AccumT& accum) {
+    return ScBundleAdjustmentBase<Scalar>::template linearizeAbs<AccumT>(
+        ald, aom, accum);
+  }
+};
+}  // namespace basalt

include/basalt/vi_estimator/sqrt_keypoint_vio.h

@@ -0,0 +1,259 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <thread>
+
+#include <basalt/imu/preintegration.h>
+#include <basalt/utils/time_utils.hpp>
+
+#include <basalt/vi_estimator/sqrt_ba_base.h>
+#include <basalt/vi_estimator/vio_estimator.h>
+
+#include <basalt/imu/preintegration.h>
+
+namespace basalt {
+
+template <class Scalar_>
+class SqrtKeypointVioEstimator : public VioEstimatorBase,
+                                 public SqrtBundleAdjustmentBase<Scalar_> {
+ public:
+  using Scalar = Scalar_;
+
+  typedef std::shared_ptr<SqrtKeypointVioEstimator> Ptr;
+
+  static const int N = 9;
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+  using VecN = Eigen::Matrix<Scalar, N, 1>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using MatN3 = Eigen::Matrix<Scalar, N, 3>;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using SE3 = Sophus::SE3<Scalar>;
+
+  using typename SqrtBundleAdjustmentBase<Scalar>::RelLinData;
+  using typename SqrtBundleAdjustmentBase<Scalar>::AbsLinData;
+
+  using BundleAdjustmentBase<Scalar>::computeError;
+  using BundleAdjustmentBase<Scalar>::get_current_points;
+  using BundleAdjustmentBase<Scalar>::computeDelta;
+  using BundleAdjustmentBase<Scalar>::computeProjections;
+  using BundleAdjustmentBase<Scalar>::triangulate;
+  using BundleAdjustmentBase<Scalar>::backup;
+  using BundleAdjustmentBase<Scalar>::restore;
+  using BundleAdjustmentBase<Scalar>::getPoseStateWithLin;
+  using BundleAdjustmentBase<Scalar>::computeModelCostChange;
+
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeHelper;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeAbsHelper;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeRel;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeAbs;
+  using SqrtBundleAdjustmentBase<Scalar>::updatePoints;
+  using SqrtBundleAdjustmentBase<Scalar>::updatePointsAbs;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeMargPrior;
+  using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorError;
+  using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorModelCostChange;
+  using SqrtBundleAdjustmentBase<Scalar>::checkNullspace;
+  using SqrtBundleAdjustmentBase<Scalar>::checkEigenvalues;
+
+  SqrtKeypointVioEstimator(const Eigen::Vector3d& g,
+                           const basalt::Calibration<double>& calib,
+                           const VioConfig& config);
+
+  void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
+                  const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg,
+                  const Eigen::Vector3d& ba) override;
+
+  void initialize(const Eigen::Vector3d& bg,
+                  const Eigen::Vector3d& ba) override;
+
+  virtual ~SqrtKeypointVioEstimator() { maybe_join(); }
+
+  inline void maybe_join() override {
+    if (processing_thread) {
+      processing_thread->join();
+      processing_thread.reset();
+    }
+  }
+
+  void addIMUToQueue(const ImuData<double>::Ptr& data) override;
+  void addVisionToQueue(const OpticalFlowResult::Ptr& data) override;
+
+  typename ImuData<Scalar>::Ptr popFromImuDataQueue();
+
+  bool measure(const OpticalFlowResult::Ptr& opt_flow_meas,
+               const typename IntegratedImuMeasurement<Scalar>::Ptr& meas);
+
+  // int64_t propagate();
+  // void addNewState(int64_t data_t_ns);
+
+  void optimize_and_marg(const std::map<int64_t, int>& num_points_connected,
+                         const std::unordered_set<KeypointId>& lost_landmaks);
+
+  void marginalize(const std::map<int64_t, int>& num_points_connected,
+                   const std::unordered_set<KeypointId>& lost_landmaks);
+  void optimize();
+
+  void debug_finalize() override;
+
+  void logMargNullspace();
+  Eigen::VectorXd checkMargNullspace() const;
+  Eigen::VectorXd checkMargEigenvalues() const;
+
+  int64_t get_t_ns() const {
+    return frame_states.at(last_state_t_ns).getState().t_ns;
+  }
+  const SE3& get_T_w_i() const {
+    return frame_states.at(last_state_t_ns).getState().T_w_i;
+  }
+  const Vec3& get_vel_w_i() const {
+    return frame_states.at(last_state_t_ns).getState().vel_w_i;
+  }
+
+  const PoseVelBiasState<Scalar>& get_state() const {
+    return frame_states.at(last_state_t_ns).getState();
+  }
+  PoseVelBiasState<Scalar> get_state(int64_t t_ns) const {
+    PoseVelBiasState<Scalar> state;
+
+    auto it = frame_states.find(t_ns);
+
+    if (it != frame_states.end()) {
+      return it->second.getState();
+    }
+
+    auto it2 = frame_poses.find(t_ns);
+    if (it2 != frame_poses.end()) {
+      state.T_w_i = it2->second.getPose();
+    }
+
+    return state;
+  }
+
+  void setMaxStates(size_t val) override { max_states = val; }
+  void setMaxKfs(size_t val) override { max_kfs = val; }
+
+  Eigen::aligned_vector<SE3> getFrameStates() const {
+    Eigen::aligned_vector<SE3> res;
+
+    for (const auto& kv : frame_states) {
+      res.push_back(kv.second.getState().T_w_i);
+    }
+
+    return res;
+  }
+
+  Eigen::aligned_vector<SE3> getFramePoses() const {
+    Eigen::aligned_vector<SE3> res;
+
+    for (const auto& kv : frame_poses) {
+      res.push_back(kv.second.getPose());
+    }
+
+    return res;
+  }
+
+  Eigen::aligned_map<int64_t, SE3> getAllPosesMap() const {
+    Eigen::aligned_map<int64_t, SE3> res;
+
+    for (const auto& kv : frame_poses) {
+      res[kv.first] = kv.second.getPose();
+    }
+
+    for (const auto& kv : frame_states) {
+      res[kv.first] = kv.second.getState().T_w_i;
+    }
+
+    return res;
+  }
+
+  Sophus::SE3d getT_w_i_init() override {
+    return T_w_i_init.template cast<double>();
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  using BundleAdjustmentBase<Scalar>::frame_poses;
+  using BundleAdjustmentBase<Scalar>::frame_states;
+  using BundleAdjustmentBase<Scalar>::lmdb;
+  using BundleAdjustmentBase<Scalar>::obs_std_dev;
+  using BundleAdjustmentBase<Scalar>::huber_thresh;
+  using BundleAdjustmentBase<Scalar>::calib;
+
+ private:
+  bool take_kf;
+  int frames_after_kf;
+  std::set<int64_t> kf_ids;
+
+  int64_t last_state_t_ns;
+  Eigen::aligned_map<int64_t, IntegratedImuMeasurement<Scalar>> imu_meas;
+
+  const Vec3 g;
+
+  // Input
+
+  Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
+
+  std::map<int64_t, int> num_points_kf;
+
+  // Marginalization
+  MargLinData<Scalar> marg_data;
+
+  // Used only for debug and log purporses.
+  MargLinData<Scalar> nullspace_marg_data;
+
+  Vec3 gyro_bias_sqrt_weight, accel_bias_sqrt_weight;
+
+  size_t max_states;
+  size_t max_kfs;
+
+  SE3 T_w_i_init;
+
+  bool initialized;
+  bool opt_started;
+
+  VioConfig config;
+
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
+
+  std::shared_ptr<std::thread> processing_thread;
+
+  // timing and stats
+  ExecutionStats stats_all_;
+  ExecutionStats stats_sums_;
+};
+}  // namespace basalt

include/basalt/vi_estimator/sqrt_keypoint_vo.h

@@ -0,0 +1,249 @@
+/**
+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 <basalt/vi_estimator/sqrt_ba_base.h>
+#include <basalt/vi_estimator/vio_estimator.h>
+
+#include <basalt/utils/time_utils.hpp>
+
+namespace basalt {
+
+template <class Scalar_>
+class SqrtKeypointVoEstimator : public VioEstimatorBase,
+                                public SqrtBundleAdjustmentBase<Scalar_> {
+ public:
+  using Scalar = Scalar_;
+
+  typedef std::shared_ptr<SqrtKeypointVoEstimator> Ptr;
+
+  static const int N = 9;
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+  using VecN = Eigen::Matrix<Scalar, N, 1>;
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using MatN3 = Eigen::Matrix<Scalar, N, 3>;
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using SE3 = Sophus::SE3<Scalar>;
+
+  using typename SqrtBundleAdjustmentBase<Scalar>::RelLinData;
+  using typename SqrtBundleAdjustmentBase<Scalar>::AbsLinData;
+
+  using BundleAdjustmentBase<Scalar>::computeError;
+  using BundleAdjustmentBase<Scalar>::get_current_points;
+  using BundleAdjustmentBase<Scalar>::computeDelta;
+  using BundleAdjustmentBase<Scalar>::computeProjections;
+  using BundleAdjustmentBase<Scalar>::triangulate;
+  using BundleAdjustmentBase<Scalar>::backup;
+  using BundleAdjustmentBase<Scalar>::restore;
+  using BundleAdjustmentBase<Scalar>::getPoseStateWithLin;
+  using BundleAdjustmentBase<Scalar>::computeModelCostChange;
+
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeHelper;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeAbsHelper;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeRel;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeAbs;
+  using SqrtBundleAdjustmentBase<Scalar>::updatePoints;
+  using SqrtBundleAdjustmentBase<Scalar>::updatePointsAbs;
+  using SqrtBundleAdjustmentBase<Scalar>::linearizeMargPrior;
+  using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorError;
+  using SqrtBundleAdjustmentBase<Scalar>::computeMargPriorModelCostChange;
+  using SqrtBundleAdjustmentBase<Scalar>::checkNullspace;
+  using SqrtBundleAdjustmentBase<Scalar>::checkEigenvalues;
+
+  SqrtKeypointVoEstimator(const basalt::Calibration<double>& calib,
+                          const VioConfig& config);
+
+  void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
+                  const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg,
+                  const Eigen::Vector3d& ba) override;
+
+  void initialize(const Eigen::Vector3d& bg,
+                  const Eigen::Vector3d& ba) override;
+
+  virtual ~SqrtKeypointVoEstimator() { maybe_join(); }
+
+  inline void maybe_join() override {
+    if (processing_thread) {
+      processing_thread->join();
+      processing_thread.reset();
+    }
+  }
+
+  void addIMUToQueue(const ImuData<double>::Ptr& data) override;
+  void addVisionToQueue(const OpticalFlowResult::Ptr& data) override;
+
+  bool measure(const OpticalFlowResult::Ptr& opt_flow_meas, bool add_frame);
+
+  // int64_t propagate();
+  // void addNewState(int64_t data_t_ns);
+
+  void optimize_and_marg(const std::map<int64_t, int>& num_points_connected,
+                         const std::unordered_set<KeypointId>& lost_landmaks);
+
+  void marginalize(const std::map<int64_t, int>& num_points_connected,
+                   const std::unordered_set<KeypointId>& lost_landmaks);
+  void optimize();
+
+  void logMargNullspace();
+  Eigen::VectorXd checkMargNullspace() const;
+  Eigen::VectorXd checkMargEigenvalues() const;
+
+  int64_t get_t_ns() const {
+    return frame_states.at(last_state_t_ns).getState().t_ns;
+  }
+  const SE3& get_T_w_i() const {
+    return frame_states.at(last_state_t_ns).getState().T_w_i;
+  }
+  const Vec3& get_vel_w_i() const {
+    return frame_states.at(last_state_t_ns).getState().vel_w_i;
+  }
+
+  const PoseVelBiasState<Scalar>& get_state() const {
+    return frame_states.at(last_state_t_ns).getState();
+  }
+  PoseVelBiasState<Scalar> get_state(int64_t t_ns) const {
+    PoseVelBiasState<Scalar> state;
+
+    auto it = frame_states.find(t_ns);
+
+    if (it != frame_states.end()) {
+      return it->second.getState();
+    }
+
+    auto it2 = frame_poses.find(t_ns);
+    if (it2 != frame_poses.end()) {
+      state.T_w_i = it2->second.getPose();
+    }
+
+    return state;
+  }
+
+  void setMaxStates(size_t val) override { max_states = val; }
+  void setMaxKfs(size_t val) override { max_kfs = val; }
+
+  Eigen::aligned_vector<SE3> getFrameStates() const {
+    Eigen::aligned_vector<SE3> res;
+
+    for (const auto& kv : frame_states) {
+      res.push_back(kv.second.getState().T_w_i);
+    }
+
+    return res;
+  }
+
+  Eigen::aligned_vector<SE3> getFramePoses() const {
+    Eigen::aligned_vector<SE3> res;
+
+    for (const auto& kv : frame_poses) {
+      res.push_back(kv.second.getPose());
+    }
+
+    return res;
+  }
+
+  Eigen::aligned_map<int64_t, SE3> getAllPosesMap() const {
+    Eigen::aligned_map<int64_t, SE3> res;
+
+    for (const auto& kv : frame_poses) {
+      res[kv.first] = kv.second.getPose();
+    }
+
+    for (const auto& kv : frame_states) {
+      res[kv.first] = kv.second.getState().T_w_i;
+    }
+
+    return res;
+  }
+
+  Sophus::SE3d getT_w_i_init() override {
+    return T_w_i_init.template cast<double>();
+  }
+
+  void debug_finalize() override;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  using BundleAdjustmentBase<Scalar>::frame_poses;
+  using BundleAdjustmentBase<Scalar>::frame_states;
+  using BundleAdjustmentBase<Scalar>::lmdb;
+  using BundleAdjustmentBase<Scalar>::obs_std_dev;
+  using BundleAdjustmentBase<Scalar>::huber_thresh;
+  using BundleAdjustmentBase<Scalar>::calib;
+
+ private:
+  bool take_kf;              // true if next frame should become kf
+  int frames_after_kf;       // number of frames since last kf
+  std::set<int64_t> kf_ids;  // sliding window frame ids
+
+  // timestamp of latest state in the sliding window
+  // TODO: check and document when this is equal to kf_ids.rbegin() and when
+  // not. It may be only relevant for VIO, not VO.
+  int64_t last_state_t_ns = -1;
+
+  // Input
+
+  Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
+
+  std::map<int64_t, int> num_points_kf;
+
+  // Marginalization
+  MargLinData<Scalar> marg_data;
+
+  // Used only for debug and log purporses.
+  MargLinData<Scalar> nullspace_marg_data;
+
+  size_t max_states;
+  size_t max_kfs;
+
+  SE3 T_w_i_init;
+
+  bool initialized;
+
+  VioConfig config;
+
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
+
+  std::shared_ptr<std::thread> processing_thread;
+
+  // timing and stats
+  ExecutionStats stats_all_;
+  ExecutionStats stats_sums_;
+};
+}  // namespace basalt

include/basalt/vi_estimator/vio_estimator.h

@@ -93,7 +93,32 @@ class VioEstimatorBase {
   virtual void initialize(const Eigen::Vector3d& bg,
                           const Eigen::Vector3d& ba) = 0;
 
-  virtual const Sophus::SE3d& getT_w_i_init() = 0;
+  virtual void maybe_join() = 0;
+
+  virtual inline void drain_input_queues() {
+    // Input threads should abort when vio is finished, but might be stuck in
+    // full push to full queue. So this can help to drain queues after joining
+    // the processing thread.
+    while (!imu_data_queue.empty()) {
+      ImuData<double>::Ptr d;
+      imu_data_queue.pop(d);
+    }
+    while (!vision_data_queue.empty()) {
+      OpticalFlowResult::Ptr d;
+      vision_data_queue.pop(d);
+    }
+  }
+
+  virtual inline void debug_finalize() {}
+
+  virtual Sophus::SE3d getT_w_i_init() = 0;
+
+  // Legacy functions. Should not be used in the new code.
+  virtual void setMaxStates(size_t val) = 0;
+  virtual void setMaxKfs(size_t val) = 0;
+
+  virtual void addIMUToQueue(const ImuData<double>::Ptr& data) = 0;
+  virtual void addVisionToQueue(const OpticalFlowResult::Ptr& data) = 0;
 };
 
 class VioEstimatorFactory {
@@ -101,7 +126,7 @@ class VioEstimatorFactory {
   static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config,
                                                const Calibration<double>& cam,
                                                const Eigen::Vector3d& g,
-                                               bool use_imu);
+                                               bool use_imu, bool use_double);
 };
 
 double alignSVD(const std::vector<int64_t>& filter_t_ns,

python/basalt/__init__.py

@@ -0,0 +1,9 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#

python/basalt/experiments.py

@@ -0,0 +1,614 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import os
+import re
+import json
+import hashlib
+import pickle
+import toml
+import argparse
+
+from string import Template
+from glob import glob
+from collections.abc import Mapping
+from munch import Munch
+from munch import munchify
+from copy import deepcopy
+from collections import abc
+
+from .run import Run
+
+from .util import copy_subdict
+
+_CURRENT_CACHE_VERSION = '1.3'
+"""cache version that can be incremented to invalidate all cache files in case the format changes"""
+
+
+def version_less(vstr1, vstr2):
+    """Order for sorting versions in the format a.b.c"""
+    return vstr1.split(".") < vstr2.split(".")
+
+
+def compute_caching_hash(d):
+    """Generate a hash from a dictionary to use as a cache file name
+    
+    This is intended to be used for experiments cache files
+    """
+    string = json.dumps(d, sort_keys=True, ensure_ascii=True)
+    h = hashlib.sha1()
+    h.update(string.encode('utf8'))
+    return h.hexdigest()
+
+
+class Experiment:
+    """Holds the logs for one experiment: a single odometry config run on a set of sequences
+    
+    For one experiment, each sequence may have at most one Run.
+    
+    Since for each run we have multiple log files and there may be many runs, we
+    cache the loaded configs / output / log files (after preprocessing) into a single
+    binary cache file (pickle). This significantly speeds up loading results when
+    we have many experiments defined in a single experiments config file.
+    """
+
+    def __init__(self,
+                 log_dirs,
+                 name,
+                 display_name=None,
+                 description=None,
+                 caching_hash=None,
+                 spec=None,
+                 seq_name_mapping=None,
+                 extend=None,
+                 extend_override=False):
+        """Load an experiment and all it's runs from a set of directories
+        
+        There may be no duplicate runs of the same sequence.      
+        
+        :param log_dirs: list of directories to look for runs in
+        :param name: experiment name
+        :param display_name: optional experiment display name
+        :param description: optional experiment description
+        :param caching_hash: own caching hash; mostly used to combine this hash with the has of extending experiments
+        :param spec: the config spec for this experiment; mainly informational for informative error messages; the
+               functionally relevant information has already be extracted an preprocessed (other arguments)
+        :param seq_name_mapping: optional mapping of sequence names; may contain only part of the sequences
+        :param extend: optionally provide base experiment whose runs are copied (and possibly extended) 
+        :param extend_override: if True, sequences in the extended experiment may be replaced, if they are also found in `log_dirs` 
+        """
+
+        self.name = name
+        self.display_name = display_name
+        self.description = description
+        self.caching_hash = caching_hash
+        self.spec = spec
+        self.runs = dict()
+
+        if extend is not None:
+            for k, v in extend.runs.items():
+                self.runs[k] = deepcopy(v)
+
+        seqs_ok_to_override = set(self.runs.keys()) if extend_override else set()
+
+        for d in log_dirs:
+            run = Run(d, seq_name_mapping)
+            if run.seq_name in self.runs:
+                if run.seq_name in seqs_ok_to_override:
+                    seqs_ok_to_override.remove(run.seq_name)  # ok only once
+                else:
+                    if extend is not None and run.seq_name in extend.runs and not extend_override:
+                        raise RuntimeError(
+                            str.format(
+                                "{} appears both in the extended experiment {} and in the extending "
+                                "experiment {} but extend_override is False:\n - {}\n - {}\n", run.seq_name,
+                                extend.name, self.name, extend.runs[run.seq_name].dirpath, run.dirpath))
+                    else:
+                        raise RuntimeError(
+                            str.format(
+                                "{} appears multiple times in experiment {}:\n - {}\n - {}\n"
+                                "Do your experiment pattern(s) '{}' match too many directories? "
+                                "Delete the additional runs or narrow the pattern.", run.seq_name, self.name,
+                                self.runs[run.seq_name].dirpath, run.dirpath, "', '".join(self.spec["pattern"])))
+            self.runs[run.seq_name] = run
+
+    def sequences(self, filter_regex=None):
+        """return list of sequence names found for this experiment
+        
+        :param filter_regex: if provided, return only they sequences that match the regex
+        """
+        if filter_regex is None:
+            return self.runs.keys()
+        else:
+            return [k for k in self.runs.keys() if re.search(filter_regex, k)]
+
+    @staticmethod
+    def load_spec(spec, base_path, cache_dir, seq_name_mapping=None, extra_filter_regex=None, other_specs=[]):
+        """Load a single experiment from logs or cache
+        
+        The cache key is determined by the 'pattern', 'filter_regex' and 'extend' keys
+        in the spec. That means changing the name or display name for example doesn't
+        invalidate the cache. If the experiment is not found in cache, it is loaded from 
+        the run directories and then saved in cache.
+        
+        :param spec: experiment spec from the config file 
+        :param base_path: base folder to search for run dirs in
+        :param cache_dir: cache directory
+        :param seq_name_mapping: optional sequence name mapping
+        :param extra_filter_regex: additional filter to limit the loaded sequences on top of what is defined in the spec; if set, caching is disabled
+        :param other_specs: other experiment specs in case our spec has the 'extend' option defined
+        :return: loaded Experiment object
+        """
+
+        # disable cache if extra filtering
+        if extra_filter_regex is not None:
+            cache_dir = None
+
+        # extending some other experiment:
+        extend = None
+        if "extend" in spec:
+            other_spec = next((s for s in other_specs if s.name == spec.extend), None)
+            if other_spec is None:
+                raise RuntimeError("Experiment {} extends unknown experiment {}.".format(spec.name, spec.extend))
+            extend = Experiment.load_spec(other_spec,
+                                          base_path,
+                                          cache_dir,
+                                          seq_name_mapping=seq_name_mapping,
+                                          extra_filter_regex=extra_filter_regex,
+                                          other_specs=other_specs)
+
+        caching_hash = None
+        if cache_dir:
+            caching_spec = copy_subdict(spec, ["pattern", "filter_regex"])
+            if extend is not None:
+                caching_spec["extend"] = extend.caching_hash
+            caching_hash = compute_caching_hash(caching_spec)
+
+            cache_filename = "experiment-cache-{}.pickle".format(caching_hash)
+            cache_path = os.path.join(cache_dir, cache_filename)
+
+            if os.path.isfile(cache_path):
+                if not spec.overwrite_cache:
+                    with open(cache_path, 'rb') as f:
+                        cache = pickle.load(f)
+                    if cache.version != _CURRENT_CACHE_VERSION:
+                        print("> experiment: {} (cache {} has version {}; expected {})".format(
+                            spec.name, cache_path, cache.version, _CURRENT_CACHE_VERSION))
+                    else:
+                        print("> experiment: {} (from cache: {})".format(spec.name, cache_path))
+                        exp = cache.experiment
+                        # overwrite names according to config
+                        exp.name = spec.name
+                        exp.display_name = spec.display_name
+                        exp.description = spec.description
+                        return exp
+                else:
+                    print("> experiment: {} (overwrite cache: {})".format(spec.name, cache_path))
+            else:
+                print("> experiment: {} (cache doesn't exist: {})".format(spec.name, cache_path))
+        else:
+            print("> experiment: {}".format(spec.name))
+
+        log_dirs = Experiment.get_log_dirs(base_path, spec, filter_regex=extra_filter_regex)
+
+        kwargs = copy_subdict(spec, ["name", "display_name", "description", "extend_override"])
+        exp = Experiment(log_dirs,
+                         caching_hash=caching_hash,
+                         seq_name_mapping=seq_name_mapping,
+                         extend=extend,
+                         spec=deepcopy(spec),
+                         **kwargs)
+
+        if cache_dir:
+            cache = Munch(version=_CURRENT_CACHE_VERSION, experiment=exp, spec=caching_spec)
+            os.makedirs(cache_dir, exist_ok=True)
+            with open(cache_path, 'wb') as f:
+                pickle.dump(cache, f)
+            print("experiment {} -> saved cache {}".format(spec.name, cache_path))
+
+        return exp
+
+    @staticmethod
+    def get_log_dirs(base_path, spec, filter_regex=None):
+        """Return list of run directories given an experiments spec
+        
+        :param base_path: base directory to search in 
+        :param spec: experiment spec, e.g. from an experiments config file
+        :param filter_regex: optional additional regex; limits result to matching paths 
+        :return: list of (filtered) paths (joined with base path)
+        """
+        log_dirs = [d for p in spec.pattern for d in glob(os.path.join(base_path, p)) if Run.is_run_dir(d)]
+        if spec.filter_regex:
+            log_dirs = [d for d in log_dirs if re.search(spec.filter_regex, d)]
+        if filter_regex:
+            log_dirs = [d for d in log_dirs if re.search(filter_regex, d)]
+        return log_dirs
+
+    @staticmethod
+    def load_all(specs, config_file, base_path, cache_dir, seq_name_mapping=None):
+        """Load a set experiments from log files or cache
+        
+        If there is more than one experiment with the same name, an error is raised.
+        
+        :param specs: list of experiments specs, e.g. from a experiments config file
+        :param config_file: experiments config file path (currently unused)
+        :param base_path: base directory relative to which all patterns in experiments are search for
+        :param cache_dir: folder to look for and/or save cached experiments
+        :param seq_name_mapping: optional mapping of sequence names
+        :return: a dict {name: experiment}
+        """
+
+        # Note: Seems saving everything to one cache file isn't much faster than the per-experiments cache...
+        use_combined_cache = False
+
+        # load all from cache
+        if use_combined_cache and cache_dir:
+
+            overwrite_cache_any = any(e.overwrite_cache for e in specs)
+            caching_specs = munchify([{k: v for k, v in s.items() if k not in ["overwrite_cache"]} for s in specs])
+            meta_info = Munch(version=_CURRENT_CACHE_VERSION, options=Munch(base_path=base_path), specs=caching_specs)
+
+            config_filename = os.path.splitext(os.path.basename(config_file))[0]
+            cache_filename = "experiment-cache-{}.pickle".format(config_filename)
+            cache_path = os.path.join(cache_dir, cache_filename)
+
+            if os.path.isfile(cache_path):
+                if not overwrite_cache_any:
+                    with open(cache_path, 'rb') as f:
+                        cached_meta_info = pickle.load(f)
+                        if cached_meta_info == meta_info:
+                            print("> loading from cache: {}".format(cache_path))
+                            exps = pickle.load(f)
+                            return exps
+
+        # load individually
+        exps = dict()
+        for spec in specs:
+            if spec.name in exps:
+                raise RuntimeError("experiment {} is duplicate".format(spec.name))
+            exps[spec.name] = Experiment.load_spec(spec,
+                                                   base_path,
+                                                   cache_dir,
+                                                   seq_name_mapping=seq_name_mapping,
+                                                   other_specs=specs)
+
+        # save all to cache
+        if use_combined_cache and cache_dir:
+            os.makedirs(cache_dir, exist_ok=True)
+            with open(cache_path, 'wb') as f:
+                pickle.dump(meta_info, f)
+                pickle.dump(exps, f)
+            print("> saved cache {}".format(cache_path))
+
+        return exps
+
+
+def load_experiments_config(path, args=None):
+    """Load experiments config file, applying substitutions and setting defaults
+    
+    An experiments config file defines general options, locations of experimental runs,
+    and results sections that define tables and plots to render.
+    
+    Substitutions and templates can be used to more concisely describe repetitive
+    definitions (e.g. generate the same plot for ALL runs of an experiment).
+    
+    :param log_dirs: optional command line arguments to override some values in the config
+    :type log_dirs: Union[dict, argparse.Namespace]
+    """
+
+    config = munchify(toml.load(path))
+
+    # default config:
+    config.setdefault("options", Munch())
+    config.options.setdefault("base_path", "$config_dir")
+    config.options.setdefault("cache_dir", "cache")
+    config.options.setdefault("output_path", "results")
+    config.options.setdefault("filter_regex", None)
+    config.options.setdefault("overwrite_cache", False)
+    config.options.setdefault("show_values_failed_runs", True)
+    config.options.setdefault("screenread", False)
+    config.options.setdefault("import_experiments", [])
+    config.setdefault("seq_name_mapping", dict())
+    config.setdefault("seq_displayname_mapping", dict())
+    config.setdefault("substitutions", [])
+    config.setdefault("templates", [])
+    config.setdefault("experiments", [])
+    config.setdefault("results", [])
+
+    # overrides from command line
+    if isinstance(args, argparse.Namespace):
+        args = vars(args)
+
+    # values
+    for k in ["base_path", "cache_dir", "output_path", "filter_regex"]:
+        if k in args and args[k] is not None:
+            config.options[k] = args[k]
+
+    # positive flags
+    for k in ["overwrite_cache"]:
+        if k in args and args[k]:
+            config.options[k] = True
+
+    # negative flags
+    for k in ["dont_show_values_failed_runs"]:
+        if k in args and args[k]:
+            config.options[k] = False
+
+    # collapse all substitutions into one dict
+    static_subs = dict()
+    for d in config.substitutions:
+        for k, v in d.items():
+            if k in static_subs:
+                raise RuntimeError("substitution {} defined multiple times".format(k))
+            static_subs[k] = v
+
+    # create dictionary from list of templates (index by name)
+    template_definitions = dict()
+    for t in config.templates:
+        template_definitions[t._name] = t
+
+    # substituion helper
+    var_pattern = re.compile(r"\$\{(\w+)\}")  # match '${foo}'
+
+    def substitute(obj, subs):
+        if isinstance(obj, Mapping):
+            # For mappings in general we simply recurse the 'substitute' call for the dict values.
+            # In case the '_template' key is present, we do template expansion.
+            if "_template" in obj:
+                # template expansion
+
+                # single templates can be abbreviated by not putting them in a list
+                # --> put the in list now to make following code the same for either case
+                templates = obj._template if isinstance(obj._template, list) else [obj._template]
+
+                # recurse 'substitute' on non-templated part
+                prototype = {k: substitute(v, subs) for k, v in obj.items() if not k.startswith("_")}
+
+                # loop over all templates
+                result = [Munch()]
+                for tmpl in templates:
+
+                    # which arguments are defined?
+                    args = [k for k in tmpl if not k.startswith("_")]
+
+                    # check template definition
+                    tmpl_def = template_definitions[tmpl._name]
+                    tmpl_args = tmpl_def._arguments if "_arguments" in tmpl_def else []
+                    if set(args) != set(tmpl_args):
+                        raise RuntimeError("Template {} required arguments {}, but supplied {} during expansion".format(
+                            tmpl._name, tmpl_def._arguments, args))
+
+                    # apply template definition to all new objects
+                    tmp = result
+                    result = list()
+                    for new_obj in tmp:
+                        # create substitutions from template arguments (recursing 'substitute' call)
+                        all_argument_combinations = [dict()]  # start with single combination (usual case)
+                        for arg in args:
+                            if isinstance(tmpl[arg], Mapping) and "_argument" in tmpl[arg]:
+                                if tmpl[arg]._argument == "product":
+                                    # given list of alternative argument values: create combination for each of them
+                                    tmp2 = all_argument_combinations
+                                    all_argument_combinations = list()
+                                    for d in tmp2:
+                                        for val in substitute(tmpl[arg]._value, subs):
+                                            d_new = deepcopy(d)
+                                            d_new[arg] = val
+                                            all_argument_combinations.append(d_new)
+                                else:
+                                    raise RuntimeError("arugment type {} not for argument {} not implemented".format(
+                                        tmpl[arg]._argument, arg))
+                            else:
+                                # simple argument: append to all combintations
+                                for d in all_argument_combinations:
+                                    assert (arg not in d)
+                                    d[arg] = substitute(tmpl[arg], subs)
+
+                        # for each argument combination, create substitutions and apply template definition
+                        for expanded_args in all_argument_combinations:
+
+                            subs_with_args = dict(subs)
+                            subs_with_args.update(expanded_args)
+
+                            # merge template definition into result, while recursing substitute call with augmented substitutions
+                            new_obj2 = deepcopy(new_obj)
+                            for k, v in tmpl_def.items():
+                                if not k.startswith("_"):
+                                    # later templates can override keys from earlier ones
+                                    new_obj2[k] = substitute(deepcopy(v), subs_with_args)
+
+                            result.append(new_obj2)
+
+                # do prototype keys last, since they may override template keys (we already recursed)
+                for new_obj in result:
+                    for k, v in prototype.items():
+                        new_obj[k] = deepcopy(v)
+
+                if len(result) == 1:
+                    return new_obj
+                else:
+                    return Munch(_return="splice", _value=result)
+            else:
+                # default case
+                for k, v in obj.items():
+                    obj[k] = substitute(v, subs)
+                return obj
+        elif isinstance(obj, list):
+            # Go over elements of list and recurse the 'substitute' call.
+            # In certain cases the returned value can indicate that we should splice in the resulting list instead of
+            # just inserting it.
+            tmp = list()
+            for v in obj:
+                val = substitute(v, subs)
+                if isinstance(val, dict) and "_return" in val:
+                    if val._return == "splice":
+                        tmp.extend(val._value)
+                    else:
+                        raise RuntimeError("Unknown return type {}".format(val._return))
+                else:
+                    tmp.append(val)
+            return tmp
+        elif isinstance(obj, str):
+            if len(obj) > 2 and obj[0] == "<" and obj[-1] == ">":
+                # if string is '<FOO>', the whole string is replaced by the substitution defined for FOO
+                var = obj[1:-1]
+                if var in subs:
+                    return substitute(subs[var], subs)
+                else:
+                    raise RuntimeError("Unknown substitution <{}>".format(var))
+            else:
+                # otherwise, find occurances of ${FOO} in the string an replace by a string representation
+                # of the substitution defined for FOO
+                obj, n = var_pattern.subn(lambda m: str(subs[m.group(1)]), obj)
+                if n > 0:
+                    # something change --> recurse
+                    return substitute(obj, subs)
+                else:
+                    # no substitution --> just return
+                    return obj
+        else:
+            return obj
+
+    # apply substitutions
+    config.experiments = substitute(config.experiments, static_subs)
+    config.results = substitute(config.results, static_subs)
+
+    # set default values for experiments specs
+    for spec in config.experiments:
+        spec.setdefault("display_name", spec.name)
+        spec.setdefault("description", None)
+        spec.setdefault("filter_regex", config.options.filter_regex)
+        spec.setdefault("overwrite_cache", config.options.overwrite_cache)
+        spec.setdefault("pattern", [])
+        spec.pattern = [spec.pattern] if isinstance(spec.pattern, str) else spec.pattern  # ensure list
+        assert isinstance(spec.pattern, abc.Sequence), "pattern {} in experiment {} is neither string nor list".format(
+            spec.pattern, spec.name)
+
+    # results: backwards-compatibility -- move old sections into 'results'
+    if "results_tables" in config:
+        for spec in config.results_tables:
+            spec["class"] = "results_table"
+            config.results.append(spec)
+        del config["results_tables"]
+
+    if "summarize_sequences_tables" in config:
+        for spec in config.summarize_sequences_tables:
+            spec["class"] = "summarize_sequences_table"
+            config.results.append(spec)
+        del config["summarize_sequences_tables"]
+
+    if "plots" in config:
+        for spec in config.plots:
+            spec["class"] = "plot"
+            config.results.append(spec)
+        del config["plots"]
+
+    if "overview_tables" in config:
+        for spec in config.overview_tables:
+            spec["class"] = "overview_table"
+            config.results.append(spec)
+        del config["overview_tables"]
+
+    # results: default values
+    for spec in config.results:
+        spec.setdefault("class", "results_table")
+
+        # set common default values
+        spec.setdefault("show", True)
+        spec.setdefault("clearpage", spec["class"] == "section")
+        spec.setdefault("filter_regex", None)
+
+        if spec["class"] == "section":
+            spec.setdefault("name", "Section")
+            spec.setdefault("pagewidth", None)
+        elif spec["class"] == "results_table":
+            spec.setdefault("metrics_legend", True)
+            spec.setdefault("escape_latex_header", True)
+            spec.setdefault("rotate_header", True)
+            spec.setdefault("vertical_bars", True)
+            spec.setdefault("export_latex", None)
+            spec.setdefault("color_failed", "red")
+            spec.setdefault("multirow", True)
+            spec.setdefault("override_as_failed", [])
+        elif spec["class"] == "summarize_sequences_table":
+            spec.setdefault("header", "")
+            spec.setdefault("export_latex", None)
+            spec.setdefault("escape_latex_header", True)
+            spec.setdefault("rotate_header", True)
+        elif spec["class"] == "plot":
+            spec.setdefault("plot_ate", False)
+            spec.setdefault("figsize", None)
+            spec.setdefault("title", None)
+            spec.setdefault("reference_experiment", None)
+            spec.setdefault("width", None)
+            spec.setdefault("ylim", Munch())
+            spec.ylim.setdefault("top", None)
+            spec.ylim.setdefault("bottom", None)
+            spec.setdefault("ylim_cost", Munch())
+            spec.ylim_cost.setdefault("top", None)
+            spec.ylim_cost.setdefault("bottom", None)
+            spec.setdefault("ylim_ate", Munch())
+            spec.ylim_ate.setdefault("top", None)
+            spec.ylim_ate.setdefault("bottom", None)
+            spec.setdefault("ylim_tolerance", Munch())
+            spec.ylim_tolerance.setdefault("top", None)
+            spec.ylim_tolerance.setdefault("bottom", None)
+            spec.setdefault("xlim_time", Munch())
+            spec.xlim_time.setdefault("right", None)
+            spec.xlim_time.setdefault("left", None)
+            spec.setdefault("xlim_time_fastest", Munch())
+            spec.xlim_time_fastest.setdefault("right", None)
+            spec.xlim_time_fastest.setdefault("left", None)
+            spec.setdefault("xlim_it", Munch())
+            spec.xlim_it.setdefault("right", None)
+            spec.xlim_it.setdefault("left", None)
+            spec.setdefault("xlimits", Munch())
+            spec.xlimits.setdefault("right", None)
+            spec.xlimits.setdefault("left", None)
+            spec.setdefault("legend_loc", "best")
+            spec.setdefault("align_fraction", None)
+            spec.setdefault("layout", "horizontal")
+            spec.setdefault("extend_x", False)
+            if "problem_size_variants" not in spec and "memory_variants" in spec:
+                # legacy support for "memory_variants"
+                spec.problem_size_variants = spec.memory_variants
+                del spec["memory_variants"]
+            spec.setdefault("problem_size_variants", ["cam", "lm", "obs"])
+            spec.setdefault("bal_cost_include", ["cost_time", "cost_it", "tr_radius", "inner_it", "memory"])
+            spec.setdefault("tolerances", [0.01, 0.001, 0.00001])
+            spec.setdefault("plot_tolerances", False)
+            spec.setdefault("best_fit_line", True)
+            spec.setdefault("reverse_zorder", False)
+            spec.setdefault("plot_cost_semilogy", True)
+            spec.setdefault("marker_size", 8)
+            spec.setdefault("ylabel", True)
+            spec.setdefault("suptitle", None)
+            spec.setdefault("rotate2d", 0)
+            spec.setdefault("trajectory_axes", "xy")
+        elif spec["class"] == "overview_table":
+            spec.setdefault("export_latex", None)
+
+    # expand templates in path names
+    template_args = dict(config_dir=os.path.dirname(os.path.abspath(path)))
+    for key in ["base_path", "output_path", "cache_dir"]:
+        config.options[key] = Template(config.options[key]).substitute(**template_args)
+    if isinstance(config.options.import_experiments, str):
+        config.options.import_experiments = [config.options.import_experiments]
+    config.options.import_experiments = [
+        Template(path).substitute(**template_args) for path in config.options.import_experiments
+    ]
+
+    # import experiments
+    imported_experiments = []
+    for path in config.options.import_experiments:
+        cfg = load_experiments_config(path, args)
+        imported_experiments.extend(cfg.experiments)
+    config.experiments = imported_experiments + config.experiments
+
+    return config

python/basalt/generate_tables.py

@@ -0,0 +1,143 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import argparse
+import os
+
+from pylatex import Document, Section, Package, NewPage
+from pylatex import Command
+from pylatex.base_classes import Arguments
+
+from .experiments import load_experiments_config
+from .experiments import Experiment
+from .latex.templates import screenread_sty
+from .util import os_open_file
+from .latex.results_table import ResultsTable
+from .latex.summarize_sequences_table import SummarizeSequencesTable
+from .latex.plot import Plot
+
+
+def generate_tables(args):
+
+    if args.change_directory:
+        os.chdir(args.change_directory)
+
+    config = load_experiments_config(args.config, args)
+
+    exps = Experiment.load_all(config.experiments,
+                               config_file=args.config,
+                               base_path=config.options.base_path,
+                               cache_dir=config.options.cache_dir,
+                               seq_name_mapping=config.seq_name_mapping)
+
+    doc = Document(geometry_options={"tmargin": "1cm", "lmargin": "1cm"})
+
+    export_basepath = "{}-export".format(config.options.output_path)
+
+    curr = doc
+
+    hide_all = False
+    for spec in config.results:
+        if spec.show:
+
+            if spec["class"] == "section":
+                if spec.clearpage:
+                    curr.append(NewPage())
+                if spec.pagewidth:
+                    curr.append(Command("SetPageScreenWidth", Arguments(spec.pagewidth)))
+                else:
+                    curr.append(Command("RestorePageScreenWidth"))
+                hide_all = False
+                curr = Section(spec.name)
+                doc.append(curr)
+                continue
+
+            if hide_all:
+                continue
+
+            if spec.clearpage:
+                curr.append(NewPage())
+
+            elif spec["class"] == "results_table":
+                elem = ResultsTable(exps,
+                                    spec,
+                                    show_values_failed_runs=config.options.show_values_failed_runs,
+                                    seq_displayname_mapping=config.seq_displayname_mapping,
+                                    export_basepath=export_basepath)
+            elif spec["class"] == "summarize_sequences_table":
+                elem = SummarizeSequencesTable(exps,
+                                               spec,
+                                               show_values_failed_runs=config.options.show_values_failed_runs,
+                                               seq_displayname_mapping=config.seq_displayname_mapping,
+                                               export_basepath=export_basepath)
+            elif spec["class"] == "plot":
+                elem = Plot(exps,
+                            spec,
+                            seq_displayname_mapping=config.seq_displayname_mapping,
+                            export_basepath=export_basepath)
+            else:
+                raise RuntimeError("Invalid results class {}".format(spec["class"]))
+
+            curr.append(elem)
+        else:
+            if spec["class"] == "section":
+                hide_all = True
+                continue
+
+    # generate auxiliary tex files
+    if config.options.screenread:
+        output_dir = os.path.dirname(config.options.output_path)
+        screenread_path = output_dir + "/screenread.sty"
+        with open(screenread_path, "w") as f:
+            f.write(screenread_sty)
+        doc.packages.add(Package('screenread'))
+
+    # create nofloatfigure environment
+    doc.preamble.append(
+        Command("newenvironment", Arguments("nofloatfigure", Command("captionsetup", Arguments(type="figure")), "")))
+    doc.packages.add(Package('caption'))
+    doc.packages.add(Package('mathtools'))
+
+    # render latex
+    doc.generate_pdf(config.options.output_path, clean_tex=not args.dont_clean_tex)
+
+    # cleanup
+    if config.options.screenread and not args.dont_clean_tex:
+        os.remove(screenread_path)
+
+    # open the generated pdf
+    if args.open:
+        os_open_file(config.options.output_path + ".pdf")
+
+
+def main():
+
+    parser = argparse.ArgumentParser("Load basalt experiment logs and generate result tables and plots.")
+    parser.add_argument("-C",
+                        "--change-directory",
+                        default=None,
+                        help="Change directory to this folder before doing anything else")
+    parser.add_argument("--config", default="experiments.toml", help="specs for experiments to load")
+    parser.add_argument("--base-path", default=None, help="overwrite basepath for loading logs defined in the config")
+    parser.add_argument("--output-path", default=None, help="output filepath")
+    parser.add_argument("--dont-clean-tex", action="store_true", help="don't remove tex file after generation")
+    parser.add_argument("--cache-dir", default=None, help="load/save experiments cache from/to give folder")
+    parser.add_argument("--overwrite-cache", action="store_true", help="reload all experiments independent of cache")
+    parser.add_argument("--dont-show-values-failed-runs",
+                        action="store_true",
+                        help="don't attempt to show values for failed logs based on partial logs")
+    parser.add_argument("--open", action="store_true", help="open after generation")
+
+    args = parser.parse_args()
+
+    generate_tables(args)
+
+
+if __name__ == "__main__":
+    main()

python/basalt/latex/containers.py

@@ -0,0 +1,109 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import math
+
+from pylatex import Package
+from pylatex.base_classes import Container
+
+from ..metric import metrics_from_config
+from ..metric import ExperimentSpec
+from ..util import alphanum
+
+
+class MyContainer(Container):
+
+    def __init__(self):
+        super().__init__()
+        # add packages that seem to not propagate properly from added elements
+        self.packages.add(Package("xcolor"))
+        self.packages.add(Package('graphicx'))
+
+    def dumps(self):
+        return self.dumps_content()
+
+
+class ExperimentsContainer(MyContainer):
+
+    def __init__(self, seq_displayname_mapping):
+        super().__init__()
+
+        self.seq_displayname_mapping = seq_displayname_mapping
+
+    def seq_displayname(self, seq):
+        return self.seq_displayname_mapping.get(seq, seq)
+
+
+class ExperimentsTable(ExperimentsContainer):
+
+    def __init__(self, exps, spec, show_values_failed_runs, seq_displayname_mapping, export_basepath):
+        super().__init__(seq_displayname_mapping)
+        self.exps = exps
+        self.spec = spec
+        self.show_values_failed_runs = show_values_failed_runs
+        self.export_basepath = export_basepath
+
+        self.experiment_specs = [ExperimentSpec(s) for s in self.spec.experiments]
+        self.metrics = metrics_from_config(self.spec.metrics)
+
+        self.seq_names = self.sequence_names([s.name for s in self.experiment_specs])
+        self.num_seqs = len(self.seq_names)
+        self.num_metrics = len(self.metrics)
+        self.num_exps = len(self.experiment_specs)
+
+    def sequence_names(self, experiment_names):
+        seq_names = set()
+        for s in experiment_names:
+            seq_names.update(self.exps[s].sequences(filter_regex=self.spec.filter_regex))
+
+        return sorted(seq_names, key=alphanum)
+
+    def is_failed(self, exp, seq):
+        if seq not in exp.runs:
+            return True
+        return exp.runs[seq].is_failed()
+
+    def render_failure(self, exp, seq):
+        if seq in self.spec.override_as_failed:
+            return "x"
+
+        if seq not in exp.runs:
+            return '?'
+        run = exp.runs[seq]
+
+        treat_as_failed = (run.log is None) if self.show_values_failed_runs else run.is_failed()
+
+        if treat_as_failed:
+            return run.failure_str()
+        else:
+            return None
+
+    def get_metrics(self, exp, seq, it):
+        if seq not in exp.runs:
+            return [math.nan for _ in self.metrics]
+        run = exp.runs[seq]
+
+        treat_as_failed = (run.log is None) if self.show_values_failed_runs else run.is_failed()
+
+        if treat_as_failed:
+            return [math.nan for _ in self.metrics]
+
+        return [m.get_value(self.exps, exp, seq, it) for m in self.metrics]
+        # try:
+        #     return [m.get_value(self.exps, exp, seq, it) for m in self.metrics]
+        # except AttributeError as e:
+        #     if e.args[0].startswith("local_error"):
+        #         if not has_imported_sophus():
+        #             print("To use local-error, you need to install sophuspy and flush the cache.")
+        #             sys.exit(1)
+        #         if not exp.runs[seq].log.has_cam_pos:
+        #             print("You cannot use local-error for experiment {}, which has no camera positions in the log.".
+        #                   format(exp.name))
+        #             sys.exit(1)
+        #     raise

python/basalt/latex/plot.py

@@ -0,0 +1,393 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import numpy as np
+import os
+import math
+import functools
+
+import matplotlib
+
+matplotlib.use('Agg')  # Not to use X server. For TravisCI.
+import matplotlib.pyplot as plt  # noqa
+from matplotlib.ticker import MaxNLocator
+
+prop_cycle = plt.rcParams['axes.prop_cycle']
+
+#default_cycler = (cycler(linestyle=['-', '--', ':', '-.']) *
+#                  cycler(color=prop_cycle.by_key()['color']))
+
+
+class ModulusList(list):
+
+    def __init__(self, *args, **kwargs):
+        list.__init__(self, *args, **kwargs)
+
+    def __getitem__(self, key):
+        return list.__getitem__(self, key % len(self))
+
+
+default_colors_finite = prop_cycle.by_key()['color']
+default_colors_finite[0] = prop_cycle.by_key()['color'][0]
+default_colors_finite[1] = prop_cycle.by_key()['color'][2]
+default_colors_finite[2] = prop_cycle.by_key()['color'][3]
+default_colors_finite[3] = prop_cycle.by_key()['color'][1]
+
+default_colors = ModulusList(default_colors_finite)
+#default_lines = ModulusList(["-", "-", ":", "--", "-.", ":", "--", "-."])
+#default_markers = ModulusList(["o", "s", "^", "X", "D", "P", "v", "h"])
+default_lines = ModulusList([":", "-", "-.", "--", ":", "--", "-.", "-"])
+default_markers = ModulusList(["o", "s", "^", "X", "D", "P", "v", "h"])
+
+from collections import deque
+from collections import defaultdict
+
+from pylatex import Figure
+from pylatex.utils import NoEscape
+
+from .containers import ExperimentsContainer
+from .util import rotation2d
+
+
+class NoFloatFigure(Figure):
+    pass
+
+
+class Plot(ExperimentsContainer):
+
+    def __init__(self, exps, spec, seq_displayname_mapping, export_basepath):
+        super().__init__(seq_displayname_mapping)
+
+        self.width = None
+
+        plotters = dict(nullspace=self.plot_nullspace,
+                        eigenvalues=self.plot_eigenvalues,
+                        trajectory=self.plot_trajectory)
+
+        plot_fn = plotters[spec.type]
+        plot_fn(exps, spec)
+
+        if spec.width is not None:
+            self.width = spec.width
+        elif self.width is None:
+            self.width = 1
+
+        plt.tight_layout()
+
+        saved_file = self._save_plot(spec, export_basepath)
+
+        if "sequence" in spec:
+            plot_name = '{} {} {}'.format(spec.type, spec.name, spec.sequence).replace("_", " ")
+        else:
+            plot_name = '{} {}'.format(spec.type, spec.name).replace("_", " ")
+
+        #with self.create(Subsection(spec.name, numbering=False)) as p:
+        with self.create(NoFloatFigure()) as f:
+            f.add_image(os.path.abspath(saved_file), width=NoEscape(r'{}\textwidth'.format(self.width)))
+            f.add_caption(plot_name)
+
+        # cleanup
+        plt.close('all')
+
+    def plot_nullspace(self, exps, spec):
+
+        logs = [exps[e].runs[spec.sequence].log for e in spec.experiments]
+        names = [exps[e].display_name for e in spec.experiments]
+
+        num_plots = len(names)
+
+        if num_plots == 4:
+            if True:
+                if spec.figsize is None:
+                    spec.figsize = [10, 2.5]
+                fig, axs = plt.subplots(1, 4, figsize=spec.figsize, sharey=True)
+            else:
+                if spec.figsize is None:
+                    spec.figsize = [10, 4.7]
+                fig, axs = plt.subplots(2, 2, figsize=spec.figsize, sharey=True)
+                axs = axs.flatten()
+        else:
+            if spec.figsize is None:
+                spec.figsize = [6, 2 * num_plots]
+            fig, axs = plt.subplots(num_plots, 1, figsize=spec.figsize, sharey=True)
+
+        if num_plots == 1:
+            axs = [axs]
+
+        for i, (log, name) in enumerate(zip(logs, names)):
+
+            if log is None:
+                continue
+
+            ax = axs[i]
+
+            ns = log.sums.marg_ns[1:]  # skip first prior, which just is all 0
+            ns = np.abs(ns)  # cost change may be negative, we are only interested in the norm
+            ns = np.maximum(ns, 1e-20)  # clamp at very small value
+
+            markerfacecolor = "white"
+
+            markevery = 1000
+            if spec.sequence == "kitti10":
+                markevery = 100
+
+            ax.semilogy(
+                ns[:, 0],
+                ":",
+                # label="x",
+                color="tab:blue")
+            ax.semilogy(
+                ns[:, 1],
+                ":",
+                # label="y",
+                color="tab:blue")
+            ax.semilogy(
+                ns[:, 2],
+                ":",
+                # label="z",
+                label="x, y, z",
+                color="tab:blue",
+                marker="o",
+                markerfacecolor=markerfacecolor,
+                markevery=(markevery // 2, markevery))
+
+            ax.semilogy(
+                ns[:, 3],
+                ":",
+                # label="roll",
+                color="tab:orange")
+            ax.semilogy(
+                ns[:, 4],
+                ":",
+                # label="pitch",
+                label="roll, pitch",
+                color="tab:orange",
+                marker="s",
+                markerfacecolor=markerfacecolor,
+                markevery=(markevery // 2, markevery))
+
+            ax.semilogy(ns[:, 5],
+                        ":",
+                        label="yaw",
+                        color="tab:green",
+                        marker="^",
+                        markerfacecolor=markerfacecolor,
+                        markevery=(0, markevery))
+
+            ax.semilogy(ns[:, 6],
+                        ":",
+                        label="random",
+                        color="tab:red",
+                        marker="D",
+                        markerfacecolor=markerfacecolor,
+                        markevery=(0, markevery))
+
+            # marker on top of lines;
+
+            ax.semilogy(ns[:, 2],
+                        color="None",
+                        marker="o",
+                        markerfacecolor=markerfacecolor,
+                        markeredgecolor="tab:blue",
+                        markevery=(markevery // 2, markevery))
+            ax.semilogy(ns[:, 4],
+                        color="None",
+                        marker="s",
+                        markerfacecolor=markerfacecolor,
+                        markeredgecolor="tab:orange",
+                        markevery=(markevery // 2, markevery))
+
+            #ax.set_yscale("symlog", linthresh=1e-12)
+
+            ax.set_title(name)
+
+            ax.set_yticks([1e-17, 1e-12, 1e-7, 1e-2, 1e3, 1e8])
+
+            if spec.sequence == "kitti10":
+                ax.set_xticks([i * 100 for i in range(4)])
+                #ax.xaxis.set_minor_locator(matplotlib.ticker.FixedLocator([i * 100 + 50 for i in range(4)]))
+
+            if i == 0:
+                ax.set_ylabel("$\\Delta E_m$", rotation=0)
+                ax.yaxis.set_label_coords(-0.05, 1.05)
+
+            if i == num_plots - 1:
+                ax.legend(loc=spec.legend_loc)
+
+            if spec.ylim.top is not None:
+                ax.set_ylim(top=spec.ylim.top)
+            if spec.ylim.bottom is not None:
+                ax.set_ylim(bottom=spec.ylim.bottom)
+
+        if spec.suptitle:
+            fig.suptitle(spec.suptitle)
+
+    def plot_eigenvalues(self, exps, spec):
+
+        logs = [exps[e].runs[spec.sequence].log for e in spec.experiments]
+        names = [exps[e].display_name for e in spec.experiments]
+
+        num_plots = 1
+
+        if spec.figsize is None:
+            spec.figsize = [5.2, 2 * num_plots]
+
+        fig, axs = plt.subplots(num_plots, 1, figsize=spec.figsize)
+
+        ax = axs
+
+        for i, (log, name) in enumerate(zip(logs, names)):
+            if log is not None:
+                min_ev = [np.min(e) for e in log.sums.marg_ev[1:]]
+                #ax.plot(min_ev, ":", label=name, color=default_colors[i])
+                ax.plot(min_ev, default_lines[i], label=name, color=default_colors[i])
+
+        ax.set_yscale("symlog", linthresh=1e-8)
+        ax.legend(loc=spec.legend_loc)
+
+        #ax.set_title("smallest eigenvalue {} {}".format(name, spec.sequence))
+
+        if spec.sequence == "eurocMH01":
+            ax.set_xticks([i * 1000 for i in range(4)])
+            ax.xaxis.set_minor_locator(matplotlib.ticker.FixedLocator([i * 1000 + 500 for i in range(4)]))
+
+        if spec.sequence == "kitti10":
+            ax.set_xticks([i * 100 for i in range(4)])
+            ax.xaxis.set_minor_locator(matplotlib.ticker.FixedLocator([i * 100 + 50 for i in range(4)]))
+
+        ax.set_yticks([-1e4, -1e-4, 0.0, 1e-4, 1e4])
+        ax.set_ylim(bottom=-1e8, top=1e8)
+        # ax.yaxis.tick_right()
+        ax.set_ylabel("$\\sigma_{min}$", rotation=0)
+        ax.yaxis.set_label_coords(0, 1.05)
+
+        if spec.ylim.top is not None:
+            ax.set_ylim(top=spec.ylim.top)
+        if spec.ylim.bottom is not None:
+            ax.set_ylim(bottom=spec.ylim.bottom)
+
+        if spec.suptitle:
+            fig.suptitle(spec.suptitle)
+
+    def plot_trajectory(self, exps, spec):
+
+        #self.width = 1.5
+
+        runs = [exps[e].runs[spec.sequence] for e in spec.experiments]
+        names = [exps[e].display_name for e in spec.experiments]
+
+        linewidth_factor = 3
+
+        R = rotation2d(spec.rotate2d)
+
+        traj_axes_idx = self._axes_spec_to_index(spec.trajectory_axes)
+
+        if spec.figsize is None:
+            spec.figsize = [6.4, 4.8]
+
+        fig, ax = plt.subplots(figsize=spec.figsize)
+
+        ax.axis("equal")
+        ax.axis('off')
+        #ax.set_xlabel("x")
+        #ax.set_ylabel("y")
+
+        gt_color = "tab:grey"
+        #gt_color = "black"
+
+        # take gt-trajectory from first experiment:
+        if runs[0].traj_gt is not None:
+            gt = runs[0].traj_gt[:, traj_axes_idx].T
+            gt = np.matmul(R, gt)
+            ax.plot(gt[0, :],
+                    gt[1, :],
+                    '-',
+                    zorder=1,
+                    linewidth=1 * linewidth_factor,
+                    color=gt_color,
+                    label="ground truth")
+
+        # https://matplotlib.org/stable/gallery/color/named_colors.html
+        linestyles = [":", ":", "--", "-"]
+        colors = [default_colors[1], default_colors[3]]
+        #colors = ["tab:blue", "tab:orange"]
+        linewidths = [2, 1]
+
+        for i, (r, name) in enumerate(zip(runs, names)):
+            # plot in decreasing zorder
+            #zorder = len(runs) - i + 1
+
+            zorder = i + 2
+
+            if r.traj_est is not None:
+                pos = r.traj_est[:, traj_axes_idx].T
+                pos = np.matmul(R, pos)
+                ax.plot(
+                    pos[0, :],
+                    pos[1, :],
+                    linestyles[i],
+                    #default_lines[i],
+                    zorder=zorder,
+                    linewidth=linewidths[i] * linewidth_factor,
+                    label=name,
+                    color=colors[i])
+
+        #ax.set_xlim(np.min(x_gt), np.max(x_gt))
+        #ax.set_ylim(np.min(y_gt), np.max(y_gt))
+
+        #lines = [gt]
+        #colors = ['black']
+        #line_segments = LineCollection(lines, colors=colors, linestyle='solid')
+
+        #ax.add_collection(line_segments)
+
+        ax.legend(loc=spec.legend_loc)
+
+        if spec.title is not None:
+            ax.set_title(spec.title.format(sequence=self.seq_displayname(spec.sequence)))
+
+    @staticmethod
+    def _axes_spec_to_index(axes_spec):
+        index = []
+        assert len(axes_spec) == 2, "Inalid axes_spec {}".format(axes_spec)
+        for c in axes_spec:
+            if c == "x":
+                index.append(0)
+            elif c == "y":
+                index.append(1)
+            elif c == "z":
+                index.append(2)
+            else:
+                assert False, "Inalid axes_spec {}".format(axes_spec)
+        return index
+
+    # static:
+    filename_counters = defaultdict(int)
+
+    def _save_plot(self, spec, basepath, extension=".pdf"):
+
+        os.makedirs(basepath, exist_ok=True)
+
+        if "sequence" in spec:
+            filename = '{}_{}_{}'.format(spec.type, spec.name, spec.sequence)
+        else:
+            filename = '{}_{}'.format(spec.type, spec.name)
+
+        filename = filename.replace(" ", "_").replace("/", "_")
+
+        Plot.filename_counters[filename] += 1
+        counter = Plot.filename_counters[filename]
+        if counter > 1:
+            filename = "{}-{}".format(filename, counter)
+
+        filepath = os.path.join(basepath, "{}.{}".format(filename, extension.strip('.')))
+
+        plt.savefig(filepath)
+
+        return filepath

python/basalt/latex/results_table.py

@@ -0,0 +1,163 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import numbers
+import os
+import math
+
+import numpy as np
+
+from pylatex import Subsection, Tabular, TextColor
+from pylatex import MultiRow, FootnoteText
+from pylatex.utils import italic, bold, NoEscape, escape_latex, dumps_list
+
+from .containers import ExperimentsTable
+from .util import format_ratio_percent
+from .util import best_two_non_repeating
+
+
+class ResultsTable(ExperimentsTable):
+
+    def __init__(self, exps, spec, show_values_failed_runs, seq_displayname_mapping, export_basepath):
+        super().__init__(exps, spec, show_values_failed_runs, seq_displayname_mapping, export_basepath)
+
+        self.doit()
+
+    def doit(self):
+
+        is_multirow = self.num_metrics > 1 and self.spec.multirow
+
+        def render_metric(value, best, second, decimals, format_string, highlight_top, relative_to):
+            if isinstance(value, numbers.Number):
+                if relative_to is None or relative_to == 0 or not np.isfinite(relative_to):
+                    # absolute number
+                    rendered = format_string.format(value, prec=decimals)
+                else:
+                    # percent
+                    rendered = format_ratio_percent(value, relative_to, decimals=decimals)
+                if highlight_top:
+                    if value == best:
+                        rendered = bold(rendered)
+                    elif value == second:
+                        rendered = italic(rendered)
+                return rendered
+            else:
+                return value
+
+        if self.spec.export_latex:
+            row_height = None
+        else:
+            row_height = 0.65 if is_multirow and self.num_metrics >= 3 else 1
+
+        column_spec = '|r' if self.spec.vertical_bars else 'r'
+        t = Tabular('l' + column_spec * self.num_exps, row_height=row_height, pos=['t'])
+        escape_header_fun = lambda text: text if self.spec.escape_latex_header else NoEscape(text)
+        if self.spec.rotate_header:
+            t.add_row([''] + [
+                NoEscape(r"\rotatebox{90}{%s}" % escape_latex(escape_header_fun(s.display_name(self.exps[s.name]))))
+                for s in self.experiment_specs
+            ])
+        else:
+            t.add_row([''] + [escape_header_fun(s.display_name(self.exps[s.name])) for s in self.experiment_specs])
+        t.add_hline()
+
+        for seq in self.seq_names:
+            fails = [self.is_failed(self.exps[s.name], seq) for s in self.experiment_specs]
+            failure_strings = [self.render_failure(self.exps[s.name], seq) for s in self.experiment_specs]
+            values = np.array([self.get_metrics(self.exps[s.name], seq, s.it) for s in self.experiment_specs])
+
+            top_values = list(range(self.num_metrics))
+            for c, m in enumerate(self.metrics):
+                try:
+                    values[:, c] = np.around(values[:, c], m.decimals)
+                except IndexError:
+                    pass
+                non_excluded_values = np.array(values[:, c])
+                for i in m.exclude_columns_highlight:
+                    non_excluded_values[i] = math.nan
+                top_values[c] = best_two_non_repeating(non_excluded_values, reverse=m.larger_is_better)
+
+            if is_multirow:
+                rows = [[MultiRow(self.num_metrics, data=self.seq_displayname(seq))]
+                       ] + [list(['']) for _ in range(1, self.num_metrics)]
+            else:
+                rows = [[self.seq_displayname(seq)]]
+            for c, (fail, failure_str, value_col) in enumerate(zip(fails, failure_strings, values)):
+                if failure_str is not None:
+                    if self.spec.color_failed:
+                        failure_str = TextColor(self.spec.color_failed, failure_str)
+                    if is_multirow:
+                        rows[0].append(MultiRow(self.num_metrics, data=failure_str))
+                        for r in range(1, self.num_metrics):
+                            rows[r].append('')
+                    else:
+                        rows[0].append(failure_str)
+                else:
+                    tmp_data = [None] * self.num_metrics
+                    for r, m in enumerate(self.metrics):
+                        if m.failed_threshold and value_col[r] > m.failed_threshold:
+                            obj = "x"
+                            if self.spec.color_failed:
+                                obj = TextColor(self.spec.color_failed, obj)
+                        else:
+                            relative_to = None
+                            if m.relative_to_column is not None and m.relative_to_column != c:
+                                relative_to = values[m.relative_to_column, r]
+                            obj = render_metric(value_col[r],
+                                                top_values[r][0],
+                                                top_values[r][1],
+                                                m.effective_display_decimals(),
+                                                m.format_string,
+                                                m.highlight_top,
+                                                relative_to=relative_to)
+                            if fail and self.spec.color_failed:
+                                obj = TextColor(self.spec.color_failed, obj)
+                        tmp_data[r] = obj
+                    if self.num_metrics == 1 or is_multirow:
+                        for r, obj in enumerate(tmp_data):
+                            rows[r].append(obj)
+                    else:
+                        entry = []
+                        for v in tmp_data:
+                            entry.append(v)
+                            entry.append(NoEscape("~/~"))
+                        entry.pop()
+                        rows[0].append(dumps_list(entry))
+
+            for row in rows:
+                t.add_row(row)
+
+            if is_multirow:
+                t.add_hline()
+
+        if self.spec.export_latex:
+            os.makedirs(self.export_basepath, exist_ok=True)
+            t.generate_tex(os.path.join(self.export_basepath, self.spec.export_latex))
+
+        with self.create(Subsection(self.spec.name, numbering=False)) as p:
+
+            if self.spec.metrics_legend:
+                legend = Tabular('|c|', row_height=row_height, pos=['t'])
+                legend.add_hline()
+                legend.add_row(["Metrics"])
+                legend.add_hline()
+                for m in self.metrics:
+                    legend.add_row([m.display_name])
+                legend.add_hline()
+
+                tab = Tabular("ll")
+                tab.add_row([t, legend])
+                content = tab
+            else:
+                content = t
+
+            if True:
+                content = FootnoteText(content)
+
+            p.append(content)

python/basalt/latex/summarize_sequences_table.py

@@ -0,0 +1,88 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import numbers
+import os
+import scipy.stats
+import numpy as np
+
+from pylatex import Subsection, FootnoteText, Tabular, NoEscape, escape_latex
+from pylatex.utils import italic, bold
+
+from .containers import ExperimentsTable
+from .util import best_two_non_repeating
+
+
+class SummarizeSequencesTable(ExperimentsTable):
+
+    def __init__(self, exps, spec, show_values_failed_runs, seq_displayname_mapping, export_basepath):
+        super().__init__(exps, spec, show_values_failed_runs, seq_displayname_mapping, export_basepath)
+
+        self.doit()
+
+    def doit(self):
+
+        def render_metric(value, best, second, decimals, format_string):
+            if isinstance(value, numbers.Number):
+                rendered = format_string.format(value, prec=decimals)
+                if value == best:
+                    rendered = bold(rendered)
+                elif value == second:
+                    rendered = italic(rendered)
+                return rendered
+            else:
+                return value
+
+        values = np.empty((self.num_metrics, self.num_seqs, self.num_exps))
+
+        for i, seq in enumerate(self.seq_names):
+            for j, s in enumerate(self.experiment_specs):
+                values[:, i, j] = np.array(self.get_metrics(self.exps[s.name], seq, s.it))
+
+        means = np.empty((self.num_metrics, self.num_exps))
+        for i, m in enumerate(self.metrics):
+            if m.geometric_mean:
+                means[i, :] = scipy.stats.gmean(values[i, :, :], axis=0)
+            else:
+                means[i, :] = np.mean(values[i, :, :], axis=0)
+
+        t = Tabular('l' + 'c' * self.num_exps)
+
+        t.add_hline()
+        escape_header_fun = lambda text: text if self.spec.escape_latex_header else NoEscape(text)
+        if self.spec.rotate_header:
+            t.add_row([self.spec.header] + [
+                NoEscape(r"\rotatebox{90}{%s}" % escape_latex(escape_header_fun(s.display_name(self.exps[s.name]))))
+                for s in self.experiment_specs
+            ])
+        else:
+            t.add_row([self.spec.header] +
+                      [escape_header_fun(s.display_name(self.exps[s.name])) for s in self.experiment_specs])
+        t.add_hline()
+
+        for i, m in enumerate(self.metrics):
+            row_values = np.around(means[i, :], m.decimals)
+            top_values = best_two_non_repeating(row_values, reverse=m.larger_is_better)
+            row = [m.display_name]
+            for v in row_values:
+                # TODO: use NoEscape only if certain flag is enabled?
+                row.append(
+                    NoEscape(
+                        render_metric(v, top_values[0], top_values[1], m.effective_display_decimals(),
+                                      m.format_string)))
+            t.add_row(row)
+
+        t.add_hline()
+
+        if self.spec.export_latex:
+            os.makedirs(self.export_basepath, exist_ok=True)
+            t.generate_tex(os.path.join(self.export_basepath, self.spec.export_latex))
+
+        with self.create(Subsection(self.spec.name, numbering=False)) as p:
+            p.append(FootnoteText(t))

python/basalt/latex/templates.py

@@ -0,0 +1,93 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+screenread_sty = r"""
+\ProvidesPackage{screenread}
+% Copyright (C) 2012 John Collins, collins@phys.psu.edu
+% License: LPPL 1.2
+
+% Note:  To avoid compatibility issues between geometry and at least one
+% class file, it may be better to set all the dimensions by hand.
+
+%  20 Nov 2014  - use `pageBreakSection` instead of clobbering `section`
+%               - increase longest page size to 575cm
+%               - make top, right, and left margins something sensible and
+%                 a bit more aesthetically pleasing
+%  24 Jan 2012  Argument to \SetScreen is screen width
+%  23 Jan 2012  Remove package showlayout
+%  22 Jan 2012  Initial version, based on ideas in 
+%               B. Veytsman amd M. Ware, Tugboat 32 (2011) 261.
+
+\RequirePackage{everyshi}
+\RequirePackage{geometry}
+
+%=======================
+
+\pagestyle{empty}
+
+\EveryShipout{%
+    \pdfpageheight=\pagetotal
+    \advance\pdfpageheight by 2in
+    \advance\pdfpageheight by \topmargin
+    \advance\pdfpageheight by \textheight % This and next allow for footnotes
+    \advance\pdfpageheight by -\pagegoal
+}
+
+\AtEndDocument{\pagebreak}
+
+\def\pageBreakSection{\pagebreak\section}
+
+\newlength\screenwidth
+\newlength{\savedscreenwidth}
+
+\newcommand\SetScreen[1]{%
+  % Argument #1 is the screen width.
+  % Set appropriate layout parameters, with only a little white space
+  %   around the text.
+  \setlength\screenwidth{#1}%
+  \setlength\savedscreenwidth{#1}%
+  \setlength\textwidth{#1}%
+  \addtolength\textwidth{-2cm}%
+  \geometry{layoutwidth=\screenwidth,
+            paperwidth=\screenwidth,
+            textwidth=\textwidth,
+            layoutheight=575cm,
+            paperheight=575cm,
+            textheight=575cm,
+            top=1cm,
+            left=1cm,
+            right=1cm,
+            hcentering=true
+  }%
+}
+
+\newcommand\SetPageScreenWidth[1]{%
+  \setlength\savedscreenwidth{\screenwidth}%
+  \setlength\screenwidth{#1}%
+  \pdfpagewidth\screenwidth%
+  \setlength\textwidth{\screenwidth}%
+  \addtolength\textwidth{-2cm}%
+}
+
+\newcommand\RestorePageScreenWidth{%
+  \setlength\screenwidth{\savedscreenwidth}%
+  \pdfpagewidth\screenwidth%
+  \setlength\textwidth{\screenwidth}%
+  \addtolength\textwidth{-2cm}%
+}
+
+
+% Compute a reasonable default screen width, and set it
+\setlength\screenwidth{\textwidth}
+\addtolength\screenwidth{1cm}
+\SetScreen{\screenwidth}
+
+\endinput
+
+"""

python/basalt/latex/util.py

@@ -0,0 +1,61 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import math
+import numpy as np
+
+
+def best_two_non_repeating(array, reverse=False):
+    if reverse:
+        best = -math.inf
+        second = -math.inf
+        for v in array:
+            if v > best:
+                second = best
+                best = v
+            elif v < best and v > second:
+                second = v
+    else:
+        best = math.inf
+        second = math.inf
+        for v in array:
+            if v < best:
+                second = best
+                best = v
+            elif v > best and v < second:
+                second = v
+
+    return best, second
+
+
+def format_ratio(val, val_ref=None, decimals=0):
+    if val_ref == 0:
+        return "{}".format(math.inf)
+    else:
+        if val_ref is not None:
+            val = float(val) / float(val_ref)
+        return "{:.{prec}f}".format(val, prec=decimals)
+
+
+def format_ratio_percent(val, val_ref=None, decimals=0):
+    if val_ref == 0:
+        return "{}".format(val)
+    else:
+        if val_ref is not None:
+            val = float(val) / float(val_ref)
+        val = 100 * val
+        return "{:.{prec}f}%".format(val, prec=decimals)
+
+
+def rotation2d(theta_deg):
+    theta = np.radians(theta_deg)
+
+    R = np.array(((np.cos(theta), -np.sin(theta)), (np.sin(theta), np.cos(theta))))
+
+    return R

python/basalt/log.py

@@ -0,0 +1,106 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import ubjson
+import json
+import os
+
+import numpy as np
+
+from collections import Mapping
+from munch import Munch
+from munch import munchify
+
+
+class ExecutionStats(Munch):
+
+    def __init__(self, path):
+        data = self._load(path)
+        if data is None:
+            Munch.__init__(self)
+        else:
+            Munch.__init__(self, data)
+
+    def _load(self, path):
+
+        if path.endswith("ubjson"):
+            with open(path, 'rb') as f:
+                data = ubjson.load(f)
+        else:
+            with open(path, 'r') as f:
+                data = json.load(f)
+
+        if isinstance(data, Mapping):
+            data = self._convert(data)
+
+        return munchify(data)
+
+    def _convert(self, data):
+
+        data_new = dict()
+
+        for k, v in data.items():
+            if k.endswith("__values"):
+                continue  # skip; processed together with __index
+            elif k.endswith("__index"):
+                idx = v
+                values = np.array(data[k.replace("__index", "__values")])
+                # convert to list of arrays according to start indices
+                res = np.split(values, idx[1:])
+                if all(len(res[0]) == len(x) for x in res):
+                    res = np.array(res)
+                data_new[k.replace("__index", "")] = res
+            else:
+                data_new[k] = np.array(v)
+
+        return data_new
+
+    def _is_imu(self):
+        return len(self.marg_ev[0]) == 15
+
+
+def detect_log_path(dir, basename):
+
+    for ext in ["ubjson", "json"]:
+        path = os.path.join(dir, basename + "." + ext)
+        if os.path.isfile(path):
+            return path
+
+    return None
+
+
+def load_execution_stats(dir, basename):
+
+    path = detect_log_path(dir, basename)
+
+    if path is not None:
+        return ExecutionStats(path)
+    else:
+        return None
+
+
+class Log(Munch):
+
+    @staticmethod
+    def load(dir):
+
+        log = Log(all=load_execution_stats(dir, "stats_all"),
+                  sums=load_execution_stats(dir, "stats_sums"),
+                  vio=load_execution_stats(dir, "stats_vio"))
+
+        if all([v is None for v in log.values()]):
+            return None
+        else:
+            return log
+
+    def __init__(self, *args, **kwargs):
+        Munch.__init__(self, *args, **kwargs)
+
+    def duration(self):
+        return (self.sums.frame_id[-1] - self.sums.frame_id[0]) * 1e-9

python/basalt/metric.py

@@ -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-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import copy
+
+import numpy as np
+
+
+class ExperimentSpec:
+
+    def __init__(self, string):
+        if "@it" in string:
+            self.name, it = string.split("@it")
+            self.it = int(it)
+        else:
+            self.name = string
+            self.it = -1
+
+    def display_name(self, exp):
+        if self.it == -1:
+            return exp.display_name
+        else:
+            return "{} @ it{}".format(exp.display_name, self.it)
+
+
+class Metric:
+
+    def __init__(self,
+                 display_name,
+                 accessor,
+                 decimals,
+                 format_string="{:.{prec}f}",
+                 highlight_top=True,
+                 geometric_mean=False,
+                 larger_is_better=False):
+        self.display_name = display_name
+        self.accessor = accessor
+        self.decimals = decimals
+        self.display_decimals = None
+        self.relative_to_column = None
+        self.relative_to_experiment = None
+        self.relative_to_metric = None
+        self.ratio = True
+        self.format_string = format_string
+        self.highlight_top = highlight_top
+        self.larger_is_better = larger_is_better
+        self.exclude_columns_highlight = []
+        self.geometric_mean = geometric_mean
+        self.failed_threshold = None
+
+    def set_config(self, spec):
+        # change defaults in case of "relative_to_..." display mode
+        if any(k in spec for k in ["relative_to_column", "relative_to_experiment", "relative_to_metric"]):
+            # maybe overwritten by explicit "decimals" / "format_string" below
+            self.decimals = 3
+            self.display_decimals = 3
+            self.format_string = "{:.3f}"
+            self.geometric_mean = True
+
+        if "display_name" in spec:
+            self.display_name = spec.display_name
+        if "decimals" in spec:
+            self.decimals = spec.decimals
+        if "display_decimals" in spec:
+            self.display_decimals = spec.display_decimals
+        if "relative_to_column" in spec:
+            self.relative_to_column = spec.relative_to_column
+        if "relative_to_experiment" in spec:
+            self.relative_to_experiment = ExperimentSpec(spec.relative_to_experiment)
+        if "relative_to_metric" in spec:
+            self.relative_to_metric = spec.relative_to_metric
+        if "ratio" in spec:
+            self.ratio = spec.ratio
+        if "format_string" in spec:
+            self.format_string = spec.format_string
+        if "highlight_top" in spec:
+            self.highlight_top = spec.highlight_top
+        if "larger_is_better" in spec:
+            self.larger_is_better = spec.larger_is_better
+        if "exclude_columns_highlight" in spec:
+            self.exclude_columns_highlight = spec.exclude_columns_highlight
+        if "geometric_mean" in spec:
+            self.geometric_mean = spec.geometric_mean
+        if "failed_threshold" in spec:
+            self.failed_threshold = spec.failed_threshold
+
+    def effective_display_decimals(self):
+        if self.display_decimals is not None:
+            return self.display_decimals
+        else:
+            return self.decimals
+
+    def get_value(self, exps, e, s, it):
+        #try:
+        value = self.accessor(e.runs[s].log, it)
+        #except AttributeError as err:
+        #    raise
+
+        if self.relative_to_metric is not None:
+            relative_to_metric_accessor = self.relative_to_metric.accessor
+        else:
+            relative_to_metric_accessor = self.accessor
+
+        if self.relative_to_experiment is not None:
+            relative_to_log = exps[self.relative_to_experiment.name].runs[s].log
+            relative_to_it = self.relative_to_experiment.it
+        else:
+            relative_to_log = e.runs[s].log
+            relative_to_it = it
+
+        if self.relative_to_metric is not None or self.relative_to_experiment is not None:
+            base_value = relative_to_metric_accessor(relative_to_log, relative_to_it)
+            if self.ratio:
+                value = value / base_value
+            else:
+                value = base_value - value
+        return value
+
+
+def peak_memory_opt(l, it):
+    if it == -1:
+        index = -1
+    else:
+        index = int(l.all.num_it[:it + 1].sum()) - 1
+    return l.all.resident_memory_peak[index] / 1024**2
+
+
+# yapf: disable
+metric_desc = dict(
+    ev_min=Metric("min ev", lambda l, it: min(min(x) for x in l.sums.marg_ev), 1),
+    avg_num_it=Metric("avg #it", lambda l, it: np.mean(l.sums.num_it), 1),
+    avg_num_it_failed=Metric("avg #it-fail", lambda l, it: np.mean(l.sums.num_it_rejected), 1),
+    duration=Metric("duration (s)", lambda l, it: l.duration(), 1),
+    time_marg=Metric("t marg", lambda l, it: np.sum(l.sums.marginalize), 2),
+    time_opt=Metric("t opt", lambda l, it: np.sum(l.sums.optimize), 2),
+    time_optmarg=Metric("t opt", lambda l, it: np.sum(l.sums.optimize) + np.sum(l.sums.marginalize), 2),
+    time_exec=Metric("t exec", lambda l, it: l.vio.exec_time_s[0], 1),
+    time_exec_realtimefactor=Metric("t exec (rtf)", lambda l, it: l.duration() / l.vio.exec_time_s[0], 1, larger_is_better=True),
+    time_measure=Metric("t meas", lambda l, it: np.sum(l.sums.measure), 1),
+    time_measure_realtimefactor=Metric("t meas (rtf)", lambda l, it: l.duration() / np.sum(l.sums.measure), 1, larger_is_better=True),
+    time_exec_minus_measure=Metric("t exec - meas", lambda l, it: l.vio.exec_time_s[0] - np.sum(l.sums.measure), 1),
+    time_measure_minus_optmarg=Metric("t exec - (opt + marg)", lambda l, it: np.sum(l.sums.measure) - (np.sum(l.sums.optimize) + np.sum(l.sums.marginalize)), 1),
+    ate_num_kfs=Metric("ATE #kf", lambda l, it: l.vio.ate_num_kfs[0], 0),
+    ate_rmse=Metric("ATE", lambda l, it: l.vio.ate_rmse[0], 3),
+    peak_memory=Metric("mem peak (MB)", lambda l, it: l.vio.resident_memory_peak[0] / 1024**2, 1),
+    #peak_memory_opt=Metric("mem peak opt (MB)", lambda l, it: l.all.resident_memory_peak[l.all.num_it[:it].sum()-1] / 1024**2, 1),
+    peak_memory_opt=Metric("mem peak opt (MB)", peak_memory_opt, 1),
+)
+# yapf: enable
+
+
+def metrics_from_config(spec):
+
+    def get_from_spec(m):
+        if isinstance(m, str):
+            obj = copy.copy(metric_desc[m])
+        else:
+            obj = copy.copy(metric_desc[m.name])
+            obj.set_config(m)
+        if obj.relative_to_metric is not None:
+            obj.relative_to_metric = get_from_spec(obj.relative_to_metric)
+
+        return obj
+
+    return [get_from_spec(m) for m in spec]

python/basalt/nullspace.py

@@ -0,0 +1,209 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+from .log import ExecutionStats
+
+import argparse
+
+import numpy as np
+
+import matplotlib.pyplot as plt
+
+prop_cycle = plt.rcParams['axes.prop_cycle']
+
+#default_cycler = (cycler(linestyle=['-', '--', ':', '-.']) *
+#                  cycler(color=prop_cycle.by_key()['color']))
+
+default_colors = prop_cycle.by_key()['color']
+
+
+def plot(args):
+
+    log = ExecutionStats(args.path)
+
+    fig, axs = plt.subplots(nrows=6, ncols=1, figsize=(10, 12.0))
+    i = 0
+
+    if log._is_imu():
+        ns = log.marg_ns[1:, [0, 1, 2, 5]]
+    else:
+        ns = log.marg_ns[1:, 0:6]
+    not_ns = log.marg_ns[1:, 6]
+
+    if True:
+        ax = axs[i]
+        i += 1
+
+        ax.semilogy(log.marg_ns[1:, 0], ":", label="x", color=default_colors[0])
+        ax.semilogy(log.marg_ns[1:, 1], ":", label="y", color=default_colors[0])
+        ax.semilogy(log.marg_ns[1:, 2], ":", label="z", color=default_colors[0])
+
+        ax.semilogy(log.marg_ns[1:, 3], ":", label="roll", color=default_colors[1])
+        ax.semilogy(log.marg_ns[1:, 4], ":", label="pitch", color=default_colors[1])
+
+        ax.semilogy(log.marg_ns[1:, 5], ":", label="yaw", color=default_colors[2])
+
+        ax.semilogy(log.marg_ns[1:, 6], ":", label="rand", color=default_colors[3])
+
+        #ax.semilogy(np.min(ns, axis=1), "-.", color=default_colors[0])
+        #ax.semilogy(np.max(ns, axis=1), ":", color=default_colors[0])
+        #ax.semilogy(not_ns, "-", label="foo", color=default_colors[0])
+        ax.set_title("nullspace")
+
+        ax.legend(loc="center right")
+
+    ev_all = np.array([x[0:7] for x in log.marg_ev[3:]])
+    ev = np.array([x[x > 1e-5][0:7] for x in log.marg_ev[3:]])
+    #ev = np.array([x[0:7] for x in log.marg_ev[3:]])
+
+    ev_ns_min = ev[:, 0]
+    if log._is_imu():
+        print("is vio")
+        ev_ns_max = ev[:, 3]
+        ev_not_ns = ev[:, 4]
+    else:
+        print("is vo")
+        ev_ns_max = ev[:, 5]
+        ev_not_ns = ev[:, 6]
+
+    if True:
+        ax = axs[i]
+        i += 1
+
+        ax.semilogy(ev_ns_min, "-.", color=default_colors[0])
+        ax.semilogy(ev_ns_max, ":", color=default_colors[0])
+        ax.semilogy(ev_not_ns, "-", label="foo", color=default_colors[0])
+
+        ax.set_title("eigenvalues (filtered all ev < 1e-5)")
+        #ax.set_title("eigenvalues")
+        #ax.legend()
+
+    if True:
+        ax = axs[i]
+        i += 1
+
+        ax.plot([sum(x < 1e-5) for x in ev_all], label="x < 1e-5", color=default_colors[0])
+
+        ax.set_title("zero ev count")
+        ax.legend()
+
+    if False:
+        ax = axs[i]
+        i += 1
+
+        ax.plot([sum(x == 0) for x in ev_all], label="== 0", color=default_colors[0])
+
+        ax.set_title("zero ev count")
+        ax.legend()
+
+    if True:
+        ax = axs[i]
+        i += 1
+
+        ax.plot([sum(x < 0) for x in ev_all], label="< 0", color=default_colors[0])
+
+        #ax.set_title("zero ev count")
+        ax.legend()
+
+    if False:
+        ax = axs[i]
+        i += 1
+
+        ax.plot([sum((x > 0) & (x <= 1e-8)) for x in ev_all], label="0 < x <= 1e-8", color=default_colors[0])
+
+        #ax.set_title("zero ev count")
+        ax.legend()
+
+    if False:
+        ax = axs[i]
+        i += 1
+
+        ax.plot([sum(x < -1e-8) for x in ev_all], label="< -1e-8", color=default_colors[0])
+
+        #ax.set_title("zero ev count")
+        ax.legend()
+
+    if True:
+        ax = axs[i]
+        i += 1
+
+        #ax.plot([sum((1e-6 <= x) & (x <= 1e2)) for x in ev_all], label="1e-8 <= x <= 1e1", color=default_colors[0])
+
+        #ax.set_title("zero ev count")
+        #ax.legend()
+
+        ev_all = np.concatenate(log.marg_ev[3:])
+        ev_all = ev_all[ev_all < 1e3]
+        num = len(log.marg_ev[3:])
+
+        ax.hist(
+            ev_all,
+            bins=[
+                -1e2,
+                -1e1,
+                -1e0,
+                -1e-1,
+                -1e-2,
+                -1e-3,
+                -1e-4,
+                -1e-5,
+                -1e-6,
+                #-1e-7,
+                #-1e-8,
+                #-1e-9,
+                #-1e-10,
+                0,
+                #1e-10,
+                #1e-9,
+                #1e-8,
+                #1e-7,
+                1e-6,
+                1e-5,
+                1e-4,
+                1e-3,
+                1e-2,
+                1e-1,
+                1e-0,
+                1e1,
+                1e2,
+                1e3,
+                #1e4,
+                #1e5,
+                #1e6,
+                #1e7,
+                #1e8,
+                #1e9,
+                #1e10,
+                #1e11
+            ])
+        ax.set_xscale("symlog", linthresh=1e-6)
+        y_vals = ax.get_yticks()
+        ax.set_yticklabels(['{:.1f}'.format(x / num) for x in y_vals])
+        ax.set_title("hist of all ev < 1e3 (count normalized by num marg-priors)")
+
+    if args.save:
+        plt.savefig(args.save)
+
+    if not args.no_gui:
+        plt.show()
+
+
+def main():
+    parser = argparse.ArgumentParser("Load multiple PBA logs and plot combined results for comparison.")
+    parser.add_argument("path", help="log file path")
+    parser.add_argument("--no-gui", action="store_true", help="show plots")
+    parser.add_argument("--save", help="save plots to specified file")
+
+    args = parser.parse_args()
+
+    plot(args)
+
+
+if __name__ == "__main__":
+    main()

python/basalt/run.py

@@ -0,0 +1,117 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import os
+from collections import Mapping
+
+from .log import Log
+
+from .util import load_json_if_exists
+from .util import load_text_if_exists
+from .util import load_trajectory_tum_if_exists
+
+
+class Run:
+    """Loads files from a single run of an experiment from a folder (config, status, output, log, ...)
+    
+    A single run is one invocation of odometry with a specific config on a specific sequence.
+    This is meant to be used on directories created with the 'generate-batch-configs' and 'run-all-in' scripts.
+    It's best-effort, loading as many of the files as are present.
+    """
+
+    def __init__(self, dirpath, seq_name_mapping):
+        self.dirpath = dirpath
+
+        self.config = load_json_if_exists(os.path.join(dirpath, 'basalt_config.json'))
+        self.output = load_text_if_exists(os.path.join(dirpath, 'output.log'))
+        self.status = load_text_if_exists(os.path.join(dirpath, 'status.log'))
+        self.traj_est = load_trajectory_tum_if_exists(os.path.join(dirpath, 'trajectory.txt'))
+        self.traj_gt = load_trajectory_tum_if_exists(os.path.join(dirpath, 'groundtruth.txt'))
+
+        self.log = Log.load(dirpath)
+
+        self.seq_name = self._infer_sequence_name(self.config, dirpath, seq_name_mapping)
+
+        print("loaded {} from '{}'".format(self.seq_name, dirpath))
+
+    def is_imu(self):
+        return self.config.batch_run.args["use-imu"] in [1, "1", True, "true"]
+
+    def is_failed(self):
+        if self.log is None:
+            return True
+        else:
+            return "Completed" not in self.status
+
+    def failure_str(self):
+        if not self.is_failed():
+            return ""
+        if self.output:
+            if "Some of your processes may have been killed by the cgroup out-of-memory handler" in self.output:
+                return "OOM"
+            if "DUE TO TIME LIMIT" in self.output:
+                return "OOT"
+        return "x"
+
+    @staticmethod
+    def _infer_sequence_name(config, dirpath, name_mapping):
+        """Tries to infer the sequence name from the config, or falls back to the parent folder name"""
+        seq_name = ""
+        try:
+            type = config.batch_run.args["dataset-type"]
+            path = config.batch_run.args["dataset-path"]
+            seq_name = os.path.basename(path)
+            if type == "euroc":
+                if seq_name.startswith("dataset-"):
+                    # assume tumvi
+                    seq_name = seq_name.replace("dataset-", "tumvi-").split("_")[0]
+                else:
+                    # assume euroc
+                    s = seq_name.split("_")
+                    seq_name = "euroc{}{}".format(s[0], s[1])
+            elif type == "kitti":
+                # assume kitti
+                seq_name = "kitti{}".format(seq_name)
+        except:
+            pass
+
+        # Fallback to detecting the sequence name base on the last component of the parent folder. This is intended
+        # to work for run folders created with the 'generate-batch-configs' script, assuming the sequence is the
+        # last component in '_batch.combinations'.
+        if seq_name == "":
+            seq_name = os.path.basename(dirpath).split("_")[-1]
+
+        # optionally remap the sequence name to something else as defined in the experiments config
+        if isinstance(name_mapping, Mapping) and seq_name in name_mapping:
+            seq_name = name_mapping[seq_name]
+
+        return seq_name
+
+    @staticmethod
+    def is_run_dir(dirpath):
+        """Returns True if the folder may be a run directory, based on the present files
+        
+        This is intended to be used for auto-detecting run directories in a file tree.
+        """
+
+        files = [
+            'status.log',
+            'slurm-output.log',
+            'output.log',
+            'stats_all.ubjson',
+            'stats_all.json',
+            'stats_sums.ubjson',
+            'stats_sums.json',
+            'stats_vio.ubjson',
+            'stats_vio.json',
+        ]
+        for f in files:
+            if os.path.isfile(os.path.join(dirpath, f)):
+                return True
+        return False

python/basalt/util.py

@@ -0,0 +1,72 @@
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+import os
+import json
+import platform
+import subprocess
+import re
+import numpy as np
+
+from munch import munchify
+
+
+def copy_subdict(d, keys):
+    res = dict()
+    for k in keys:
+        if k in d:
+            res[k] = d[k]
+    return res
+
+
+def load_json_if_exists(filepath):
+    if os.path.isfile(filepath):
+        with open(filepath, 'r') as f:
+            return munchify(json.load(f))
+    return None
+
+
+def load_text_if_exists(filepath):
+    if os.path.isfile(filepath):
+        with open(filepath, 'r') as f:
+            return f.read()
+    return None
+
+
+def load_trajectory_tum(filepath):
+    # first row is header
+    # format for each row is: ts x y z qz qy qz qw
+    traj = np.loadtxt(filepath, delimiter=" ", skiprows=1)
+    # return just translation for now
+    traj = traj[:, 1:4]
+    return traj
+
+
+def load_trajectory_tum_if_exists(filepath):
+    if os.path.isfile(filepath):
+        return load_trajectory_tum(filepath)
+    return None
+
+
+def os_open_file(filepath):
+    if platform.system() == 'Darwin':
+        subprocess.call(('open', filepath))
+    elif platform.system() == 'Windows':
+        os.startfile(filepath)
+    else:
+        subprocess.call(('xdg-open', filepath))
+
+
+# key for 'human' sorting
+def alphanum(key):
+
+    def convert(text):
+        return float(text) if text.isdigit() else text
+
+    return [convert(c) for c in re.split('([-+]?[0-9]*\.?[0-9]*)', key)]

scripts/basalt_capture_mocap.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import sys
 import os

scripts/basalt_convert_kitti_calib.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import sys
 import math

scripts/basalt_response_calib.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import sys
 import math

scripts/basalt_verify_dataset.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import sys
 import math

scripts/batch/generate-batch-configs.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+
+#
+# Generate basalt configurations from a batch config file.
+#
+# Example:
+#     ./generate-batch-config.py /path/to/folder
+#
+# It looks for the file named `basalt_batch_config.toml` inside the given folder.
+
+import os
+import toml
+import json
+import argparse
+from pprint import pprint
+from copy import deepcopy
+from collections import OrderedDict
+import itertools
+import shutil
+import datetime
+import sys
+
+
+def isdict(o):
+    return isinstance(o, dict) or isinstance(o, OrderedDict)
+
+
+def merge_config(a, b):
+    "merge b into a"
+    for k, v in b.items():
+        if k in a:
+            if isdict(v) and isdict(a[k]):
+                #print("dict {}".format(k))
+                merge_config(a[k], b[k])
+            elif not isdict(v) and not isdict(a[k]):
+                a[k] = deepcopy(v)
+                #print("not dict {}".format(k))
+            else:
+                raise RuntimeError("Incompatible types for key {}".format(k))
+        else:
+            a[k] = deepcopy(v)
+
+
+def save_config(template, configs, combination, path_prefix):
+    filename = os.path.join(path_prefix, "basalt_config_{}.json".format("_".join(combination)))
+    config = deepcopy(template)
+    #import ipdb; ipdb.set_trace()
+    for override in combination:
+        merge_config(config, configs[override])
+    #import ipdb; ipdb.set_trace()
+    with open(filename, 'w') as f:
+        json.dump(config, f, indent=4)
+    print(filename)
+
+
+def generate_configs(root_path, cmdline=[], overwrite_existing=False, revision_override=None):
+
+    # load and parse batch config file
+    batch_config_path = os.path.join(root_path, "basalt_batch_config.toml")
+    template = toml.load(batch_config_path, OrderedDict)
+    cfg = template["_batch"]
+    del template["_batch"]
+
+    # parse batch configuration
+    revision = str(cfg.get("revision", 0)) if revision_override is None else revision_override
+    configs = cfg["config"]
+    alternatives = cfg.get("alternatives", dict())
+    combinations = cfg["combinations"]
+
+    # prepare output directory
+    date_str = datetime.datetime.now().strftime("%Y%m%d-%H%M%S")
+    outdir = root_path if revision is None else os.path.join(root_path, revision)
+    if overwrite_existing and os.path.exists(outdir):
+        print("WARNING: output directory exists, overwriting existing files: {}".format(outdir))
+    else:
+        os.makedirs(outdir)
+    shutil.copy(batch_config_path, outdir)
+    with open(os.path.join(outdir, "timestamp"), 'w') as f:
+        f.write(date_str)
+    with open(os.path.join(outdir, "commandline"), 'w') as f:
+        f.write(cmdline)
+
+    # expand single entry in combination array
+    def expand_one(x):
+        if x in alternatives:
+            return alternatives[x]
+        elif isinstance(x, list):
+            # allow "inline" alternative
+            return x
+        else:
+            return [x]
+
+    def flatten(l):
+        for el in l:
+            if isinstance(el, list):
+                yield from flatten(el)
+            else:
+                yield el
+
+    # generate all configurations
+    for name, description in combinations.items():
+        if True or len(combinations) > 1:
+            path_prefix = os.path.join(outdir, name)
+            if not (overwrite_existing and os.path.exists(path_prefix)):
+                os.mkdir(path_prefix)
+        else:
+            path_prefix = outdir
+        expanded = [expand_one(x) for x in description]
+        for comb in itertools.product(*expanded):
+            # flatten list to allow each alternative to reference multiple configs
+            comb = list(flatten(comb))
+            save_config(template, configs, comb, path_prefix)
+
+
+def main():
+    cmdline = str(sys.argv)
+    parser = argparse.ArgumentParser("Generate basalt configurations from a batch config file.")
+    parser.add_argument("path", help="path to look for config and templates")
+    parser.add_argument("--revision", help="override revision")
+    parser.add_argument("--force", "-f", action="store_true", help="overwrite existing files")
+    args = parser.parse_args()
+    generate_configs(args.path, cmdline, args.force, args.revision)
+
+
+if __name__ == "__main__":
+    main()

scripts/batch/generate-tables.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+
+# Dependencies:
+# pip3 install -U --user py_ubjson matplotlib numpy munch scipy pylatex toml
+
+# also: latexmk and latex
+#
+# Ubuntu:
+#     sudo apt install texlive-latex-extra latexmk
+import os
+import sys
+
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "python")))
+
+import basalt.generate_tables
+
+basalt.generate_tables.main()

scripts/batch/list-jobs.sh

@@ -0,0 +1,139 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+#
+# Usage:
+#    list-jobs.sh DIRNAME [DIRNAME ...] [-s|--short] [-o|--only STATUS]
+#
+# Lists all batch jobs found in DIRNAME. If the optional argument
+# STATUS is passed, only lists jobs with that status. Multiple
+# statuses can be passed in a space-separated string.
+#
+# Possible status arguments: queued, running, completed, failed, unknown
+# You can also use 'active' as a synonym for 'queued running unknown'
+
+# exit on error
+set -o errexit -o pipefail
+
+
+# we need GNU getopt...
+GETOPT=getopt
+if [[ "$OSTYPE" == "darwin"* ]]; then
+    if [ -f /usr/local/opt/gnu-getopt/bin/getopt ]; then
+        GETOPT="/usr/local/opt/gnu-getopt/bin/getopt"
+    fi
+fi
+
+# option parsing, see: https://stackoverflow.com/a/29754866/1813258
+usage() { echo "Usage: `basename $0` DIRNAME [DIRNAME ...] [-s|--short] [-o|--only STATUS]" ; exit 1; }
+
+# -allow a command to fail with !’s side effect on errexit
+# -use return value from ${PIPESTATUS[0]}, because ! hosed $?
+! "$GETOPT" --test > /dev/null
+if [[ ${PIPESTATUS[0]} -ne 4 ]]; then
+    echo 'I’m sorry, `getopt --test` failed in this environment.'
+    exit 1
+fi
+
+OPTIONS=hsjo:
+LONGOPTS=help,short,jobids,only:
+
+# -regarding ! and PIPESTATUS see above
+# -temporarily store output to be able to check for errors
+# -activate quoting/enhanced mode (e.g. by writing out “--options”)
+# -pass arguments only via   -- "$@"   to separate them correctly
+! PARSED=$("$GETOPT" --options=$OPTIONS --longoptions=$LONGOPTS --name "`basename $0`" -- "$@")
+if [[ ${PIPESTATUS[0]} -ne 0 ]]; then
+    # e.g. return value is 1
+    #  then getopt has complained about wrong arguments to stdout
+    usage
+fi
+# read getopt’s output this way to handle the quoting right:
+eval set -- "$PARSED"
+
+SHORT=n
+ONLY=""
+JOBIDS=n
+# now enjoy the options in order and nicely split until we see --
+while true; do
+    case "$1" in
+        -h|--help) usage ;;
+        -s|--short) SHORT=y; shift ;;
+        -j|--jobids) JOBIDS=y; shift ;;
+        -o|--only) ONLY="$2"; shift 2 ;;
+        --) shift; break ;;
+        *) echo "Programming error"; exit 3 ;;
+    esac
+done
+
+# handle non-option arguments --> directories
+if [[ $# -lt 1 ]]; then
+    echo "Error: Pass at least one folder"
+    usage
+fi
+DIRS=("$@")
+
+# status aliases:
+ONLY="${ONLY/active/queued running unknown}"
+ONLY="${ONLY/notcompleted/queued running failed unknown}"
+
+contains() {
+    [[ $1 =~ (^| )$2($| ) ]] && return 0 || return 1
+}
+
+display() {
+    if [ -z "$ONLY" ] || contains "$ONLY" $2; then
+        if [ $SHORT = y ]; then
+            echo "$1"
+        else
+            echo -n "$1 : $2"
+            if [ -n "$3" ]; then
+                echo -n " - $3"
+            fi
+            echo ""
+        fi
+    fi
+}
+
+for d in "${DIRS[@]}"; do
+    for f in `find "$d" -name status.log | sort`; do
+        DIR=`dirname "$f"`
+
+        # ignore backup folder from "rerun" scripts
+        if [[ `basename $DIR` = results-backup* ]]; then
+            continue
+        fi
+
+        if ! grep Started "$f" > /dev/null; then
+            display "$DIR" unknown "not started"
+            continue
+        fi
+
+        # job has started:
+
+        if grep Completed "$f" > /dev/null ; then
+            display "$DIR" completed ""
+            continue
+        fi
+
+        # job has started, but not completed (cleanly)
+
+        # check signs of termination
+        if [ -f "$DIR"/output.log ] && grep "Command terminated by signal" "$DIR"/output.log > /dev/null; then
+            display "$DIR" failed killed "`grep -oP 'Command terminated by \Ksignal .+' "$DIR"/output.log`"
+            continue
+        fi
+
+        # might be running or aborted
+        display "$DIR" unknown started
+
+    done
+done

scripts/batch/plot.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+
+import os
+import sys
+
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "python")))
+
+import basalt.nullspace
+
+basalt.nullspace.main()

scripts/batch/query-config.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
+
+#
+# Example usage:
+#     $ ./query-config.py path/to/basalt_config.json value0.\"config.vio_debug\"
+#     10G
+
+import json
+import toml
+import argparse
+import sys
+
+
+def parse_query(query):
+    query_list = []
+
+    quote_open_char = None
+    curr = ""
+    for c in query:
+        if quote_open_char:
+            if c == quote_open_char:
+                quote_open_char = None
+            else:
+                curr += c
+        elif c in ['"', "'"]:
+            quote_open_char = c
+        elif c == '.':
+            query_list.append(curr)
+            curr = ""
+        else:
+            curr += c
+    query_list.append(curr)
+
+    return query_list
+
+
+def query_config(path, query, default_value=None, format_env=False, format_cli=False):
+    query_list = parse_query(query)
+    with open(path) as f:
+        cfg = json.load(f)
+    try:
+        curr = cfg
+        for q in query_list:
+            curr = curr[q]
+        result = curr
+    except:
+        if default_value is None:
+            result = ""
+        else:
+            result = default_value
+    if isinstance(result, dict):
+        if format_env:
+            lines = []
+            for k, v in result.items():
+                # NOTE: assumes no special escaping is necessary
+                lines.append("{}='{}'".format(k, v))
+            return "\n".join(lines)
+        elif format_cli:
+            args = ["--{} {}".format(k, v) for k, v in result.items()]
+            return " ".join(args)
+        else:
+            result = toml.dumps(result)
+    else:
+        result = "{}".format(result)
+    return result
+
+
+def main():
+    parser = argparse.ArgumentParser("Parse toml file and print content of query key.")
+    parser.add_argument("config_path", help="path to toml file")
+    parser.add_argument("query", help="query string")
+    parser.add_argument("default_value", help="value printed if query is not successful", nargs='?')
+    parser.add_argument(
+        "--format-env",
+        action="store_true",
+        help="Expect dictionary as query result and output like environment variables, i.e. VAR='VALUE' lines.")
+    parser.add_argument("--format-cli",
+                        action="store_true",
+                        help="Expect dictionary as query result and output like cli arguments, i.e. --VAR 'VALUE'.")
+    args = parser.parse_args()
+
+    res = query_config(args.config_path,
+                       args.query,
+                       default_value=args.default_value,
+                       format_env=args.format_env,
+                       format_cli=args.format_cli)
+    print(res)
+
+
+if __name__ == "__main__":
+    main()

scripts/batch/rerun-failed-in.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+#
+# Usage:
+#    rerun-failed-in.sh FOLDER
+#
+# Reruns all failed experiments that are found in a given folder.
+
+set -e
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+"$SCRIPT_DIR"/list-jobs.sh "$1" -s -o failed | while read f; do
+    echo "$f"
+    "$SCRIPT_DIR"/rerun-one-in.sh "$f" || true
+done

scripts/batch/rerun-one-in.sh

@@ -0,0 +1,35 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+set -e
+set -x
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+FOLDER="${1}"
+
+cd "$FOLDER"
+
+# backup previous files
+DATE=`date +'%Y%m%d-%H%M%S'`
+BACKUP_FOLDER=results-backup-$DATE
+for f in *.jobid *.log stats*.*json; do
+    if [ -f $f ]; then
+        mkdir -p $BACKUP_FOLDER
+        mv $f $BACKUP_FOLDER/
+    fi
+done
+
+echo "Created" > status.log
+echo "Restarted" >> status.log
+
+echo "Starting run in $PWD"
+"$SCRIPT_DIR"/run-one.sh "$PWD"

scripts/batch/run-all-in.sh

@@ -0,0 +1,60 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+
+# given folder with basalt_config_*.json, run optimization for each config in
+# corresponding subfolder
+
+set -e
+set -x
+
+# number of logical cores on linux and macos
+NUM_CORES=`(which nproc > /dev/null && nproc) || sysctl -n hw.logicalcpu || echo 1`
+
+echo "Running on '`hostname`', nproc: $NUM_CORES"
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+# loop over all arguments, and in each folder find configs and run them
+for FOLDER in "$@"
+do
+
+    pushd "$FOLDER"
+
+    FILE_PATTERN='basalt_config_*.json'
+    FILE_REGEX='basalt_config_(.*)\.json'
+
+    DATE=`date +'%Y%m%d-%H%M%S'`
+    mkdir -p $DATE
+
+    declare -a RUN_DIRS=()
+
+    for f in `find . -name "$FILE_PATTERN" -type f | sort`; do
+        if [[ `basename $f` =~ $FILE_REGEX ]]; then
+            RUN_DIR=${DATE}/`dirname $f`/${BASH_REMATCH[1]}
+            echo "Creating run with config $f in $RUN_DIR"
+            mkdir -p "$RUN_DIR"
+            cp $f "$RUN_DIR"/basalt_config.json
+            echo "Created" > "$RUN_DIR"/status.log
+            RUN_DIRS+=($RUN_DIR)
+        else
+            echo "Skipping $f"
+        fi
+    done
+
+    for RUN_DIR in "${RUN_DIRS[@]}"; do
+        echo "Starting run in $RUN_DIR"
+        "$SCRIPT_DIR"/run-one.sh "$RUN_DIR" || true
+    done
+
+    popd
+
+done

scripts/batch/run-one.sh

@@ -0,0 +1,83 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+#
+# This script runs on the slurm nodes to run rootba for one config.
+
+set -e
+set -o pipefail
+set -x
+
+error() {
+  local parent_lineno="$1"
+  local message="$2"
+  local code="${3:-1}"
+  if [[ -n "$message" ]] ; then
+    echo "Error on or near line ${parent_lineno}: ${message}; exiting with status ${code}"
+  else
+    echo "Error on or near line ${parent_lineno}; exiting with status ${code}"
+  fi
+  echo "Failed" >> status.log
+  exit "${code}"
+}
+trap 'error ${LINENO}' ERR
+
+# number of logical cores on linux and macos
+NUM_CORES=`(which nproc > /dev/null && nproc) || sysctl -n hw.logicalcpu || echo 1`
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+BASALT_BIN_DIR="${BASALT_BIN_DIR:-$SCRIPT_DIR/../../build}"
+
+FOLDER="${1}"
+
+cd "$FOLDER"
+
+if ! which time 2> /dev/null; then
+    echo "Did not find 'time' executable. Not installed?"
+    exit 1
+fi
+
+if [[ "$OSTYPE" == "darwin"* ]]; then
+    TIMECMD="`which time` -lp"
+else
+    TIMECMD="`which time` -v"
+fi
+
+echo "Started" >> status.log
+
+# set environment variables according to config
+while read l; do
+    if [ -n "$l" ]; then
+        eval "export $l"
+    fi
+done <<< `"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.env --format-env`
+
+# lookup executable to run
+EXECUTABLE=`"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.executable basalt_vio`
+
+# lookup args
+ARGS=`"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.args --format-cli`
+
+CMD="$BASALT_BIN_DIR/$EXECUTABLE"
+
+echo "Running on '`hostname`', nproc: $NUM_CORES, bin: $CMD"
+
+# run as many times as specified (for timing tests to make sure filecache is hot); default is once
+rm -f output.log
+NUM_RUNS=`"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.num_runs 1`
+echo "Will run $NUM_RUNS times."
+for i in $(seq $NUM_RUNS); do
+    echo ">>> Run $i" |& tee -a output.log
+    { $TIMECMD "$CMD" $ARGS; } |& tee -a output.log
+done
+
+echo "Completed" >> status.log

scripts/clang-format-all.sh

@@ -1,4 +1,13 @@
 #!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 # Format all source files in the project.
 # Optionally take folder as argument; default is full inlude and src dirs.

scripts/compare_calib.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 
 import argparse

scripts/eval_full/gen_results.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import os
 import sys

scripts/eval_full/gen_results_kitti.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import os
 import sys

scripts/eval_full/gen_results_tumvi.py

@@ -1,4 +1,13 @@
 #!/usr/bin/env python3
+#
+# BSD 3-Clause License
+#
+# This file is part of the Basalt project.
+# https://gitlab.com/VladyslavUsenko/basalt.git
+#
+# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+# All rights reserved.
+#
 
 import os
 import sys

scripts/eval_full/run_evaluations.sh

@@ -1,4 +1,13 @@
 #!/bin/bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 set -e
 set -x

scripts/eval_full/run_evaluations_kitti.sh

@@ -1,4 +1,13 @@
 #!/bin/bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 set -e
 set -x

scripts/eval_full/run_evaluations_tumvi.sh

@@ -1,4 +1,13 @@
 #!/bin/bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 set -e
 set -x

scripts/install_deps.sh

@@ -1,4 +1,13 @@
 #!/bin/bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
 

scripts/install_mac_os_deps.sh

@@ -1,4 +1,13 @@
 #!/bin/sh
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 brew install \
      boost \

scripts/install_ubuntu_deps.sh

@@ -1,4 +1,13 @@
 #!/bin/sh
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 sudo apt-get update
 sudo apt-get install -y gcc g++ cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libgtest-dev libopencv-dev

scripts/templates/license-py-sh.tmpl

@@ -0,0 +1,7 @@
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) ${years}, ${owner}.
+All rights reserved.

scripts/update-license-headers.sh

@@ -0,0 +1,33 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+# Update license headers in source files.
+
+# Dependency: licenseheaders python package (install with pip)
+
+# TODO: Make it also update C++ files automatically. (Consider files with multiple headers, e.g. track.h and union_find.h)
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+DIRS=(
+    "$SCRIPT_DIR/../python/"
+    "$SCRIPT_DIR/../scripts"
+)
+
+YEAR="2019-2021"
+OWNER="Vladyslav Usenko and Nikolaus Demmel"
+TEMPLATE="$SCRIPT_DIR/templates/license-py-sh.tmpl"
+
+for d in "${DIRS[@]}"
+do
+    licenseheaders -d "$d" -y $YEAR -o "$OWNER" -t "$TEMPLATE" -vv
+done
+

scripts/update_submodules.sh

@@ -1,4 +1,13 @@
 #!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
 
 set -x
 

scripts/yapf-all.sh

@@ -0,0 +1,27 @@
+#!/usr/bin/env bash
+##
+## BSD 3-Clause License
+##
+## This file is part of the Basalt project.
+## https://gitlab.com/VladyslavUsenko/basalt.git
+##
+## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
+## All rights reserved.
+##
+
+
+# Format all python source files in the project.
+# Optionally take folder as argument; default are `python` and `script` dirs.
+
+set -e
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+# default folders if not passed
+if [ $# -lt 1 ]; then
+    set -- "$SCRIPT_DIR"/../python "$SCRIPT_DIR/batch"
+fi
+
+echo "Formatting: $@"
+
+yapf -i -r "$@"

src/linearization/landmark_block.cpp

@@ -0,0 +1,63 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/linearization/landmark_block_abs_dynamic.hpp>
+
+namespace basalt {
+
+template <typename Scalar>
+template <int POSE_SIZE>
+std::unique_ptr<LandmarkBlock<Scalar>>
+LandmarkBlock<Scalar>::createLandmarkBlock() {
+  return std::make_unique<LandmarkBlockAbsDynamic<Scalar, POSE_SIZE>>();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate factory templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template std::unique_ptr<LandmarkBlock<double>>
+LandmarkBlock<double>::createLandmarkBlock<6>();
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template std::unique_ptr<LandmarkBlock<float>>
+LandmarkBlock<float>::createLandmarkBlock<6>();
+#endif
+
+}  // namespace basalt

src/linearization/linearization_abs_qr.cpp

@@ -0,0 +1,711 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_abs_qr.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+#include <basalt/linearization/imu_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsQR<Scalar, POSE_SIZE>::LinearizationAbsQR(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg)
+    : options_(options),
+      estimator(estimator),
+      lmdb_(estimator->lmdb),
+      frame_poses(estimator->frame_poses),
+      calib(estimator->calib),
+      aom(aom),
+      used_frames(used_frames),
+      marg_lin_data(marg_lin_data),
+      imu_lin_data(imu_lin_data),
+      pose_damping_diagonal(0),
+      pose_damping_diagonal_sqrt(0) {
+  UNUSED(last_state_to_marg);
+
+  BASALT_ASSERT_STREAM(
+      options.lb_options.huber_parameter == estimator->huber_thresh,
+      "Huber threshold should be set to the same value");
+
+  BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
+                       "obs_std_dev should be set to the same value");
+
+  // Allocate memory for relative pose linearization
+  for (const auto& [tcid_h, target_map] : lmdb_.getObservations()) {
+    // if (used_frames && used_frames->count(tcid_h.frame_id) == 0) continue;
+    const size_t host_idx = host_to_idx_.size();
+    host_to_idx_.try_emplace(tcid_h, host_idx);
+    host_to_landmark_block.try_emplace(tcid_h);
+
+    // assumption: every host frame has at least target frame with
+    // observations
+    // NOTE: in case a host frame loses all of its landmarks due
+    // to outlier removal or marginalization of other frames, it becomes quite
+    // useless and is expected to be removed before optimization.
+    BASALT_ASSERT(!target_map.empty());
+
+    for (const auto& [tcid_t, obs] : target_map) {
+      // assumption: every target frame has at least one observation
+      BASALT_ASSERT(!obs.empty());
+
+      std::pair<TimeCamId, TimeCamId> key(tcid_h, tcid_t);
+      relative_pose_lin.emplace(key, RelPoseLin<Scalar>());
+    }
+  }
+
+  // Populate lookup for relative poses grouped by host-frame
+  for (const auto& [tcid_h, target_map] : lmdb_.getObservations()) {
+    // if (used_frames && used_frames->count(tcid_h.frame_id) == 0) continue;
+    relative_pose_per_host.emplace_back();
+
+    for (const auto& [tcid_t, _] : target_map) {
+      std::pair<TimeCamId, TimeCamId> key(tcid_h, tcid_t);
+      auto it = relative_pose_lin.find(key);
+
+      BASALT_ASSERT(it != relative_pose_lin.end());
+
+      relative_pose_per_host.back().emplace_back(it);
+    }
+  }
+
+  num_cameras = frame_poses.size();
+
+  landmark_ids.clear();
+  for (const auto& [k, v] : lmdb_.getLandmarks()) {
+    if (used_frames || lost_landmarks) {
+      if (used_frames && used_frames->count(v.host_kf_id.frame_id)) {
+        landmark_ids.emplace_back(k);
+      } else if (lost_landmarks && lost_landmarks->count(k)) {
+        landmark_ids.emplace_back(k);
+      }
+    } else {
+      landmark_ids.emplace_back(k);
+    }
+  }
+  size_t num_landmakrs = landmark_ids.size();
+
+  // std::cout << "num_landmakrs " << num_landmakrs << std::endl;
+
+  landmark_blocks.resize(num_landmakrs);
+
+  {
+    auto body = [&](const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        KeypointId lm_id = landmark_ids[r];
+        auto& lb = landmark_blocks[r];
+        auto& landmark = lmdb_.getLandmark(lm_id);
+
+        lb = LandmarkBlock<Scalar>::template createLandmarkBlock<POSE_SIZE>();
+
+        lb->allocateLandmark(landmark, relative_pose_lin, calib, aom,
+                             options.lb_options);
+      }
+    };
+
+    tbb::blocked_range<size_t> range(0, num_landmakrs);
+    tbb::parallel_for(range, body);
+  }
+
+  landmark_block_idx.reserve(num_landmakrs);
+
+  num_rows_Q2r = 0;
+  for (size_t i = 0; i < num_landmakrs; i++) {
+    landmark_block_idx.emplace_back(num_rows_Q2r);
+    const auto& lb = landmark_blocks[i];
+    num_rows_Q2r += lb->numQ2rows();
+
+    host_to_landmark_block.at(lb->getHostKf()).emplace_back(lb.get());
+  }
+
+  if (imu_lin_data) {
+    for (const auto& kv : imu_lin_data->imu_meas) {
+      imu_blocks.emplace_back(
+          new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
+    }
+  }
+
+  //    std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
+  //    num_cameras
+  //              << std::endl;
+}
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsQR<Scalar, POSE_SIZE>::~LinearizationAbsQR() = default;
+
+template <typename Scalar_, int POSE_SIZE_>
+void LinearizationAbsQR<Scalar_, POSE_SIZE_>::log_problem_stats(
+    ExecutionStats& stats) const {
+  UNUSED(stats);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsQR<Scalar, POSE_SIZE>::linearizeProblem(
+    bool* numerically_valid) {
+  // reset damping and scaling (might be set from previous iteration)
+  pose_damping_diagonal = 0;
+  pose_damping_diagonal_sqrt = 0;
+  marg_scaling = VecX();
+
+  // Linearize relative poses
+  for (const auto& [tcid_h, target_map] : lmdb_.getObservations()) {
+    // if (used_frames && used_frames->count(tcid_h.frame_id) == 0) continue;
+
+    for (const auto& [tcid_t, _] : target_map) {
+      std::pair<TimeCamId, TimeCamId> key(tcid_h, tcid_t);
+      RelPoseLin<Scalar>& rpl = relative_pose_lin.at(key);
+
+      if (tcid_h != tcid_t) {
+        const PoseStateWithLin<Scalar>& state_h =
+            estimator->getPoseStateWithLin(tcid_h.frame_id);
+        const PoseStateWithLin<Scalar>& state_t =
+            estimator->getPoseStateWithLin(tcid_t.frame_id);
+
+        // compute relative pose & Jacobians at linearization point
+        Sophus::SE3<Scalar> T_t_h_sophus =
+            computeRelPose(state_h.getPoseLin(), calib.T_i_c[tcid_h.cam_id],
+                           state_t.getPoseLin(), calib.T_i_c[tcid_t.cam_id],
+                           &rpl.d_rel_d_h, &rpl.d_rel_d_t);
+
+        // if either state is already linearized, then the current state
+        // estimate is different from the linearization point, so recompute
+        // the value (not Jacobian) again based on the current state.
+        if (state_h.isLinearized() || state_t.isLinearized()) {
+          T_t_h_sophus =
+              computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
+                             state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
+        }
+
+        rpl.T_t_h = T_t_h_sophus.matrix();
+      } else {
+        rpl.T_t_h.setIdentity();
+        rpl.d_rel_d_h.setZero();
+        rpl.d_rel_d_t.setZero();
+      }
+    }
+  }
+
+  // Linearize landmarks
+  size_t num_landmarks = landmark_blocks.size();
+
+  auto body = [&](const tbb::blocked_range<size_t>& range,
+                  std::pair<Scalar, bool> error_valid) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      error_valid.first += landmark_blocks[r]->linearizeLandmark();
+      error_valid.second =
+          error_valid.second && !landmark_blocks[r]->isNumericalFailure();
+    }
+    return error_valid;
+  };
+
+  std::pair<Scalar, bool> initial_value = {0.0, true};
+  auto join = [](auto p1, auto p2) {
+    p1.first += p2.first;
+    p1.second = p1.second && p2.second;
+    return p1;
+  };
+
+  tbb::blocked_range<size_t> range(0, num_landmarks);
+  auto reduction_res = tbb::parallel_reduce(range, initial_value, body, join);
+
+  if (numerically_valid) *numerically_valid = reduction_res.second;
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      reduction_res.first += imu_block->linearizeImu(estimator->frame_states);
+    }
+  }
+
+  if (marg_lin_data) {
+    Scalar marg_prior_error;
+    estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
+    reduction_res.first += marg_prior_error;
+  }
+
+  return reduction_res.first;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::performQR() {
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->performQR();
+    }
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::setPoseDamping(
+    const Scalar lambda) {
+  BASALT_ASSERT(lambda >= 0);
+
+  pose_damping_diagonal = lambda;
+  pose_damping_diagonal_sqrt = std::sqrt(lambda);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsQR<Scalar, POSE_SIZE>::backSubstitute(
+    const VecX& pose_inc) {
+  BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
+
+  auto body = [&](const tbb::blocked_range<size_t>& range, Scalar l_diff) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->backSubstitute(pose_inc, l_diff);
+    }
+    return l_diff;
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  Scalar l_diff =
+      tbb::parallel_reduce(range, Scalar(0), body, std::plus<Scalar>());
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->backSubstitute(pose_inc, l_diff);
+    }
+  }
+
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    VecX pose_inc_marg = pose_inc.head(marg_size);
+
+    l_diff += estimator->computeMargPriorModelCostChange(
+        *marg_lin_data, marg_scaling, pose_inc_marg);
+  }
+
+  return l_diff;
+}
+
+template <typename Scalar, int POSE_SIZE>
+typename LinearizationAbsQR<Scalar, POSE_SIZE>::VecX
+LinearizationAbsQR<Scalar, POSE_SIZE>::getJp_diag2() const {
+  // TODO: group relative by host frame
+
+  struct Reductor {
+    Reductor(size_t num_rows,
+             const std::vector<LandmarkBlockPtr>& landmark_blocks)
+        : num_rows_(num_rows), landmark_blocks_(landmark_blocks) {
+      res_.setZero(num_rows);
+    }
+
+    void operator()(const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        const auto& lb = landmark_blocks_[r];
+        lb->addJp_diag2(res_);
+      }
+    }
+
+    Reductor(Reductor& a, tbb::split)
+        : num_rows_(a.num_rows_), landmark_blocks_(a.landmark_blocks_) {
+      res_.setZero(num_rows_);
+    };
+
+    inline void join(const Reductor& b) { res_ += b.res_; }
+
+    size_t num_rows_;
+    const std::vector<LandmarkBlockPtr>& landmark_blocks_;
+    VecX res_;
+  };
+
+  Reductor r(aom.total_size, landmark_blocks);
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_reduce(range, r);
+  // r(range);
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->addJp_diag2(r.res_);
+    }
+  }
+
+  // TODO: We don't include pose damping here, b/c we use this to compute
+  // jacobian scale. Make sure this is clear in the the usage, possibly rename
+  // to reflect this, or add assert such that it fails when pose damping is
+  // set.
+
+  // Note: ignore damping here
+
+  // Add marginalization prior
+  // Asumes marginalization part is in the head. Check for this is located
+  // outside
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    if (marg_scaling.rows() > 0) {
+      r.res_.head(marg_size) += (marg_lin_data->H * marg_scaling.asDiagonal())
+                                    .colwise()
+                                    .squaredNorm();
+    } else {
+      r.res_.head(marg_size) += marg_lin_data->H.colwise().squaredNorm();
+    }
+  }
+
+  return r.res_;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::scaleJl_cols() {
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->scaleJl_cols();
+    }
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::scaleJp_cols(
+    const VecX& jacobian_scaling) {
+  //    auto body = [&](const tbb::blocked_range<size_t>& range) {
+  //      for (size_t r = range.begin(); r != range.end(); ++r) {
+  //        landmark_blocks[r]->scaleJp_cols(jacobian_scaling);
+  //      }
+  //    };
+
+  //    tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  //    tbb::parallel_for(range, body);
+
+  if (true) {
+    // In case of absolute poses, we scale Jp in the LMB.
+
+    auto body = [&](const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        landmark_blocks[r]->scaleJp_cols(jacobian_scaling);
+      }
+    };
+
+    tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+    tbb::parallel_for(range, body);
+  } else {
+    // In case LMB use relative poses we cannot directly scale the relative pose
+    // Jacobians. We have
+    //
+    //     Jp * diag(S) = Jp_rel * J_rel_to_abs * diag(S)
+    //
+    // so instead we scale the rel-to-abs jacobians.
+    //
+    // Note that since we do perform operations like J^T * J on the relative
+    // pose Jacobians, we should maybe consider additional scaling like
+    //
+    //     (Jp_rel * diag(S_rel)) * (diag(S_rel)^-1 * J_rel_to_abs * diag(S)),
+    //
+    // but this might be only relevant if we do something more sensitive like
+    // also include camera intrinsics in the optimization.
+
+    for (auto& [k, v] : relative_pose_lin) {
+      size_t h_idx = aom.abs_order_map.at(k.first.frame_id).first;
+      size_t t_idx = aom.abs_order_map.at(k.second.frame_id).first;
+
+      v.d_rel_d_h *=
+          jacobian_scaling.template segment<POSE_SIZE>(h_idx).asDiagonal();
+
+      v.d_rel_d_t *=
+          jacobian_scaling.template segment<POSE_SIZE>(t_idx).asDiagonal();
+    }
+  }
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->scaleJp_cols(jacobian_scaling);
+    }
+  }
+
+  // Add marginalization scaling
+  if (marg_lin_data) {
+    // We are only supposed to apply the scaling once.
+    BASALT_ASSERT(marg_scaling.size() == 0);
+
+    size_t marg_size = marg_lin_data->H.cols();
+    marg_scaling = jacobian_scaling.head(marg_size);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->setLandmarkDamping(lambda);
+    }
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
+    MatX& Q2Jp, VecX& Q2r) const {
+  size_t total_size = num_rows_Q2r;
+  size_t poses_size = aom.total_size;
+
+  size_t lm_start_idx = 0;
+
+  // Space for IMU data if present
+  size_t imu_start_idx = total_size;
+  if (imu_lin_data) {
+    total_size += imu_lin_data->imu_meas.size() * POSE_VEL_BIAS_SIZE;
+  }
+
+  // Space for damping if present
+  size_t damping_start_idx = total_size;
+  if (hasPoseDamping()) {
+    total_size += poses_size;
+  }
+
+  // Space for marg-prior if present
+  size_t marg_start_idx = total_size;
+  if (marg_lin_data) total_size += marg_lin_data->H.rows();
+
+  Q2Jp.setZero(total_size, poses_size);
+  Q2r.setZero(total_size);
+
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      const auto& lb = landmark_blocks[r];
+      lb->get_dense_Q2Jp_Q2r(Q2Jp, Q2r, lm_start_idx + landmark_block_idx[r]);
+    }
+  };
+
+  // go over all host frames
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+  // body(range);
+
+  if (imu_lin_data) {
+    size_t start_idx = imu_start_idx;
+    for (const auto& imu_block : imu_blocks) {
+      imu_block->add_dense_Q2Jp_Q2r(Q2Jp, Q2r, start_idx);
+      start_idx += POSE_VEL_BIAS_SIZE;
+    }
+  }
+
+  // Add damping
+  get_dense_Q2Jp_Q2r_pose_damping(Q2Jp, damping_start_idx);
+
+  // Add marginalization
+  get_dense_Q2Jp_Q2r_marg_prior(Q2Jp, Q2r, marg_start_idx);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
+                                                          VecX& b) const {
+  struct Reductor {
+    Reductor(size_t opt_size,
+             const std::vector<LandmarkBlockPtr>& landmark_blocks)
+        : opt_size_(opt_size), landmark_blocks_(landmark_blocks) {
+      H_.setZero(opt_size_, opt_size_);
+      b_.setZero(opt_size_);
+    }
+
+    void operator()(const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        auto& lb = landmark_blocks_[r];
+        lb->add_dense_H_b(H_, b_);
+      }
+    }
+
+    Reductor(Reductor& a, tbb::split)
+        : opt_size_(a.opt_size_), landmark_blocks_(a.landmark_blocks_) {
+      H_.setZero(opt_size_, opt_size_);
+      b_.setZero(opt_size_);
+    };
+
+    inline void join(Reductor& b) {
+      H_ += b.H_;
+      b_ += b.b_;
+    }
+
+    size_t opt_size_;
+    const std::vector<LandmarkBlockPtr>& landmark_blocks_;
+
+    MatX H_;
+    VecX b_;
+  };
+
+  size_t opt_size = aom.total_size;
+
+  Reductor r(opt_size, landmark_blocks);
+
+  // go over all host frames
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_reduce(range, r);
+
+  // Add imu
+  add_dense_H_b_imu(r.H_, r.b_);
+
+  // Add damping
+  add_dense_H_b_pose_damping(r.H_);
+
+  // Add marginalization
+  add_dense_H_b_marg_prior(r.H_, r.b_);
+
+  H = std::move(r.H_);
+  b = std::move(r.b_);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
+    MatX& Q2Jp, size_t start_idx) const {
+  size_t poses_size = num_cameras * POSE_SIZE;
+  if (hasPoseDamping()) {
+    Q2Jp.template block(start_idx, 0, poses_size, poses_size)
+        .diagonal()
+        .array() = pose_damping_diagonal_sqrt;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
+    MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
+  if (!marg_lin_data) return;
+
+  BASALT_ASSERT(marg_lin_data->is_sqrt);
+
+  size_t marg_rows = marg_lin_data->H.rows();
+  size_t marg_cols = marg_lin_data->H.cols();
+
+  VecX delta;
+  estimator->computeDelta(marg_lin_data->order, delta);
+
+  if (marg_scaling.rows() > 0) {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
+        marg_lin_data->H * marg_scaling.asDiagonal();
+  } else {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
+  }
+
+  Q2r.template segment(start_idx, marg_rows) =
+      marg_lin_data->H * delta + marg_lin_data->b;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
+    MatX& H) const {
+  if (hasPoseDamping()) {
+    H.diagonal().array() += pose_damping_diagonal;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
+    MatX& H, VecX& b) const {
+  if (!marg_lin_data) return;
+
+  // Scaling not supported ATM
+  BASALT_ASSERT(marg_scaling.rows() == 0);
+
+  Scalar marg_prior_error;
+  estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
+
+  //  size_t marg_size = marg_lin_data->H.cols();
+
+  //  VecX delta;
+  //  estimator->computeDelta(marg_lin_data->order, delta);
+
+  //  if (marg_scaling.rows() > 0) {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
+  //        marg_lin_data->H * marg_scaling.asDiagonal();
+
+  //    b.head(marg_size) += marg_scaling.asDiagonal() *
+  //                         marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+
+  //  } else {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_lin_data->H.transpose() * marg_lin_data->H;
+
+  //    b.head(marg_size) += marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+  //  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_imu(
+    DenseAccumulator<Scalar>& accum) const {
+  if (!imu_lin_data) return;
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
+                                                              VecX& b) const {
+  if (!imu_lin_data) return;
+
+  // workaround: create an accumulator here, to avoid implementing the
+  // add_dense_H_b(H, b) overload in ImuBlock
+  DenseAccumulator<Scalar> accum;
+  accum.reset(b.size());
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+
+  H += accum.getH();
+  b += accum.getB();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template class LinearizationAbsQR<double, 6>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template class LinearizationAbsQR<float, 6>;
+#endif
+
+}  // namespace basalt

src/linearization/linearization_abs_sc.cpp

@@ -0,0 +1,432 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_abs_sc.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+#include <basalt/vi_estimator/sc_ba_base.h>
+#include <basalt/linearization/imu_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsSC<Scalar, POSE_SIZE>::LinearizationAbsSC(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg)
+    : options_(options),
+      estimator(estimator),
+      lmdb_(estimator->lmdb),
+      calib(estimator->calib),
+      aom(aom),
+      used_frames(used_frames),
+      marg_lin_data(marg_lin_data),
+      imu_lin_data(imu_lin_data),
+      lost_landmarks(lost_landmarks),
+      last_state_to_marg(last_state_to_marg),
+      pose_damping_diagonal(0),
+      pose_damping_diagonal_sqrt(0) {
+  BASALT_ASSERT_STREAM(
+      options.lb_options.huber_parameter == estimator->huber_thresh,
+      "Huber threshold should be set to the same value");
+
+  BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
+                       "obs_std_dev should be set to the same value");
+
+  if (imu_lin_data) {
+    for (const auto& kv : imu_lin_data->imu_meas) {
+      imu_blocks.emplace_back(
+          new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
+    }
+  }
+
+  //    std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
+  //    num_cameras
+  //              << std::endl;
+}
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsSC<Scalar, POSE_SIZE>::~LinearizationAbsSC() = default;
+
+template <typename Scalar_, int POSE_SIZE_>
+void LinearizationAbsSC<Scalar_, POSE_SIZE_>::log_problem_stats(
+    ExecutionStats& stats) const {
+  UNUSED(stats);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsSC<Scalar, POSE_SIZE>::linearizeProblem(
+    bool* numerically_valid) {
+  // reset damping and scaling (might be set from previous iteration)
+  pose_damping_diagonal = 0;
+  pose_damping_diagonal_sqrt = 0;
+  marg_scaling = VecX();
+
+  std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
+      obs_to_lin;
+
+  if (used_frames) {
+    const auto& obs = lmdb_.getObservations();
+
+    // select all observations where the host frame is about to be
+    // marginalized
+
+    if (lost_landmarks) {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        } else {
+          std::map<TimeCamId, std::set<KeypointId>> lost_obs_map;
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            for (const auto& lm_id : it2->second) {
+              if (lost_landmarks->count(lm_id)) {
+                if (it2->first.frame_id <= last_state_to_marg)
+                  lost_obs_map[it2->first].emplace(lm_id);
+              }
+            }
+          }
+          if (!lost_obs_map.empty()) {
+            obs_to_lin[it->first] = lost_obs_map;
+          }
+        }
+      }
+
+    } else {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        }
+      }
+    }
+  } else {
+    obs_to_lin = lmdb_.getObservations();
+  }
+
+  Scalar error;
+
+  ScBundleAdjustmentBase<Scalar>::linearizeHelperAbsStatic(ald_vec, obs_to_lin,
+                                                           estimator, error);
+
+  // TODO: Fix the computation of numarically valid points
+  if (numerically_valid) *numerically_valid = true;
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      error += imu_block->linearizeImu(estimator->frame_states);
+    }
+  }
+
+  if (marg_lin_data) {
+    Scalar marg_prior_error;
+    estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
+    error += marg_prior_error;
+  }
+
+  return error;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::performQR() {}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::setPoseDamping(
+    const Scalar lambda) {
+  BASALT_ASSERT(lambda >= 0);
+
+  pose_damping_diagonal = lambda;
+  pose_damping_diagonal_sqrt = std::sqrt(lambda);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsSC<Scalar, POSE_SIZE>::backSubstitute(
+    const VecX& pose_inc) {
+  BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
+
+  // Update points
+  tbb::blocked_range<size_t> keys_range(0, ald_vec.size());
+  auto update_points_func = [&](const tbb::blocked_range<size_t>& r,
+                                Scalar l_diff) {
+    for (size_t i = r.begin(); i != r.end(); ++i) {
+      const auto& ald = ald_vec[i];
+      ScBundleAdjustmentBase<Scalar>::updatePointsAbs(aom, ald, -pose_inc,
+                                                      lmdb_, &l_diff);
+    }
+
+    return l_diff;
+  };
+  Scalar l_diff = tbb::parallel_reduce(keys_range, Scalar(0),
+                                       update_points_func, std::plus<Scalar>());
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->backSubstitute(pose_inc, l_diff);
+    }
+  }
+
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    VecX pose_inc_marg = pose_inc.head(marg_size);
+
+    l_diff += estimator->computeMargPriorModelCostChange(
+        *marg_lin_data, marg_scaling, pose_inc_marg);
+  }
+
+  return l_diff;
+}
+
+template <typename Scalar, int POSE_SIZE>
+typename LinearizationAbsSC<Scalar, POSE_SIZE>::VecX
+LinearizationAbsSC<Scalar, POSE_SIZE>::getJp_diag2() const {
+  // TODO: group relative by host frame
+
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::scaleJl_cols() {
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::scaleJp_cols(
+    const VecX& jacobian_scaling) {
+  UNUSED(jacobian_scaling);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
+  UNUSED(lambda);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
+    MatX& Q2Jp, VecX& Q2r) const {
+  MatX H;
+  VecX b;
+  get_dense_H_b(H, b);
+
+  Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+
+  VecX D_sqrt = ldlt.vectorD().array().max(0).sqrt().matrix();
+
+  // After LDLT, we have
+  //     marg_H == P^T*L*D*L^T*P,
+  // so we compute the square root as
+  //     marg_sqrt_H = sqrt(D)*L^T*P,
+  // such that
+  //     marg_sqrt_H^T marg_sqrt_H == marg_H.
+  Q2Jp.setIdentity(H.rows(), H.cols());
+  Q2Jp = ldlt.transpositionsP() * Q2Jp;
+  Q2Jp = ldlt.matrixU() * Q2Jp;  // U == L^T
+  Q2Jp = D_sqrt.asDiagonal() * Q2Jp;
+
+  // For the right hand side, we want
+  //     marg_b == marg_sqrt_H^T * marg_sqrt_b
+  // so we compute
+  //     marg_sqrt_b = (marg_sqrt_H^T)^-1 * marg_b
+  //                 = (P^T*L*sqrt(D))^-1 * marg_b
+  //                 = sqrt(D)^-1 * L^-1 * P * marg_b
+  Q2r = ldlt.transpositionsP() * b;
+  ldlt.matrixL().solveInPlace(Q2r);
+
+  // We already clamped negative values in D_sqrt to 0 above, but for values
+  // close to 0 we set b to 0.
+  for (int i = 0; i < Q2r.size(); ++i) {
+    if (D_sqrt(i) > std::sqrt(std::numeric_limits<Scalar>::min()))
+      Q2r(i) /= D_sqrt(i);
+    else
+      Q2r(i) = 0;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
+                                                          VecX& b) const {
+  typename ScBundleAdjustmentBase<Scalar>::template LinearizeAbsReduce2<
+      DenseAccumulator<Scalar>>
+      lopt_abs(aom);
+
+  tbb::blocked_range<typename Eigen::aligned_vector<AbsLinData>::const_iterator>
+      range(ald_vec.cbegin(), ald_vec.cend());
+  tbb::parallel_reduce(range, lopt_abs);
+  // lopt_abs(range);
+
+  // Add imu
+  add_dense_H_b_imu(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  // Add damping
+  add_dense_H_b_pose_damping(lopt_abs.accum.getH());
+
+  // Add marginalization
+  add_dense_H_b_marg_prior(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  H = std::move(lopt_abs.accum.getH());
+  b = std::move(lopt_abs.accum.getB());
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
+    MatX& Q2Jp, size_t start_idx) const {
+  UNUSED(Q2Jp);
+  UNUSED(start_idx);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
+    MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
+  if (!marg_lin_data) return;
+
+  size_t marg_rows = marg_lin_data->H.rows();
+  size_t marg_cols = marg_lin_data->H.cols();
+
+  VecX delta;
+  estimator->computeDelta(marg_lin_data->order, delta);
+
+  if (marg_scaling.rows() > 0) {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
+        marg_lin_data->H * marg_scaling.asDiagonal();
+  } else {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
+  }
+
+  Q2r.template segment(start_idx, marg_rows) =
+      marg_lin_data->H * delta + marg_lin_data->b;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
+    MatX& H) const {
+  if (hasPoseDamping()) {
+    H.diagonal().array() += pose_damping_diagonal;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
+    MatX& H, VecX& b) const {
+  if (!marg_lin_data) return;
+
+  BASALT_ASSERT(marg_scaling.rows() == 0);
+
+  Scalar marg_prior_error;
+  estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
+
+  //  size_t marg_size = marg_lin_data->H.cols();
+
+  //  VecX delta;
+  //  estimator->computeDelta(marg_lin_data->order, delta);
+
+  // Scaling not supported ATM
+
+  //  if (marg_scaling.rows() > 0) {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
+  //        marg_lin_data->H * marg_scaling.asDiagonal();
+
+  //    b.head(marg_size) += marg_scaling.asDiagonal() *
+  //                         marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+
+  //  } else {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_lin_data->H.transpose() * marg_lin_data->H;
+
+  //    b.head(marg_size) += marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+  //  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(
+    DenseAccumulator<Scalar>& accum) const {
+  if (!imu_lin_data) return;
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
+                                                              VecX& b) const {
+  if (!imu_lin_data) return;
+
+  // workaround: create an accumulator here, to avoid implementing the
+  // add_dense_H_b(H, b) overload in ImuBlock
+  DenseAccumulator<Scalar> accum;
+  accum.reset(b.size());
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+
+  H += accum.getH();
+  b += accum.getB();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template class LinearizationAbsSC<double, 6>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template class LinearizationAbsSC<float, 6>;
+#endif
+
+}  // namespace basalt

src/linearization/linearization_base.cpp

@@ -0,0 +1,119 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_abs_qr.hpp>
+#include <basalt/linearization/linearization_abs_sc.hpp>
+#include <basalt/linearization/linearization_base.hpp>
+#include <basalt/linearization/linearization_rel_sc.hpp>
+
+#include <magic_enum.hpp>
+
+namespace basalt {
+
+bool isLinearizationSqrt(const LinearizationType& type) {
+  switch (type) {
+    case LinearizationType::ABS_QR:
+      return true;
+    case LinearizationType::ABS_SC:
+    case LinearizationType::REL_SC:
+      return false;
+    default:
+      BASALT_ASSERT_STREAM(false, "Linearization type is not supported.");
+      return false;
+  }
+}
+
+template <typename Scalar_, int POSE_SIZE_>
+std::unique_ptr<LinearizationBase<Scalar_, POSE_SIZE_>>
+LinearizationBase<Scalar_, POSE_SIZE_>::create(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg) {
+  //  std::cout << "Creaing Linearization of type "
+  //            << magic_enum::enum_name(options.linearization_type) <<
+  //            std::endl;
+
+  switch (options.linearization_type) {
+    case LinearizationType::ABS_QR:
+      return std::make_unique<LinearizationAbsQR<Scalar, POSE_SIZE>>(
+          estimator, aom, options, marg_lin_data, imu_lin_data, used_frames,
+          lost_landmarks, last_state_to_marg);
+
+    case LinearizationType::ABS_SC:
+      return std::make_unique<LinearizationAbsSC<Scalar, POSE_SIZE>>(
+          estimator, aom, options, marg_lin_data, imu_lin_data, used_frames,
+          lost_landmarks, last_state_to_marg);
+
+    case LinearizationType::REL_SC:
+      return std::make_unique<LinearizationRelSC<Scalar, POSE_SIZE>>(
+          estimator, aom, options, marg_lin_data, imu_lin_data, used_frames,
+          lost_landmarks, last_state_to_marg);
+
+    default:
+      std::cerr << "Could not select a valid linearization." << std::endl;
+      std::abort();
+  }
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate factory templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template std::unique_ptr<LinearizationBase<double, 6>>
+LinearizationBase<double, 6>::create(
+    BundleAdjustmentBase<double>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<double>* marg_lin_data,
+    const ImuLinData<double>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg);
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template std::unique_ptr<LinearizationBase<float, 6>>
+LinearizationBase<float, 6>::create(
+    BundleAdjustmentBase<float>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<float>* marg_lin_data,
+    const ImuLinData<float>* imu_lin_data, const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg);
+#endif
+
+}  // namespace basalt

src/linearization/linearization_rel_sc.cpp

@@ -0,0 +1,431 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_rel_sc.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+#include <basalt/vi_estimator/sc_ba_base.h>
+#include <basalt/linearization/imu_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationRelSC<Scalar, POSE_SIZE>::LinearizationRelSC(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg)
+    : options_(options),
+      estimator(estimator),
+      lmdb_(estimator->lmdb),
+      calib(estimator->calib),
+      aom(aom),
+      used_frames(used_frames),
+      marg_lin_data(marg_lin_data),
+      imu_lin_data(imu_lin_data),
+      lost_landmarks(lost_landmarks),
+      last_state_to_marg(last_state_to_marg),
+      pose_damping_diagonal(0),
+      pose_damping_diagonal_sqrt(0) {
+  BASALT_ASSERT_STREAM(
+      options.lb_options.huber_parameter == estimator->huber_thresh,
+      "Huber threshold should be set to the same value");
+
+  BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
+                       "obs_std_dev should be set to the same value");
+
+  if (imu_lin_data) {
+    for (const auto& kv : imu_lin_data->imu_meas) {
+      imu_blocks.emplace_back(
+          new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
+    }
+  }
+
+  //    std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
+  //    num_cameras
+  //              << std::endl;
+}
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationRelSC<Scalar, POSE_SIZE>::~LinearizationRelSC() = default;
+
+template <typename Scalar_, int POSE_SIZE_>
+void LinearizationRelSC<Scalar_, POSE_SIZE_>::log_problem_stats(
+    ExecutionStats& stats) const {
+  UNUSED(stats);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationRelSC<Scalar, POSE_SIZE>::linearizeProblem(
+    bool* numerically_valid) {
+  // reset damping and scaling (might be set from previous iteration)
+  pose_damping_diagonal = 0;
+  pose_damping_diagonal_sqrt = 0;
+  marg_scaling = VecX();
+
+  std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
+      obs_to_lin;
+
+  if (used_frames) {
+    const auto& obs = lmdb_.getObservations();
+
+    // select all observations where the host frame is about to be
+    // marginalized
+
+    if (lost_landmarks) {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        } else {
+          std::map<TimeCamId, std::set<KeypointId>> lost_obs_map;
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            for (const auto& lm_id : it2->second) {
+              if (lost_landmarks->count(lm_id)) {
+                if (it2->first.frame_id <= last_state_to_marg)
+                  lost_obs_map[it2->first].emplace(lm_id);
+              }
+            }
+          }
+          if (!lost_obs_map.empty()) {
+            obs_to_lin[it->first] = lost_obs_map;
+          }
+        }
+      }
+
+    } else {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            // TODO: Check how ABS_QR works without last_state_to_marg
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        }
+      }
+    }
+  } else {
+    obs_to_lin = lmdb_.getObservations();
+  }
+
+  Scalar error;
+
+  ScBundleAdjustmentBase<Scalar>::linearizeHelperStatic(rld_vec, obs_to_lin,
+                                                        estimator, error);
+
+  // TODO: Fix the computation of numerically valid points
+  if (numerically_valid) *numerically_valid = true;
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      error += imu_block->linearizeImu(estimator->frame_states);
+    }
+  }
+
+  if (marg_lin_data) {
+    Scalar marg_prior_error;
+    estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
+    error += marg_prior_error;
+  }
+
+  return error;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::performQR() {}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::setPoseDamping(
+    const Scalar lambda) {
+  BASALT_ASSERT(lambda >= 0);
+
+  pose_damping_diagonal = lambda;
+  pose_damping_diagonal_sqrt = std::sqrt(lambda);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationRelSC<Scalar, POSE_SIZE>::backSubstitute(
+    const VecX& pose_inc) {
+  BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
+
+  // Update points
+  tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+  auto update_points_func = [&](const tbb::blocked_range<size_t>& r,
+                                Scalar l_diff) {
+    for (size_t i = r.begin(); i != r.end(); ++i) {
+      const auto& rld = rld_vec[i];
+      ScBundleAdjustmentBase<Scalar>::updatePoints(aom, rld, -pose_inc, lmdb_,
+                                                   &l_diff);
+    }
+
+    return l_diff;
+  };
+  Scalar l_diff = tbb::parallel_reduce(keys_range, Scalar(0),
+                                       update_points_func, std::plus<Scalar>());
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->backSubstitute(pose_inc, l_diff);
+    }
+  }
+
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    VecX pose_inc_marg = pose_inc.head(marg_size);
+
+    l_diff += estimator->computeMargPriorModelCostChange(
+        *marg_lin_data, marg_scaling, pose_inc_marg);
+  }
+
+  return l_diff;
+}
+
+template <typename Scalar, int POSE_SIZE>
+typename LinearizationRelSC<Scalar, POSE_SIZE>::VecX
+LinearizationRelSC<Scalar, POSE_SIZE>::getJp_diag2() const {
+  // TODO: group relative by host frame
+
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::scaleJl_cols() {
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::scaleJp_cols(
+    const VecX& jacobian_scaling) {
+  UNUSED(jacobian_scaling);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
+  UNUSED(lambda);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
+    MatX& Q2Jp, VecX& Q2r) const {
+  MatX H;
+  VecX b;
+  get_dense_H_b(H, b);
+
+  Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+
+  VecX D_sqrt = ldlt.vectorD().array().max(0).sqrt().matrix();
+
+  // After LDLT, we have
+  //     marg_H == P^T*L*D*L^T*P,
+  // so we compute the square root as
+  //     marg_sqrt_H = sqrt(D)*L^T*P,
+  // such that
+  //     marg_sqrt_H^T marg_sqrt_H == marg_H.
+  Q2Jp.setIdentity(H.rows(), H.cols());
+  Q2Jp = ldlt.transpositionsP() * Q2Jp;
+  Q2Jp = ldlt.matrixU() * Q2Jp;  // U == L^T
+  Q2Jp = D_sqrt.asDiagonal() * Q2Jp;
+
+  // For the right hand side, we want
+  //     marg_b == marg_sqrt_H^T * marg_sqrt_b
+  // so we compute
+  //     marg_sqrt_b = (marg_sqrt_H^T)^-1 * marg_b
+  //                 = (P^T*L*sqrt(D))^-1 * marg_b
+  //                 = sqrt(D)^-1 * L^-1 * P * marg_b
+  Q2r = ldlt.transpositionsP() * b;
+  ldlt.matrixL().solveInPlace(Q2r);
+
+  // We already clamped negative values in D_sqrt to 0 above, but for values
+  // close to 0 we set b to 0.
+  for (int i = 0; i < Q2r.size(); ++i) {
+    if (D_sqrt(i) > std::sqrt(std::numeric_limits<Scalar>::min()))
+      Q2r(i) /= D_sqrt(i);
+    else
+      Q2r(i) = 0;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
+                                                          VecX& b) const {
+  typename ScBundleAdjustmentBase<Scalar>::template LinearizeAbsReduce<
+      DenseAccumulator<Scalar>>
+      lopt_abs(aom);
+
+  tbb::blocked_range<typename Eigen::aligned_vector<RelLinData>::const_iterator>
+      range(rld_vec.cbegin(), rld_vec.cend());
+  tbb::parallel_reduce(range, lopt_abs);
+
+  // Add imu
+  add_dense_H_b_imu(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  // Add damping
+  add_dense_H_b_pose_damping(lopt_abs.accum.getH());
+
+  // Add marginalization
+  add_dense_H_b_marg_prior(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  H = std::move(lopt_abs.accum.getH());
+  b = std::move(lopt_abs.accum.getB());
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
+    MatX& Q2Jp, size_t start_idx) const {
+  UNUSED(Q2Jp);
+  UNUSED(start_idx);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
+    MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
+  if (!marg_lin_data) return;
+
+  size_t marg_rows = marg_lin_data->H.rows();
+  size_t marg_cols = marg_lin_data->H.cols();
+
+  VecX delta;
+  estimator->computeDelta(marg_lin_data->order, delta);
+
+  if (marg_scaling.rows() > 0) {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
+        marg_lin_data->H * marg_scaling.asDiagonal();
+  } else {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
+  }
+
+  Q2r.template segment(start_idx, marg_rows) =
+      marg_lin_data->H * delta + marg_lin_data->b;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
+    MatX& H) const {
+  if (hasPoseDamping()) {
+    H.diagonal().array() += pose_damping_diagonal;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
+    MatX& H, VecX& b) const {
+  if (!marg_lin_data) return;
+
+  // Scaling not supported ATM
+  BASALT_ASSERT(marg_scaling.rows() == 0);
+
+  Scalar marg_prior_error;
+  estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
+
+  //  size_t marg_size = marg_lin_data->H.cols();
+
+  //  VecX delta;
+  //  estimator->computeDelta(marg_lin_data->order, delta);
+
+  //  if (marg_scaling.rows() > 0) {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
+  //        marg_lin_data->H * marg_scaling.asDiagonal();
+
+  //    b.head(marg_size) += marg_scaling.asDiagonal() *
+  //                         marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+
+  //  } else {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_lin_data->H.transpose() * marg_lin_data->H;
+
+  //    b.head(marg_size) += marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+  //  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(
+    DenseAccumulator<Scalar>& accum) const {
+  if (!imu_lin_data) return;
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
+                                                              VecX& b) const {
+  if (!imu_lin_data) return;
+
+  // workaround: create an accumulator here, to avoid implementing the
+  // add_dense_H_b(H, b) overload in ImuBlock
+  DenseAccumulator<Scalar> accum;
+  accum.reset(b.size());
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+
+  H += accum.getH();
+  b += accum.getB();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template class LinearizationRelSC<double, 6>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template class LinearizationRelSC<float, 6>;
+#endif
+
+}  // namespace basalt

src/mapper_sim.cpp

@@ -60,7 +60,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <basalt/utils/nfr.h>
 #include <basalt/utils/sim_utils.h>
 #include <basalt/utils/vis_utils.h>
-#include <basalt/vi_estimator/keypoint_vio.h>
+#include <basalt/vi_estimator/marg_helper.h>
 #include <basalt/vi_estimator/nfr_mapper.h>
 #include <basalt/calibration/calibration.hpp>
 
@@ -360,7 +360,7 @@ void processMargData(basalt::MargData& m) {
             std::make_pair(aom_new.total_size, POSE_SIZE);
         aom_new.total_size += POSE_SIZE;
 
-        basalt::PoseStateWithLin p = m.frame_states.at(kv.first);
+        basalt::PoseStateWithLin<double> p(m.frame_states.at(kv.first));
         m.frame_poses[kv.first] = p;
         m.frame_states.erase(kv.first);
       } else {
@@ -379,7 +379,7 @@ void processMargData(basalt::MargData& m) {
 
   Eigen::MatrixXd marg_H_new;
   Eigen::VectorXd marg_b_new;
-  basalt::KeypointVioEstimator::marginalizeHelper(
+  basalt::MargHelper<double>::marginalizeHelperSqToSq(
       m.abs_H, m.abs_b, idx_to_keep, idx_to_marg, marg_H_new, marg_b_new);
 
   std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "

src/mapper_sim_naive.cpp

@@ -55,7 +55,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <basalt/io/marg_data_io.h>
 #include <basalt/spline/se3_spline.h>
 #include <basalt/utils/sim_utils.h>
-#include <basalt/vi_estimator/keypoint_vio.h>
+#include <basalt/vi_estimator/vio_estimator.h>
 
 #include <basalt/calibration/calibration.hpp>
 
@@ -102,7 +102,7 @@ std::string marg_data_path;
 
 // VIO vars
 basalt::Calibration<double> calib;
-basalt::KeypointVioEstimator::Ptr vio;
+basalt::VioEstimatorBase::Ptr vio;
 
 // Visualization vars
 std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
@@ -723,7 +723,8 @@ void setup_vio() {
   basalt::VioConfig config;
   config.vio_debug = true;
 
-  vio.reset(new basalt::KeypointVioEstimator(g, calib, config));
+  vio = basalt::VioEstimatorFactory::getVioEstimator(config, calib, g, true,
+                                                     true);
   vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
                   gt_accel_bias.front());
 

src/optical_flow/optical_flow.cpp

@@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <basalt/optical_flow/optical_flow.h>
 
 #include <basalt/optical_flow/frame_to_frame_optical_flow.h>
+#include <basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h>
 #include <basalt/optical_flow/patch_optical_flow.h>
 
 namespace basalt {
@@ -95,6 +96,36 @@ OpticalFlowBase::Ptr OpticalFlowFactory::getOpticalFlow(
         std::abort();
     }
   }
+
+  if (config.optical_flow_type == "multiscale_frame_to_frame") {
+    switch (config.optical_flow_pattern) {
+      case 24:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern24>(
+            config, cam));
+        break;
+
+      case 52:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern52>(
+            config, cam));
+        break;
+
+      case 51:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern51>(
+            config, cam));
+        break;
+
+      case 50:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern50>(
+            config, cam));
+        break;
+
+      default:
+        std::cerr << "config.optical_flow_pattern "
+                  << config.optical_flow_pattern << " is not supported."
+                  << std::endl;
+        std::abort();
+    }
+  }
   return res;
 }
 }  // namespace basalt

src/rs_t265_vio.cpp

@@ -173,7 +173,7 @@ int main(int argc, char** argv) {
   t265_device->image_data_queue = &opt_flow_ptr->input_queue;
 
   vio = basalt::VioEstimatorFactory::getVioEstimator(
-      vio_config, calib, basalt::constants::g, true);
+      vio_config, calib, basalt::constants::g, true, true);
   vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
   t265_device->imu_data_queue = &vio->imu_data_queue;
 

src/utils/keypoints.cpp

@@ -167,19 +167,20 @@ void detectKeypoints(
   kd.corner_descriptors.clear();
 
   const size_t x_start = (img_raw.w % PATCH_SIZE) / 2;
-  const size_t x_stop = x_start + img_raw.w - PATCH_SIZE;
+  const size_t x_stop = x_start + PATCH_SIZE * (img_raw.w / PATCH_SIZE - 1);
 
   const size_t y_start = (img_raw.h % PATCH_SIZE) / 2;
-  const size_t y_stop = y_start + img_raw.h - PATCH_SIZE;
+  const size_t y_stop = y_start + PATCH_SIZE * (img_raw.h / PATCH_SIZE - 1);
 
-  // std::cerr << "x_start " << x_start << " x_stop " << x_stop << std::endl;
+  //  std::cerr << "x_start " << x_start << " x_stop " << x_stop << std::endl;
-  // std::cerr << "y_start " << y_start << " y_stop " << y_stop << std::endl;
+  //  std::cerr << "y_start " << y_start << " y_stop " << y_stop << std::endl;
 
   Eigen::MatrixXi cells;
   cells.setZero(img_raw.h / PATCH_SIZE + 1, img_raw.w / PATCH_SIZE + 1);
 
   for (const Eigen::Vector2d& p : current_points) {
-    if (p[0] >= x_start && p[1] >= y_start) {
+    if (p[0] >= x_start && p[1] >= y_start && p[0] < x_stop + PATCH_SIZE &&
+        p[1] < y_stop + PATCH_SIZE) {
       int x = (p[0] - x_start) / PATCH_SIZE;
       int y = (p[1] - y_start) / PATCH_SIZE;
 
@@ -187,8 +188,8 @@ void detectKeypoints(
     }
   }
 
-  for (size_t x = x_start; x < x_stop; x += PATCH_SIZE) {
+  for (size_t x = x_start; x <= x_stop; x += PATCH_SIZE) {
-    for (size_t y = y_start; y < y_stop; y += PATCH_SIZE) {
+    for (size_t y = y_start; y <= y_stop; y += PATCH_SIZE) {
       if (cells((y - y_start) / PATCH_SIZE, (x - x_start) / PATCH_SIZE) > 0)
         continue;
 

src/utils/system_utils.cpp

@@ -0,0 +1,96 @@
+/**
+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.
+*/
+
+#include <basalt/utils/system_utils.h>
+
+#include <fstream>
+
+#if __APPLE__
+#include <mach/mach.h>
+#include <mach/task.h>
+#elif __linux__
+#include <unistd.h>
+
+#include <sys/resource.h>
+#endif
+
+namespace basalt {
+
+bool get_memory_info(MemoryInfo& info) {
+#if __APPLE__
+  mach_task_basic_info_data_t t_info;
+  mach_msg_type_number_t t_info_count = MACH_TASK_BASIC_INFO_COUNT;
+
+  if (KERN_SUCCESS != task_info(mach_task_self(), MACH_TASK_BASIC_INFO,
+                                (task_info_t)&t_info, &t_info_count)) {
+    return false;
+  }
+  info.resident_memory = t_info.resident_size;
+  info.resident_memory_peak = t_info.resident_size_max;
+
+  /*
+  struct rusage resource_usage;
+  getrusage(RUSAGE_SELF, &resource_usage);
+  info.resident_memory_peak = resource_usage.ru_maxrss;
+  */
+
+  return true;
+#elif __linux__
+
+  // get current memory first
+  std::size_t program_size = 0;
+  std::size_t resident_size = 0;
+
+  std::ifstream fs("/proc/self/statm");
+  if (fs.fail()) {
+    return false;
+  }
+  fs >> program_size;
+  fs >> resident_size;
+
+  info.resident_memory = resident_size * sysconf(_SC_PAGESIZE);
+
+  // get peak memory after that
+  struct rusage resource_usage;
+  getrusage(RUSAGE_SELF, &resource_usage);
+  info.resident_memory_peak = resource_usage.ru_maxrss * 1024;
+
+  return true;
+#else
+  return false;
+#endif
+}
+
+}  // namespace basalt

src/utils/time_utils.cpp

@@ -0,0 +1,212 @@
+#include <basalt/utils/assert.h>
+#include <basalt/utils/time_utils.hpp>
+
+#include <fstream>
+#include <iomanip>
+
+#include <fmt/format.h>
+#include <Eigen/Dense>
+#include <nlohmann/json.hpp>
+
+namespace basalt {
+
+using namespace fmt::literals;
+
+// compute the median of an eigen vector
+// Note: Changes the order of elements in the vector!
+// Note: For even sized vectors we don't return the mean of the middle two, but
+// simply the second one as is.
+template <class Scalar, int Rows>
+Scalar median_non_const(Eigen::Matrix<Scalar, Rows, 1>& vec) {
+  static_assert(Rows != 0);
+  if constexpr (Rows < 0) {
+    BASALT_ASSERT(vec.size() >= 1);
+  }
+  int n = vec.size() / 2;
+  std::nth_element(vec.begin(), vec.begin() + n, vec.end());
+  return vec(n);
+}
+
+template <class Scalar, int N>
+Scalar variance(const Eigen::Matrix<Scalar, N, 1>& vec) {
+  static_assert(N != 0);
+  const Eigen::Matrix<Scalar, N, 1> centered = vec.array() - vec.mean();
+  return centered.squaredNorm() / Scalar(vec.size());
+}
+
+ExecutionStats::Meta& ExecutionStats::add(const std::string& name,
+                                          double value) {
+  auto [it, new_item] = stats_.try_emplace(name);
+  if (new_item) {
+    order_.push_back(name);
+    it->second.data_ = std::vector<double>();
+  }
+  std::get<std::vector<double>>(it->second.data_).push_back(value);
+  return it->second;
+}
+
+ExecutionStats::Meta& ExecutionStats::add(const std::string& name,
+                                          const Eigen::VectorXd& value) {
+  auto [it, new_item] = stats_.try_emplace(name);
+  if (new_item) {
+    order_.push_back(name);
+    it->second.data_ = std::vector<Eigen::VectorXd>();
+  }
+  std::get<std::vector<Eigen::VectorXd>>(it->second.data_).push_back(value);
+  return it->second;
+}
+
+ExecutionStats::Meta& ExecutionStats::add(const std::string& name,
+                                          const Eigen::VectorXf& value) {
+  Eigen::VectorXd x = value.cast<double>();
+  return add(name, x);
+}
+
+void ExecutionStats::merge_all(const ExecutionStats& other) {
+  for (const auto& name : other.order_) {
+    const auto& meta = other.stats_.at(name);
+    std::visit(
+        [&](auto& data) {
+          for (auto v : data) {
+            add(name, v);
+          }
+        },
+        meta.data_);
+    stats_.at(name).set_meta(meta);
+  }
+}
+
+namespace {  // helper
+// ////////////////////////////////////////////////////////////////////////////
+// overloads for generic lambdas
+// See also: https://stackoverflow.com/q/55087826/1813258
+template <class... Ts>
+struct overload : Ts... {
+  using Ts::operator()...;
+};
+template <class... Ts>
+overload(Ts...) -> overload<Ts...>;
+}  // namespace
+
+void ExecutionStats::merge_sums(const ExecutionStats& other) {
+  for (const auto& name : other.order_) {
+    const auto& meta = other.stats_.at(name);
+    std::visit(overload{[&](const std::vector<double>& data) {
+                          Eigen::Map<const Eigen::VectorXd> map(data.data(),
+                                                                data.size());
+                          add(name, map.sum());
+                        },
+                        [&](const std::vector<Eigen::VectorXd>& data) {
+                          UNUSED(data);
+                          // TODO: for now no-op
+                        }},
+               meta.data_);
+    stats_.at(name).set_meta(meta);
+  }
+}
+
+void ExecutionStats::print() const {
+  for (const auto& name : order_) {
+    const auto& meta = stats_.at(name);
+
+    std::visit(
+        overload{
+            [&](const std::vector<double>& data) {
+              Eigen::Map<const Eigen::VectorXd> map(data.data(), data.size());
+
+              // create a copy for median computation
+              Eigen::VectorXd vec = map;
+
+              if (meta.format_ == "ms") {
+                // convert seconds to milliseconds
+                vec *= 1000;
+              }
+
+              int count = vec.size();
+              // double sum = vec.sum();
+              double mean = vec.mean();
+              double stddev = std::sqrt(variance(vec));
+              double max = vec.maxCoeff();
+              double min = vec.minCoeff();
+
+              // double median = median_non_const(vec);
+
+              if (meta.format_ == "count") {
+                std::cout << "{:20} ({:>4}):{: 8.1f}+-{:.1f} [{}, {}]\n"_format(
+                    name, count, mean, stddev, min, max);
+              } else if (meta.format_ != "none") {
+                std::cout
+                    << "{:20} ({:>4}):{: 8.2f}+-{:.2f} [{:.2f}, {:.2f}]\n"_format(
+                           name, count, mean, stddev, min, max);
+              }
+            },
+            [&](const std::vector<Eigen::VectorXd>& data) {
+              int count = data.size();
+              std::cout << "{:20} ({:>4})\n"_format(name, count);
+            }},
+        meta.data_);
+  }
+}
+
+bool ExecutionStats::save_json(const std::string& path) const {
+  using json = nlohmann::json;
+  json result;
+
+  for (const auto& name : order_) {
+    const auto& meta = stats_.at(name);
+
+    std::visit(
+        overload{[&](const std::vector<double>& data) { result[name] = data; },
+                 [&](const std::vector<Eigen::VectorXd>& data) {
+                   std::vector<int> indices;
+                   std::vector<double> values;
+                   for (const auto& v : data) {
+                     indices.push_back(int(values.size()));
+                     values.insert(values.end(), v.begin(), v.end());
+                   }
+                   std::string name_values = std::string(name) + "__values";
+                   std::string name_indices = std::string(name) + "__index";
+                   result[name_indices] = indices;
+                   result[name_values] = values;
+                 }},
+        meta.data_);
+  }
+
+  constexpr bool save_as_json = false;
+  constexpr bool save_as_ubjson = true;
+
+  // save json
+  if (save_as_json) {
+    std::ofstream ofs(path);
+
+    if (!ofs.is_open()) {
+      std::cerr << "Could not save ExecutionStats to {}.\n"_format(path);
+      return false;
+    }
+
+    ofs << std::setw(4) << result;  //!< pretty printing
+    // ofs << result;  //!< no pretty printing
+
+    std::cout << "Saved ExecutionStats to {}.\n"_format(path);
+  }
+
+  // save ubjson
+  if (save_as_ubjson) {
+    std::string ubjson_path =
+        path.substr(0, path.find_last_of('.')) + ".ubjson";
+    std::ofstream ofs(ubjson_path, std::ios_base::binary);
+
+    if (!ofs.is_open()) {
+      std::cerr << "Could not save ExecutionStats to {}.\n"_format(ubjson_path);
+      return false;
+    }
+
+    json::to_ubjson(result, ofs);
+
+    std::cout << "Saved ExecutionStats to {}.\n"_format(ubjson_path);
+  }
+
+  return true;
+}
+
+}  // namespace basalt

src/utils/vio_config.cpp

@@ -33,12 +33,14 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <basalt/utils/assert.h>
 #include <basalt/utils/vio_config.h>
 
 #include <fstream>
 
 #include <cereal/archives/json.hpp>
 #include <cereal/cereal.hpp>
+#include <magic_enum.hpp>
 
 namespace basalt {
 
@@ -53,29 +55,42 @@ VioConfig::VioConfig() {
   optical_flow_epipolar_error = 0.005;
   optical_flow_skip_frames = 1;
 
+  vio_linearization_type = LinearizationType::ABS_QR;
+  vio_sqrt_marg = true;
+
   vio_max_states = 3;
   vio_max_kfs = 7;
   vio_min_frames_after_kf = 5;
   vio_new_kf_keypoints_thresh = 0.7;
 
   vio_debug = false;
+  vio_extended_logging = false;
   vio_obs_std_dev = 0.5;
   vio_obs_huber_thresh = 1.0;
   vio_min_triangulation_dist = 0.05;
-  vio_outlier_threshold = 3.0;
+  //  vio_outlier_threshold = 3.0;
-  vio_filter_iteration = 4;
+  //  vio_filter_iteration = 4;
   vio_max_iterations = 7;
 
   vio_enforce_realtime = false;
 
   vio_use_lm = false;
+  vio_lm_lambda_initial = 1e-8;
   vio_lm_lambda_min = 1e-32;
   vio_lm_lambda_max = 1e2;
+  vio_lm_landmark_damping_variant = 0;
+  vio_lm_pose_damping_variant = 0;
+
+  vio_scale_jacobian = true;
 
   vio_init_pose_weight = 1e8;
   vio_init_ba_weight = 1e1;
   vio_init_bg_weight = 1e2;
 
+  vio_marg_lost_landmarks = true;
+
+  vio_kf_marg_feature_ratio = 0.1;
+
   mapper_obs_std_dev = 0.25;
   mapper_obs_huber_thresh = 1.5;
   mapper_detection_num_points = 800;
@@ -119,6 +134,31 @@ void VioConfig::load(const std::string& filename) {
 
 namespace cereal {
 
+template <class Archive>
+std::string save_minimal(const Archive& ar,
+                         const basalt::LinearizationType& linearization_type) {
+  UNUSED(ar);
+  auto name = magic_enum::enum_name(linearization_type);
+  return std::string(name);
+}
+
+template <class Archive>
+void load_minimal(const Archive& ar,
+                  basalt::LinearizationType& linearization_type,
+                  const std::string& name) {
+  UNUSED(ar);
+
+  auto lin_enum = magic_enum::enum_cast<basalt::LinearizationType>(name);
+
+  if (lin_enum.has_value()) {
+    linearization_type = lin_enum.value();
+  } else {
+    std::cerr << "Could not find the LinearizationType for " << name
+              << std::endl;
+    std::abort();
+  }
+}
+
 template <class Archive>
 void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.optical_flow_type));
@@ -130,14 +170,17 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.optical_flow_levels));
   ar(CEREAL_NVP(config.optical_flow_skip_frames));
 
+  ar(CEREAL_NVP(config.vio_linearization_type));
+  ar(CEREAL_NVP(config.vio_sqrt_marg));
   ar(CEREAL_NVP(config.vio_max_states));
   ar(CEREAL_NVP(config.vio_max_kfs));
   ar(CEREAL_NVP(config.vio_min_frames_after_kf));
   ar(CEREAL_NVP(config.vio_new_kf_keypoints_thresh));
   ar(CEREAL_NVP(config.vio_debug));
+  ar(CEREAL_NVP(config.vio_extended_logging));
   ar(CEREAL_NVP(config.vio_max_iterations));
-  ar(CEREAL_NVP(config.vio_outlier_threshold));
+  //  ar(CEREAL_NVP(config.vio_outlier_threshold));
-  ar(CEREAL_NVP(config.vio_filter_iteration));
+  //  ar(CEREAL_NVP(config.vio_filter_iteration));
 
   ar(CEREAL_NVP(config.vio_obs_std_dev));
   ar(CEREAL_NVP(config.vio_obs_huber_thresh));
@@ -146,13 +189,21 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.vio_enforce_realtime));
 
   ar(CEREAL_NVP(config.vio_use_lm));
+  ar(CEREAL_NVP(config.vio_lm_lambda_initial));
   ar(CEREAL_NVP(config.vio_lm_lambda_min));
   ar(CEREAL_NVP(config.vio_lm_lambda_max));
+  ar(CEREAL_NVP(config.vio_lm_landmark_damping_variant));
+  ar(CEREAL_NVP(config.vio_lm_pose_damping_variant));
+
+  ar(CEREAL_NVP(config.vio_scale_jacobian));
 
   ar(CEREAL_NVP(config.vio_init_pose_weight));
   ar(CEREAL_NVP(config.vio_init_ba_weight));
   ar(CEREAL_NVP(config.vio_init_bg_weight));
 
+  ar(CEREAL_NVP(config.vio_marg_lost_landmarks));
+  ar(CEREAL_NVP(config.vio_kf_marg_feature_ratio));
+
   ar(CEREAL_NVP(config.mapper_obs_std_dev));
   ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
   ar(CEREAL_NVP(config.mapper_detection_num_points));