ICCV'21 square root marginalization paper code release

Major changes:

- New square-root implementation for optimization and
  marginalization, giving faster optimization and numerically
  more stable marginalization. The square root solver is the new
  default, but the Schur complement based implementation is still
  available. (Implements the ICCV'21 paper.)

- The odometry estimator is now fully templetized and you can run
  in float or double. Default is float, which works well with the
  new square-root implementation and gives best runtimes.

- Batch evaluation scripts and documentation to reproduce the
  ICCV'21 experiments.

Additional changes:

- New options in VIO to marginalize lost landmark right away and
  not only when the frame is marginalized (enabled by default).

- small bugfix for keypoint patch extraction bounds

- basalt_vio: more logging for batch evaluation

- basalt_vio: better handling of closing the GUI while estimator is still running

- basalt_vio: new command line argument to limit the number of frames processed

- basalt_vio: new command line argument to save ground truth trajectory

- added unit tests for square root marginalization

- update basalt-headers

- new submodules: gmt, nlohmann/json, magic_enum

.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)
 
+# mapper sim native doesn't use template free interface
+if(BASALT_INSTANTIATIONS_DOUBLE)
   add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp)
   target_link_libraries(basalt_mapper_sim_naive basalt pangolin)
+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)

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,453 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <thread>
+
+#include <sophus/se2.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/concurrent_unordered_map.h>
+#include <tbb/parallel_for.h>
+
+#include <basalt/optical_flow/optical_flow.h>
+#include <basalt/optical_flow/patch.h>
+
+#include <basalt/image/image_pyr.h>
+#include <basalt/utils/keypoints.h>
+
+namespace basalt {
+
+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(
-  template <class CamT>
+      Eigen::aligned_vector<Eigen::Matrix<Scalar2, 3, 1>>& points,
-  static bool linearizePoint(
-      const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
-      const Eigen::Matrix4d& T_t_h, const CamT& cam, Eigen::Vector2d& res,
-      Eigen::Matrix<double, 2, POSE_SIZE>* d_res_d_xi = nullptr,
-      Eigen::Matrix<double, 2, 3>* d_res_d_p = nullptr,
-      Eigen::Vector4d* proj = nullptr) {
-    // Todo implement without jacobians
-    Eigen::Matrix<double, 4, 2> Jup;
-    Eigen::Vector4d p_h_3d;
-    p_h_3d = StereographicParam<double>::unproject(kpt_pos.dir, &Jup);
-    p_h_3d[3] = kpt_pos.id;
-
-    Eigen::Vector4d p_t_3d = T_t_h * p_h_3d;
-
-    Eigen::Matrix<double, 4, POSE_SIZE> d_point_d_xi;
-    d_point_d_xi.topLeftCorner<3, 3>() =
-        Eigen::Matrix3d::Identity() * kpt_pos.id;
-    d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(p_t_3d.head<3>());
-    d_point_d_xi.row(3).setZero();
-
-    Eigen::Matrix<double, 2, 4> Jp;
-    bool valid = cam.project(p_t_3d, res, &Jp);
-    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->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.
+  void computeDelta(const AbsOrderMap& marg_order, VecX& delta) const;
-  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,
+  void linearizeMargPrior(const MargLinData<Scalar>& mld,
-                    Eigen::VectorXd& delta) const;
+                          const AbsOrderMap& aom, MatX& abs_H, VecX& abs_b,
+                          Scalar& marg_prior_error) const;
 
-  void linearizeMargPrior(const AbsOrderMap& marg_order,
+  void computeMargPriorError(const MargLinData<Scalar>& mld,
-                          const Eigen::MatrixXd& marg_H,
+                             Scalar& marg_prior_error) const;
-                          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,
+  Scalar computeMargPriorModelCostChange(const MargLinData<Scalar>& mld,
-                             const Eigen::MatrixXd& marg_H,
+                                         const VecX& marg_scaling,
-                             const Eigen::VectorXd& marg_b,
+                                         const VecX& marg_pose_inc) const;
-                             double& marg_prior_error) const;
 
-  static Eigen::VectorXd checkNullspace(
+  // TODO: Old version for squared H and b. Remove when no longer needed.
-      const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
+  Scalar computeModelCostChange(const MatX& H, const VecX& b,
-      const AbsOrderMap& marg_order,
+                                const VecX& inc) const;
-      const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
+
-          frame_states,
+  template <class Scalar2>
-      const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& frame_poses);
+  void computeProjections(
+      std::vector<Eigen::aligned_vector<Eigen::Matrix<Scalar2, 4, 1>>>& data,
+      FrameId last_state_t_ns) const;
 
   /// 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,10 +167,10 @@ 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 << "y_start " << y_start << " y_stop " << y_stop << std::endl;
@@ -179,7 +179,8 @@ void detectKeypoints(
   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));