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

Merge branch 'root_vo_squashed' into 'master' 

ICCV'21 square root marginalization paper code release

See merge request basalt/basalt!47
This commit is contained in:
Nikolaus Demmel 2021-10-15 13:14:46 +00:00
parent cc6d896c47 8d8a69c775
commit 943137191e
124 changed files with 41723 additions and 3908 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

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

49
.gitlab-ci.yml
View File

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

6
.gitmodules vendored
View File

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

3
.style.yapf Normal file
View File

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

61
CMakeLists.txt
View File

@ -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/vi_estimator/keypoint_vio_linearize.cpp
src/vi_estimator/keypoint_vo.cpp
src/linearization/landmark_block.cpp
src/linearization/linearization_base.cpp
src/linearization/linearization_abs_qr.cpp
src/linearization/linearization_abs_sc.cpp
src/linearization/linearization_rel_sc.cpp
src/utils/vio_config.cpp
src/utils/system_utils.cpp
src/utils/time_utils.cpp
src/utils/keypoints.cpp
src/vi_estimator/marg_helper.cpp
src/vi_estimator/sqrt_keypoint_vio.cpp
src/vi_estimator/sqrt_keypoint_vo.cpp
src/vi_estimator/vio_estimator.cpp
src/vi_estimator/ba_base.cpp
src/vi_estimator/sqrt_ba_base.cpp
src/vi_estimator/sc_ba_base.cpp
src/vi_estimator/nfr_mapper.cpp
src/vi_estimator/landmark_database.cpp
src/utils/keypoints.cpp)
src/vi_estimator/landmark_database.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

7
README.md
View File

@ -27,6 +27,9 @@ Calibration (demonstrates how these tools can be used for dataset calibration):
Calibration (describes B-spline trajectory representation used in camera-IMU calibration):
* **Efficient Derivative Computation for Cumulative B-Splines on Lie Groups**, C. Sommer, V. Usenko, D. Schubert, N. Demmel, D. Cremers, In 2020 Conference on Computer Vision and Pattern Recognition (CVPR), [[DOI:10.1109/CVPR42600.2020.01116]](https://doi.org/10.1109/CVPR42600.2020.01116), [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860).
Optimization (describes square-root optimization and marginalization used in VIO/VO):
* **Square Root Marginalization for Sliding-Window Bundle Adjustment**, N. Demmel, D. Schubert, C. Sommer, D. Cremers, V. Usenko, In 2021 International Conference on Computer Vision (ICCV), [[arXiv:2109.02182]](https://arxiv.org/abs/2109.02182)
## Installation
### APT installation for Ubuntu 20.04 and 18.04 (Fast)
@ -55,6 +58,7 @@ make -j8
* [Visual-inertial odometry and mapping. (TUM-VI and Euroc datasets)](doc/VioMapping.md)
* [Visual odometry (no IMU). (KITTI dataset)](doc/Vo.md)
* [Simulation tools to test different components of the system.](doc/Simulation.md)
* [Batch evaluation tutorial (ICCV'21 experiments)](doc/BatchEvaluation.md)
## Device support
* [Tutorial on Camera-IMU and Motion capture calibration with Realsense T265.](doc/Realsense.md)
@ -65,3 +69,6 @@ make -j8
## Licence
The code is provided under a BSD 3-clause license. See the LICENSE file for details.
Note also the different licenses of thirdparty submodules.
Some improvements are ported back from the fork
[granite](https://github.com/DLR-RM/granite) (MIT license).

14
data/euroc_config.json
View File

@ -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_lm_lambda_min": 1e-32,
"config.vio_use_lm": true,
"config.vio_lm_lambda_initial": 1e-4,
"config.vio_lm_lambda_min": 1e-6,
"config.vio_lm_lambda_max": 1e2,
"config.vio_lm_landmark_damping_variant": 1,
"config.vio_lm_pose_damping_variant": 1,
"config.vio_scale_jacobian": false,
"config.vio_init_pose_weight": 1e8,
"config.vio_init_ba_weight": 1e1,
"config.vio_init_bg_weight": 1e2,
"config.vio_marg_lost_landmarks": true,
"config.vio_kf_marg_feature_ratio": 0.1,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,
"config.mapper_detection_num_points": 800,

10
data/euroc_config_no_factors.json
View File

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

10
data/euroc_config_no_weights.json
View File

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

56
data/euroc_config_vo.json Normal file
View File

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

424
data/iccv21/basalt_batch_config.toml Normal file
View File

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

593
data/iccv21/experiments-iccv.toml Normal file
View File

@ -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",
]

12
data/kitti_config.json
View File

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

13
data/tumvi_512_config.json
View File

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

261
doc/BatchEvaluation.md Normal file
View File

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

1
docker/b_image_bionic/Dockerfile
View File

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

4
include/basalt/io/marg_data_io.h
View File

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

89
include/basalt/linearization/block_diagonal.hpp Normal file
View File

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

235
include/basalt/linearization/imu_block.hpp Normal file
View File

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

140
include/basalt/linearization/landmark_block.hpp Normal file
View File

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

585
include/basalt/linearization/landmark_block_abs_dynamic.hpp Normal file
View File

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

141
include/basalt/linearization/linearization_abs_qr.hpp Normal file
View File

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

142
include/basalt/linearization/linearization_abs_sc.hpp Normal file
View File

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

63
include/basalt/linearization/linearization_base.hpp Normal file
View File

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

143
include/basalt/linearization/linearization_rel_sc.hpp Normal file
View File

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

456
include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h Normal file
View File

@ -0,0 +1,456 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
// Original source for multi-scale implementation:
// https://github.com/DLR-RM/granite (MIT license)
#pragma once
#include <thread>
#include <sophus/se2.hpp>
#include <tbb/blocked_range.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/patch.h>
#include <basalt/image/image_pyr.h>
#include <basalt/utils/keypoints.h>
namespace basalt {
template <typename Scalar, template <typename> typename Pattern>
class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
public:
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Sophus::SE2<Scalar> SE2;
MultiscaleFrameToFrameOpticalFlow(const VioConfig& config,
const basalt::Calibration<double>& calib)
: t_ns(-1), frame_counter(0), last_keypoint_id(0), config(config) {
input_queue.set_capacity(10);
this->calib = calib.cast<Scalar>();
patch_coord = PatchT::pattern2.template cast<float>();
if (calib.intrinsics.size() > 1) {
Eigen::Matrix4d Ed;
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
computeEssential(T_i_j, Ed);
E = Ed.cast<Scalar>();
}
processing_thread.reset(new std::thread(
&MultiscaleFrameToFrameOpticalFlow::processingLoop, this));
}
~MultiscaleFrameToFrameOpticalFlow() { processing_thread->join(); }
void processingLoop() {
OpticalFlowInput::Ptr input_ptr;
while (true) {
input_queue.pop(input_ptr);
if (!input_ptr.get()) {
if (output_queue) output_queue->push(nullptr);
break;
}
processFrame(input_ptr->t_ns, input_ptr);
}
}
bool processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
for (const auto& v : new_img_vec->img_data) {
if (!v.img.get()) {
std::cout << "Image for " << curr_t_ns << " not present!" << std::endl;
return true;
}
}
if (t_ns < 0) {
t_ns = curr_t_ns;
transforms.reset(new OpticalFlowResult);
transforms->observations.resize(calib.intrinsics.size());
transforms->pyramid_levels.resize(calib.intrinsics.size());
transforms->t_ns = t_ns;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<u_int16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
pyramid->at(i).setFromImage(
*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
});
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
} else {
t_ns = curr_t_ns;
old_pyramid = pyramid;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<u_int16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
pyramid->at(i).setFromImage(
*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
});
OpticalFlowResult::Ptr new_transforms;
new_transforms.reset(new OpticalFlowResult);
new_transforms->observations.resize(calib.intrinsics.size());
new_transforms->pyramid_levels.resize(calib.intrinsics.size());
new_transforms->t_ns = t_ns;
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
trackPoints(old_pyramid->at(i), pyramid->at(i),
transforms->observations[i], transforms->pyramid_levels[i],
new_transforms->observations[i],
new_transforms->pyramid_levels[i]);
}
// std::cout << t_ns << ": Could track "
// << new_transforms->observations.at(0).size() << " points."
// << std::endl;
transforms = new_transforms;
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
}
if (frame_counter % config.optical_flow_skip_frames == 0) {
try {
output_queue->push(transforms);
} catch (const tbb::user_abort&) {
return false;
};
}
frame_counter++;
return true;
}
void trackPoints(
const basalt::ManagedImagePyr<u_int16_t>& pyr_1,
const basalt::ManagedImagePyr<u_int16_t>& pyr_2,
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1,
const std::map<KeypointId, size_t>& pyramid_levels_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
std::map<KeypointId, size_t>& pyramid_levels_2) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
std::vector<size_t> pyramid_level;
ids.reserve(num_points);
init_vec.reserve(num_points);
pyramid_level.reserve(num_points);
for (const auto& kv : transform_map_1) {
ids.push_back(kv.first);
init_vec.push_back(kv.second);
pyramid_level.push_back(pyramid_levels_1.at(kv.first));
}
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
std::hash<KeypointId>>
result_transforms;
tbb::concurrent_unordered_map<KeypointId, size_t, std::hash<KeypointId>>
result_pyramid_level;
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
for (size_t r = range.begin(); r != range.end(); ++r) {
const KeypointId id = ids[r];
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1;
bool valid = trackPoint(pyr_1, pyr_2, transform_1, pyramid_level[r],
transform_2);
if (valid) {
Eigen::AffineCompact2f transform_1_recovered = transform_2;
valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r],
transform_1_recovered);
if (valid) {
const Scalar scale = 1 << pyramid_level[r];
Scalar dist2 = (transform_1.translation() / scale -
transform_1_recovered.translation() / scale)
.squaredNorm();
if (dist2 < config.optical_flow_max_recovered_dist2) {
result_transforms[id] = transform_2;
result_pyramid_level[id] = pyramid_level[r];
}
}
}
}
};
tbb::blocked_range<size_t> range(0, num_points);
tbb::parallel_for(range, compute_func);
// compute_func(range);
transform_map_2.clear();
transform_map_2.insert(result_transforms.begin(), result_transforms.end());
pyramid_levels_2.clear();
pyramid_levels_2.insert(result_pyramid_level.begin(),
result_pyramid_level.end());
}
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& old_pyr,
const basalt::ManagedImagePyr<uint16_t>& pyr,
const Eigen::AffineCompact2f& old_transform,
const size_t pyramid_level,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
transform.linear().setIdentity();
for (ssize_t level = config.optical_flow_levels;
level >= static_cast<ssize_t>(pyramid_level); level--) {
const Scalar scale = 1 << level;
Eigen::AffineCompact2f transform_tmp = transform;
transform_tmp.translation() /= scale;
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
// Perform tracking on current level
patch_valid = trackPointAtLevel(pyr.lvl(level), p, transform_tmp);
if (level == static_cast<ssize_t>(pyramid_level) + 1 && !patch_valid) {
return false;
}
transform_tmp.translation() *= scale;
if (patch_valid) {
transform = transform_tmp;
}
}
transform.linear() = old_transform.linear() * transform.linear();
return patch_valid;
}
inline bool trackPointAtLevel(const Image<const u_int16_t>& img_2,
const PatchT& dp,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
for (int iteration = 0;
patch_valid && iteration < config.optical_flow_max_iterations;
iteration++) {
typename PatchT::VectorP res;
typename PatchT::Matrix2P transformed_pat =
transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation();
bool valid = dp.residual(img_2, transformed_pat, res);
if (valid) {
Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
if (!img_2.InBounds(transform.translation(), filter_margin))
patch_valid = false;
} else {
patch_valid = false;
}
}
return patch_valid;
}
void addPoints() {
KeypointsData kd;
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses_main,
new_poses_stereo;
std::map<KeypointId, size_t> new_pyramid_levels_main,
new_pyramid_levels_stereo;
for (ssize_t level = 0;
level < static_cast<ssize_t>(config.optical_flow_levels) - 1;
level++) {
Eigen::aligned_vector<Eigen::Vector2d> pts;
for (const auto& kv : transforms->observations.at(0)) {
const ssize_t point_level =
transforms->pyramid_levels.at(0).at(kv.first);
// do not create points were already points at similar levels are
if (point_level <= level + 1 && point_level >= level - 1) {
// if (point_level == level) {
const Scalar scale = 1 << point_level;
pts.emplace_back(
(kv.second.translation() / scale).template cast<double>());
}
}
detectKeypoints(pyramid->at(0).lvl(level), kd,
config.optical_flow_detection_grid_size, 1, pts);
const Scalar scale = 1 << level;
for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::AffineCompact2f transform;
transform.setIdentity();
transform.translation() =
kd.corners[i].cast<Scalar>() * scale; // TODO cast float?
transforms->observations.at(0)[last_keypoint_id] = transform;
transforms->pyramid_levels.at(0)[last_keypoint_id] = level;
new_poses_main[last_keypoint_id] = transform;
new_pyramid_levels_main[last_keypoint_id] = level;
last_keypoint_id++;
}
trackPoints(pyramid->at(0), pyramid->at(1), new_poses_main,
new_pyramid_levels_main, new_poses_stereo,
new_pyramid_levels_stereo);
for (const auto& kv : new_poses_stereo) {
transforms->observations.at(1).emplace(kv);
transforms->pyramid_levels.at(1)[kv.first] =
new_pyramid_levels_stereo.at(kv.first);
}
}
}
void filterPoints() {
std::set<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid;
Eigen::aligned_vector<Eigen::Vector2f> proj0, proj1;
for (const auto& kv : transforms->observations.at(1)) {
auto it = transforms->observations.at(0).find(kv.first);
if (it != transforms->observations.at(0).end()) {
proj0.emplace_back(it->second.translation());
proj1.emplace_back(kv.second.translation());
kpid.emplace_back(kv.first);
}
}
Eigen::aligned_vector<Eigen::Vector4f> p3d_main, p3d_stereo;
std::vector<bool> p3d_main_success, p3d_stereo_success;
calib.intrinsics[0].unproject(proj0, p3d_main, p3d_main_success);
calib.intrinsics[1].unproject(proj1, p3d_stereo, p3d_stereo_success);
for (size_t i = 0; i < p3d_main_success.size(); i++) {
if (p3d_main_success[i] && p3d_stereo_success[i]) {
const double epipolar_error =
std::abs(p3d_main[i].transpose() * E * p3d_stereo[i]);
const Scalar scale = 1 << transforms->pyramid_levels.at(0).at(kpid[i]);
if (epipolar_error > config.optical_flow_epipolar_error * scale) {
lm_to_remove.emplace(kpid[i]);
}
} else {
lm_to_remove.emplace(kpid[i]);
}
}
for (int id : lm_to_remove) {
transforms->observations.at(1).erase(id);
}
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
int64_t t_ns;
size_t frame_counter;
KeypointId last_keypoint_id;
VioConfig config;
basalt::Calibration<Scalar> calib;
OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<u_int16_t>>> old_pyramid,
pyramid;
// map from stereo pair -> essential matrix
Matrix4 E;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

4
include/basalt/optical_flow/optical_flow.h
View File

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

8
include/basalt/optimization/accumulator.h
View File

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

146
include/basalt/utils/ba_utils.h Normal file
View File

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

64
include/basalt/utils/cast_utils.hpp Normal file
View File

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

92
include/basalt/utils/imu_types.h
View File

@ -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,
const Eigen::Vector3d& vel_w_i,
const Eigen::Vector3d& bias_gyro,
const Eigen::Vector3d& bias_accel, bool linearized)
PoseVelBiasStateWithLin(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i,
const Vec3& bias_gyro, const Vec3& 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,
bool linearized = false)
PoseStateWithLin(int64_t t_ns, const SE3& T_w_i, 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 {
return pose_linearized.T_w_i;
}
inline const SE3& getPoseLin() const { 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;

8
include/basalt/utils/system_utils.h
View File

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

141
include/basalt/utils/time_utils.hpp Normal file
View File

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

19
include/basalt/utils/vio_config.h
View File

@ -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;
int vio_filter_iteration;
// double vio_outlier_threshold;
// 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

373
include/basalt/vi_estimator/ba_base.h
View File

@ -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 {
std::vector<std::pair<TimeCamId, TimeCamId>> order;
using Scalar = Scalar_;
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;
Eigen::aligned_vector<Sophus::Matrix6d> d_rel_d_t;
};
using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
struct FrameRelLinData {
Sophus::Matrix6d Hpp;
Sophus::Vector6d bp;
using SE3 = Sophus::SE3<Scalar>;
std::vector<int> lm_id;
Eigen::aligned_vector<Eigen::Matrix<double, 6, 3>> Hpl;
FrameRelLinData() {
Hpp.setZero();
bp.setZero();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct RelLinData : public RelLinDataBase {
RelLinData(size_t num_keypoints, size_t num_rel_poses) {
Hll.reserve(num_keypoints);
bl.reserve(num_keypoints);
lm_to_obs.reserve(num_keypoints);
Hpppl.reserve(num_rel_poses);
order.reserve(num_rel_poses);
d_rel_d_h.reserve(num_rel_poses);
d_rel_d_t.reserve(num_rel_poses);
error = 0;
}
void invert_keypoint_hessians() {
for (auto& kv : Hll) {
Eigen::Matrix3d Hll_inv;
Hll_inv.setIdentity();
kv.second.ldlt().solveInPlace(Hll_inv);
kv.second = Hll_inv;
}
}
Eigen::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>>>*
void computeError(Scalar& error,
std::map<int, std::vector<std::pair<TimeCamId, Scalar>>>*
outliers = nullptr,
double outlier_threshold = 0) const;
Scalar outlier_threshold = 0) const;
void linearizeHelper(
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;
void filterOutliers(Scalar outlier_threshold, int min_num_obs);
static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H,
Eigen::VectorXd& b);
void optimize_single_frame_pose(
PoseStateWithLin<Scalar>& state_t,
const std::vector<std::vector<int>>& connected_obs) const;
void filterOutliers(double outlier_threshold, int min_num_obs);
template <class CamT>
static bool linearizePoint(
const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
const Eigen::Matrix4d& T_t_h, const CamT& cam, Eigen::Vector2d& res,
Eigen::Matrix<double, 2, POSE_SIZE>* d_res_d_xi = nullptr,
Eigen::Matrix<double, 2, 3>* d_res_d_p = nullptr,
Eigen::Vector4d* proj = nullptr) {
// Todo implement without jacobians
Eigen::Matrix<double, 4, 2> Jup;
Eigen::Vector4d p_h_3d;
p_h_3d = StereographicParam<double>::unproject(kpt_pos.dir, &Jup);
p_h_3d[3] = kpt_pos.id;
Eigen::Vector4d p_t_3d = T_t_h * p_h_3d;
Eigen::Matrix<double, 4, POSE_SIZE> d_point_d_xi;
d_point_d_xi.topLeftCorner<3, 3>() =
Eigen::Matrix3d::Identity() * kpt_pos.id;
d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(p_t_3d.head<3>());
d_point_d_xi.row(3).setZero();
Eigen::Matrix<double, 2, 4> Jp;
bool valid = cam.project(p_t_3d, res, &Jp);
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,
template <class Scalar2>
void get_current_points(
Eigen::aligned_vector<Eigen::Matrix<Scalar2, 3, 1>>& points,
std::vector<int>& ids) const;
// Modifies abs_H and abs_b as a side effect.
static void marginalizeHelper(Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
const std::set<int>& idx_to_keep,
const std::set<int>& idx_to_marg,
Eigen::MatrixXd& marg_H,
Eigen::VectorXd& marg_b);
void computeDelta(const AbsOrderMap& marg_order, VecX& delta) const;
void computeDelta(const AbsOrderMap& marg_order,
Eigen::VectorXd& delta) const;
void linearizeMargPrior(const MargLinData<Scalar>& mld,
const AbsOrderMap& aom, MatX& abs_H, VecX& abs_b,
Scalar& marg_prior_error) const;
void linearizeMargPrior(const AbsOrderMap& marg_order,
const Eigen::MatrixXd& marg_H,
const Eigen::VectorXd& marg_b, const AbsOrderMap& aom,
Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
double& marg_prior_error) const;
void computeMargPriorError(const MargLinData<Scalar>& mld,
Scalar& marg_prior_error) const;
void computeMargPriorError(const AbsOrderMap& marg_order,
const Eigen::MatrixXd& marg_H,
const Eigen::VectorXd& marg_b,
double& marg_prior_error) const;
Scalar computeMargPriorModelCostChange(const MargLinData<Scalar>& mld,
const VecX& marg_scaling,
const VecX& marg_pose_inc) const;
static Eigen::VectorXd checkNullspace(
const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
const AbsOrderMap& marg_order,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& frame_poses);
// TODO: Old version for squared H and b. Remove when no longer needed.
Scalar computeModelCostChange(const MatX& H, const VecX& b,
const VecX& inc) const;
template <class Scalar2>
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;
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
// suffix "2" to avoid name clash with class-wide typedefs
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);
Vec4 worldPoint = mySVD.matrixV().col(3);
Eigen::JacobiSVD<Eigen::Matrix<Scalar_2, 4, 4>> mySVD(A,
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, PoseStateWithLin<double>> frame_poses;
Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>> frame_states;
Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>> frame_poses;
// Point management
LandmarkDatabase lmdb;
LandmarkDatabase<Scalar> lmdb;
double obs_std_dev;
double huber_thresh;
Scalar obs_std_dev;
Scalar huber_thresh;
basalt::Calibration<double> calib;
basalt::Calibration<Scalar> calib;
};
} // namespace basalt

227
include/basalt/vi_estimator/keypoint_vio.h
View File

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

201
include/basalt/vi_estimator/keypoint_vo.h
View File

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

95
include/basalt/vi_estimator/landmark_database.h
View File

@ -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 {
TimeCamId kf_id;
Eigen::Vector2d dir;
double id;
template <class Scalar_>
struct Keypoint {
using Scalar = Scalar_;
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_id = id;
backup_direction = direction;
backup_inv_dist = inv_dist;
}
inline void restore() {
dir = backup_dir;
id = backup_id;
direction = backup_direction;
inv_dist = backup_inv_dist;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
Eigen::Vector2d backup_dir;
double backup_id;
};
struct KeypointObservation {
int kpt_id;
Eigen::Vector2d pos;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vec2 backup_direction;
Scalar backup_inv_dist;
};
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<
TimeCamId, Eigen::aligned_map<
TimeCamId, Eigen::aligned_vector<KeypointObservation>>>&
const std::unordered_map<TimeCamId,
std::map<TimeCamId, std::set<KeypointId>>>&
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;
Eigen::aligned_map<
TimeCamId,
Eigen::aligned_map<TimeCamId, Eigen::aligned_vector<KeypointObservation>>>
obs;
using MapIter =
typename Eigen::aligned_unordered_map<KeypointId,
Keypoint<Scalar>>::iterator;
MapIter removeLandmarkHelper(MapIter it);
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;
Eigen::aligned_unordered_map<int, int> kpts_num_obs;
std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
observations;
static constexpr int min_num_obs = 2;
};
} // namespace basalt

68
include/basalt/vi_estimator/marg_helper.h Normal file
View File

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

28
include/basalt/vi_estimator/nfr_mapper.h
View File

@ -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) {
for (RelLinData& rld : range) {
rld.invert_keypoint_hessians();
void operator()(const tbb::blocked_range<RelLinDataConstIter>& range) {
for (const RelLinData& rld : range) {
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);

452
include/basalt/vi_estimator/sc_ba_base.h Normal file
View File

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

124
include/basalt/vi_estimator/sqrt_ba_base.h Normal file
View File

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

259
include/basalt/vi_estimator/sqrt_keypoint_vio.h Normal file
View File

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

249
include/basalt/vi_estimator/sqrt_keypoint_vo.h Normal file
View File

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

29
include/basalt/vi_estimator/vio_estimator.h
View File

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

9
python/basalt/__init__.py Normal file
View File

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

614
python/basalt/experiments.py Normal file
View File

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

143
python/basalt/generate_tables.py Normal file
View File

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

109
python/basalt/latex/containers.py Normal file
View File

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

393
python/basalt/latex/plot.py Normal file
View File

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

163
python/basalt/latex/results_table.py Normal file
View File

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

88
python/basalt/latex/summarize_sequences_table.py Normal file
View File

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

93
python/basalt/latex/templates.py Normal file
View File

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

61
python/basalt/latex/util.py Normal file
View File

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

106
python/basalt/log.py Normal file
View File

@ -0,0 +1,106 @@
#
# BSD 3-Clause License
#
# This file is part of the Basalt project.
# https://gitlab.com/VladyslavUsenko/basalt.git
#
# Copyright (c) 2019-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

171
python/basalt/metric.py Normal file
View File

@ -0,0 +1,171 @@
#
# BSD 3-Clause License
#
# This file is part of the Basalt project.
# https://gitlab.com/VladyslavUsenko/basalt.git
#
# Copyright (c) 2019-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]

209
python/basalt/nullspace.py Normal file
View File

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

117
python/basalt/run.py Normal file
View File

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

72
python/basalt/util.py Normal file
View File

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

9
scripts/basalt_capture_mocap.py
View File

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

9
scripts/basalt_convert_kitti_calib.py
View File

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

9
scripts/basalt_response_calib.py
View File

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

9
scripts/basalt_verify_dataset.py
View File

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

135
scripts/batch/generate-batch-configs.py Executable file
View File

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

26
scripts/batch/generate-tables.py Executable file
View File

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

139
scripts/batch/list-jobs.sh Executable file
View File

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

19
scripts/batch/plot.py Executable file
View File

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

100
scripts/batch/query-config.py Executable file
View File

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

25
scripts/batch/rerun-failed-in.sh Executable file
View File

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

35
scripts/batch/rerun-one-in.sh Executable file
View File

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

60
scripts/batch/run-all-in.sh Executable file
View File

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

83
scripts/batch/run-one.sh Executable file
View File

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

9
scripts/clang-format-all.sh
View File

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

9
scripts/compare_calib.py
View File

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

9
scripts/eval_full/gen_results.py
View File

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

9
scripts/eval_full/gen_results_kitti.py
View File

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

9
scripts/eval_full/gen_results_tumvi.py
View File

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

9
scripts/eval_full/run_evaluations.sh
View File

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

9
scripts/eval_full/run_evaluations_kitti.sh
View File

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

9
scripts/eval_full/run_evaluations_tumvi.sh
View File

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

9
scripts/install_deps.sh
View File

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

9
scripts/install_mac_os_deps.sh
View File

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

9
scripts/install_ubuntu_deps.sh
View File

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

7
scripts/templates/license-py-sh.tmpl Normal file
View File

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

33
scripts/update-license-headers.sh Executable file
View File

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

9
scripts/update_submodules.sh
View File

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

27
scripts/yapf-all.sh Executable file
View File

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

63
src/linearization/landmark_block.cpp Normal file
View File

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

711
src/linearization/linearization_abs_qr.cpp Normal file
View File

@ -0,0 +1,711 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 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

432
src/linearization/linearization_abs_sc.cpp Normal file
View File

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

119
src/linearization/linearization_base.cpp Normal file
View File

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

431
src/linearization/linearization_rel_sc.cpp Normal file
View File

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

6
src/mapper_sim.cpp
View File

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

7
src/mapper_sim_naive.cpp
View File

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

31
src/optical_flow/optical_flow.cpp
View File

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

2
src/rs_t265_vio.cpp
View File

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

11
src/utils/keypoints.cpp
View File

@ -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 y = y_start; y < y_stop; y += 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) {
if (cells((y - y_start) / PATCH_SIZE, (x - x_start) / PATCH_SIZE) > 0)
continue;

96
src/utils/system_utils.cpp Normal file
View File

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

212
src/utils/time_utils.cpp Normal file
View File

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

59
src/utils/vio_config.cpp
View File

@ -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_filter_iteration = 4;
// vio_outlier_threshold = 3.0;
// 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_filter_iteration));
// ar(CEREAL_NVP(config.vio_outlier_threshold));
// 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));

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