diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 6cd4961fb..99b5daeba 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -119,37 +119,37 @@ jobs: - name: SLAM test (monocular) with EuRoC MAV dataset (MH_01) run: | cd build - ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_01 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db MH_01_mono.msg + ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_01 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db-out MH_01_mono.msg mv frame_trajectory.txt ../artifact/frame_trajectory_MH_01_mono_slam.txt mv track_times.txt ../artifact/track_times_MH_01_mono_slam.txt - name: SLAM test (stereo) with EuRoC MAV dataset (MH_01) run: | cd build - ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_01 -c ../example/euroc/EuRoC_stereo.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db MH_01_stereo.msg + ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_01 -c ../example/euroc/EuRoC_stereo.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db-out MH_01_stereo.msg mv frame_trajectory.txt ../artifact/frame_trajectory_MH_01_stereo_slam.txt mv track_times.txt ../artifact/track_times_MH_01_stereo_slam.txt - name: Localization test (monocular) with EuRoC MAV dataset (MH_01) run: | cd build - ./run_euroc_slam --load-map --disable-mapping -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_01 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db MH_01_mono.msg + ./run_euroc_slam --disable-mapping -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_01 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db-in MH_01_mono.msg mv frame_trajectory.txt ../artifact/frame_trajectory_MH_01_mono_localization.txt mv track_times.txt ../artifact/track_times_MH_01_mono_localization.txt - name: SLAM test (monocular) with EuRoC MAV dataset (MH_04) run: | cd build - ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_04 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db MH_04_mono.msg + ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_04 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db-out MH_04_mono.msg mv frame_trajectory.txt ../artifact/frame_trajectory_MH_04_mono_slam.txt mv track_times.txt ../artifact/track_times_MH_04_mono_slam.txt - name: SLAM test (stereo) with EuRoC MAV dataset (MH_04) run: | cd build - ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_04 -c ../example/euroc/EuRoC_stereo.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db MH_04_stereo.msg + ./run_euroc_slam -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_04 -c ../example/euroc/EuRoC_stereo.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db-out MH_04_stereo.msg mv frame_trajectory.txt ../artifact/frame_trajectory_MH_04_stereo_slam.txt mv track_times.txt ../artifact/track_times_MH_04_stereo_slam.txt - name: Localization test (monocular) with EuRoC MAV dataset (MH_04) run: | cd build - ./run_euroc_slam --load-map --disable-mapping -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_04 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db MH_04_mono.msg + ./run_euroc_slam --disable-mapping -v /datasets/orb_vocab/orb_vocab.fbow -d /datasets/EuRoC/MH_04 -c ../example/euroc/EuRoC_mono.yaml --frame-skip 2 --no-sleep --log-level=debug --eval-log-dir . --map-db-in MH_04_mono.msg mv frame_trajectory.txt ../artifact/frame_trajectory_MH_04_mono_localization.txt mv track_times.txt ../artifact/track_times_MH_04_mono_localization.txt - name: download openvslam_test_dataset @@ -163,19 +163,19 @@ jobs: - name: SLAM test with equirectangular dataset (1) run: | cd build - ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ../openvslam_test_dataset/1/equirectangular/video.mp4 -c ../equirectangular.yaml --no-sleep --log-level=debug --eval-log-dir . --map-db equirectangular_map.msg -t 0.0 + ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ../openvslam_test_dataset/1/equirectangular/video.mp4 -c ../equirectangular.yaml --no-sleep --log-level=debug --eval-log-dir . --map-db-out equirectangular_map.msg -t 0.0 mv frame_trajectory.txt ../artifact/frame_trajectory_equirectangular_1.txt mv track_times.txt ../artifact/track_times_equirectangular_1.txt - name: SLAM test with equirectangular dataset (2) run: | cd build - ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ../openvslam_test_dataset/2/equirectangular/video.mp4 -c ../equirectangular.yaml --no-sleep --log-level=debug --eval-log-dir . --map-db equirectangular_map.msg -t 0.0 + ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ../openvslam_test_dataset/2/equirectangular/video.mp4 -c ../equirectangular.yaml --no-sleep --log-level=debug --eval-log-dir . --map-db-out equirectangular_map.msg -t 0.0 mv frame_trajectory.txt ../artifact/frame_trajectory_equirectangular_2.txt mv track_times.txt ../artifact/track_times_equirectangular_2.txt - name: SLAM test with equirectangular dataset (3) run: | cd build - ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ../openvslam_test_dataset/3/equirectangular/video.mp4 -c ../equirectangular.yaml --no-sleep --log-level=debug --eval-log-dir . --map-db equirectangular_map.msg -t 0.0 + ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ../openvslam_test_dataset/3/equirectangular/video.mp4 -c ../equirectangular.yaml --no-sleep --log-level=debug --eval-log-dir . --map-db-out equirectangular_map.msg -t 0.0 mv frame_trajectory.txt ../artifact/frame_trajectory_equirectangular_3.txt mv track_times.txt ../artifact/track_times_equirectangular_3.txt - name: Evaluation @@ -256,11 +256,11 @@ jobs: - name: mapping test with the tutorial run: | cd build - ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ./video_for_ci_1/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --log-level=debug --eval-log-dir . --map-db map.msg + ./run_video_slam -v /datasets/orb_vocab/orb_vocab.fbow -m ./video_for_ci_1/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --log-level=debug --eval-log-dir . --map-db-out map.msg - name: localization test with the tutorial run: | cd build - ./run_video_slam --load-map --disable-mapping -v /datasets/orb_vocab/orb_vocab.fbow -m ./video_for_ci_1/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --log-level=debug --eval-log-dir . --map-db map.msg + ./run_video_slam --disable-mapping -v /datasets/orb_vocab/orb_vocab.fbow -m ./video_for_ci_1/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --log-level=debug --eval-log-dir . --map-db-in map.msg rosdep_foxy: runs-on: ubuntu-latest diff --git a/.gitmodules b/.gitmodules index 7d4b6256d..4d7b985ab 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "3rd/tinycolormap"] path = 3rd/tinycolormap url = https://github.com/yuki-koyama/tinycolormap.git +[submodule "3rd/filesystem"] + path = 3rd/filesystem + url = https://github.com/gulrak/filesystem diff --git a/3rd/filesystem b/3rd/filesystem new file mode 160000 index 000000000..cd6805e94 --- /dev/null +++ b/3rd/filesystem @@ -0,0 +1 @@ +Subproject commit cd6805e94dd5d6346be1b75a54cdc27787319dd2 diff --git a/README.md b/README.md index 86d28c34f..6f28be619 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,7 @@ Feedbacks, feature requests, and contribution are welcome! The following files are derived from third-party libraries. +- `./3rd/filesystem` : [gulrak/filesystem](https://github.com/gulrak/filesystem) (MIT license) - `./3rd/json` : [nlohmann/json \[v3.6.1\]](https://github.com/nlohmann/json) (MIT license) - `./3rd/popl` : [badaix/popl \[v1.2.0\]](https://github.com/badaix/popl) (MIT license) - `./3rd/spdlog` : [gabime/spdlog \[v1.3.1\]](https://github.com/gabime/spdlog) (MIT license) diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 455c96532..ff76af08f 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -1,5 +1,8 @@ # ----- Find dependencies ----- +# filesystem +set(filesystem_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/3rd/filesystem/include) + # popl set(popl_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/3rd/popl/include) @@ -96,9 +99,10 @@ foreach(EXECUTABLE_TARGET IN LISTS EXECUTABLE_TARGETS) opencv_imgcodecs opencv_videoio) - # include popl and spdlog headers + # include 3rd party library headers target_include_directories(${EXECUTABLE_TARGET} PRIVATE $ + $ $) endforeach() diff --git a/example/run_camera_slam.cc b/example/run_camera_slam.cc index b8da2c21b..c08a91452 100644 --- a/example/run_camera_slam.cc +++ b/example/run_camera_slam.cc @@ -22,6 +22,9 @@ #include #include +#include +namespace fs = ghc::filesystem; + #ifdef USE_STACK_TRACE_LOGGER #include #endif @@ -234,9 +237,9 @@ int main(int argc, char* argv[]) { auto config_file_path = op.add>("c", "config", "config file path"); auto mask_img_path = op.add>("", "mask", "mask image path", ""); auto scale = op.add>("s", "scale", "scaling ratio of images", 1.0); - auto map_db_path = op.add>("p", "map-db", "store a map database at this path after slam", ""); + auto map_db_path_in = op.add>("i", "map-db-in", "load a map from this path", ""); + auto map_db_path_out = op.add>("o", "map-db-out", "store a map database at this path after slam", ""); auto log_level = op.add>("", "log-level", "log level", "info"); - auto load_map = op.add("", "load-map", "load a map database"); auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); try { op.parse(argc, argv); @@ -289,10 +292,19 @@ int main(int argc, char* argv[]) { // build a slam system stella_vslam::system slam(cfg, vocab_file_path->value()); bool need_initialize = true; - if (load_map->is_set()) { + if (map_db_path_in->is_set()) { need_initialize = false; - // load the prebuilt map - slam.load_map_database(map_db_path->value()); + const auto path = fs::path(map_db_path_in->value()); + if (path.extension() == ".yaml") { + YAML::Node node = YAML::LoadFile(path); + for (const auto& map_path : node["maps"].as>()) { + slam.load_map_database(path.parent_path() / map_path); + } + } + else { + // load the prebuilt map + slam.load_map_database(path); + } } slam.startup(need_initialize); if (disable_mapping->is_set()) { @@ -306,7 +318,7 @@ int main(int argc, char* argv[]) { cam_num->value(), mask_img_path->value(), scale->value(), - map_db_path->value()); + map_db_path_out->value()); } else if (slam.get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Stereo) { stereo_tracking(slam, @@ -314,7 +326,7 @@ int main(int argc, char* argv[]) { cam_num->value(), mask_img_path->value(), scale->value(), - map_db_path->value()); + map_db_path_out->value()); } else { throw std::runtime_error("Invalid setup type: " + slam.get_camera()->get_setup_type_string()); diff --git a/example/run_euroc_slam.cc b/example/run_euroc_slam.cc index 15fa64874..ec8a2936d 100644 --- a/example/run_euroc_slam.cc +++ b/example/run_euroc_slam.cc @@ -26,6 +26,9 @@ #include #include +#include +namespace fs = ghc::filesystem; + #ifdef USE_STACK_TRACE_LOGGER #include #endif @@ -320,8 +323,8 @@ int main(int argc, char* argv[]) { auto auto_term = op.add("", "auto-term", "automatically terminate the viewer"); auto log_level = op.add>("", "log-level", "log level", "info"); auto eval_log_dir = op.add>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", ""); - auto map_db_path = op.add>("p", "map-db", "store a map database at this path after slam", ""); - auto load_map = op.add("", "load-map", "load a map database"); + auto map_db_path_in = op.add>("i", "map-db-in", "load a map from this path", ""); + auto map_db_path_out = op.add>("o", "map-db-out", "store a map database at this path after slam", ""); auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); auto equal_hist = op.add("", "equal-hist", "apply histogram equalization"); @@ -376,10 +379,19 @@ int main(int argc, char* argv[]) { // build a slam system stella_vslam::system slam(cfg, vocab_file_path->value()); bool need_initialize = true; - if (load_map->is_set()) { + if (map_db_path_in->is_set()) { need_initialize = false; - // load the prebuilt map - slam.load_map_database(map_db_path->value()); + const auto path = fs::path(map_db_path_in->value()); + if (path.extension() == ".yaml") { + YAML::Node node = YAML::LoadFile(path); + for (const auto& map_path : node["maps"].as>()) { + slam.load_map_database(path.parent_path() / map_path); + } + } + else { + // load the prebuilt map + slam.load_map_database(path); + } } slam.startup(need_initialize); if (disable_mapping->is_set()) { @@ -396,7 +408,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value(), + map_db_path_out->value(), equal_hist->is_set()); } else if (slam.get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Stereo) { @@ -408,7 +420,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value(), + map_db_path_out->value(), equal_hist->is_set()); } else { diff --git a/example/run_image_slam.cc b/example/run_image_slam.cc index af7dcefe5..3eb3214e7 100644 --- a/example/run_image_slam.cc +++ b/example/run_image_slam.cc @@ -22,6 +22,9 @@ #include #include +#include +namespace fs = ghc::filesystem; + #ifdef USE_STACK_TRACE_LOGGER #include #endif @@ -174,8 +177,8 @@ int main(int argc, char* argv[]) { auto auto_term = op.add("", "auto-term", "automatically terminate the viewer"); auto log_level = op.add>("", "log-level", "log level", "info"); auto eval_log_dir = op.add>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", ""); - auto map_db_path = op.add>("p", "map-db", "store a map database at this path after slam", ""); - auto load_map = op.add("", "load-map", "load a map database"); + auto map_db_path_in = op.add>("i", "map-db-in", "load a map from this path", ""); + auto map_db_path_out = op.add>("o", "map-db-out", "store a map database at this path after slam", ""); auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); auto start_timestamp = op.add>("t", "start-timestamp", "timestamp of the start of the video capture"); try { @@ -244,10 +247,19 @@ int main(int argc, char* argv[]) { // build a slam system stella_vslam::system slam(cfg, vocab_file_path->value()); bool need_initialize = true; - if (load_map->is_set()) { + if (map_db_path_in->is_set()) { need_initialize = false; - // load the prebuilt map - slam.load_map_database(map_db_path->value()); + const auto path = fs::path(map_db_path_in->value()); + if (path.extension() == ".yaml") { + YAML::Node node = YAML::LoadFile(path); + for (const auto& map_path : node["maps"].as>()) { + slam.load_map_database(path.parent_path() / map_path); + } + } + else { + // load the prebuilt map + slam.load_map_database(path); + } } slam.startup(need_initialize); if (disable_mapping->is_set()) { @@ -265,7 +277,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value(), + map_db_path_out->value(), timestamp); } else { diff --git a/example/run_kitti_slam.cc b/example/run_kitti_slam.cc index 74bf87e9f..d8da3d558 100644 --- a/example/run_kitti_slam.cc +++ b/example/run_kitti_slam.cc @@ -24,6 +24,9 @@ #include #include +#include +namespace fs = ghc::filesystem; + #ifdef USE_STACK_TRACE_LOGGER #include #endif @@ -290,8 +293,8 @@ int main(int argc, char* argv[]) { auto auto_term = op.add("", "auto-term", "automatically terminate the viewer"); auto log_level = op.add>("", "log-level", "log level", "info"); auto eval_log_dir = op.add>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", ""); - auto map_db_path = op.add>("p", "map-db", "store a map database at this path after slam", ""); - auto load_map = op.add("", "load-map", "load a map database"); + auto map_db_path_in = op.add>("i", "map-db-in", "load a map from this path", ""); + auto map_db_path_out = op.add>("o", "map-db-out", "store a map database at this path after slam", ""); auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); try { op.parse(argc, argv); @@ -343,10 +346,19 @@ int main(int argc, char* argv[]) { // build a slam system stella_vslam::system slam(cfg, vocab_file_path->value()); bool need_initialize = true; - if (load_map->is_set()) { + if (map_db_path_in->is_set()) { need_initialize = false; - // load the prebuilt map - slam.load_map_database(map_db_path->value()); + const auto path = fs::path(map_db_path_in->value()); + if (path.extension() == ".yaml") { + YAML::Node node = YAML::LoadFile(path); + for (const auto& map_path : node["maps"].as>()) { + slam.load_map_database(path.parent_path() / map_path); + } + } + else { + // load the prebuilt map + slam.load_map_database(path); + } } slam.startup(need_initialize); if (disable_mapping->is_set()) { @@ -363,7 +375,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value()); + map_db_path_out->value()); } else if (slam.get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Stereo) { stereo_tracking(slam, @@ -374,7 +386,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value()); + map_db_path_out->value()); } else { throw std::runtime_error("Invalid setup type: " + slam.get_camera()->get_setup_type_string()); diff --git a/example/run_tum_rgbd_slam.cc b/example/run_tum_rgbd_slam.cc index 1e6d59ce6..980ad818c 100644 --- a/example/run_tum_rgbd_slam.cc +++ b/example/run_tum_rgbd_slam.cc @@ -24,6 +24,9 @@ #include #include +#include +namespace fs = ghc::filesystem; + #ifdef USE_STACK_TRACE_LOGGER #include #endif @@ -290,8 +293,8 @@ int main(int argc, char* argv[]) { auto auto_term = op.add("", "auto-term", "automatically terminate the viewer"); auto log_level = op.add>("", "log-level", "log level", "info"); auto eval_log_dir = op.add>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", ""); - auto map_db_path = op.add>("p", "map-db", "store a map database at this path after slam", ""); - auto load_map = op.add("", "load-map", "load a map database"); + auto map_db_path_in = op.add>("i", "map-db-in", "load a map from this path", ""); + auto map_db_path_out = op.add>("o", "map-db-out", "store a map database at this path after slam", ""); auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); try { @@ -344,10 +347,19 @@ int main(int argc, char* argv[]) { // build a slam system stella_vslam::system slam(cfg, vocab_file_path->value()); bool need_initialize = true; - if (load_map->is_set()) { + if (map_db_path_in->is_set()) { need_initialize = false; - // load the prebuilt map - slam.load_map_database(map_db_path->value()); + const auto path = fs::path(map_db_path_in->value()); + if (path.extension() == ".yaml") { + YAML::Node node = YAML::LoadFile(path); + for (const auto& map_path : node["maps"].as>()) { + slam.load_map_database(path.parent_path() / map_path); + } + } + else { + // load the prebuilt map + slam.load_map_database(path); + } } slam.startup(need_initialize); if (disable_mapping->is_set()) { @@ -364,7 +376,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value()); + map_db_path_out->value()); } else if (slam.get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::RGBD) { rgbd_tracking(slam, @@ -375,7 +387,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value()); + map_db_path_out->value()); } else { throw std::runtime_error("Invalid setup type: " + slam.get_camera()->get_setup_type_string()); diff --git a/example/run_video_slam.cc b/example/run_video_slam.cc index b31f2c36e..d712f84be 100644 --- a/example/run_video_slam.cc +++ b/example/run_video_slam.cc @@ -21,6 +21,9 @@ #include #include +#include +namespace fs = ghc::filesystem; + #ifdef USE_STACK_TRACE_LOGGER #include #endif @@ -184,8 +187,8 @@ int main(int argc, char* argv[]) { auto auto_term = op.add("", "auto-term", "automatically terminate the viewer"); auto log_level = op.add>("", "log-level", "log level", "info"); auto eval_log_dir = op.add>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", ""); - auto map_db_path = op.add>("p", "map-db", "store a map database at this path after slam", ""); - auto load_map = op.add("", "load-map", "load a map database"); + auto map_db_path_in = op.add>("i", "map-db-in", "load a map from this path", ""); + auto map_db_path_out = op.add>("o", "map-db-out", "store a map database at this path after slam", ""); auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); auto start_timestamp = op.add>("t", "start-timestamp", "timestamp of the start of the video capture"); try { @@ -254,10 +257,19 @@ int main(int argc, char* argv[]) { // build a slam system stella_vslam::system slam(cfg, vocab_file_path->value()); bool need_initialize = true; - if (load_map->is_set()) { + if (map_db_path_in->is_set()) { need_initialize = false; - // load the prebuilt map - slam.load_map_database(map_db_path->value()); + const auto path = fs::path(map_db_path_in->value()); + if (path.extension() == ".yaml") { + YAML::Node node = YAML::LoadFile(path); + for (const auto& map_path : node["maps"].as>()) { + slam.load_map_database(path.parent_path() / map_path); + } + } + else { + // load the prebuilt map + slam.load_map_database(path); + } } slam.startup(need_initialize); if (disable_mapping->is_set()) { @@ -276,7 +288,7 @@ int main(int argc, char* argv[]) { wait_loop_ba->is_set(), auto_term->is_set(), eval_log_dir->value(), - map_db_path->value(), + map_db_path_out->value(), timestamp); } else { diff --git a/scripts/ubuntu/README.md b/scripts/ubuntu/README.md index c53dad0ad..ada10198d 100644 --- a/scripts/ubuntu/README.md +++ b/scripts/ubuntu/README.md @@ -56,7 +56,7 @@ If you finish all the above procedure, you will see the terminal inside a docker #### run tracking and mapping ```shell -./run_video_slam -v /vocab/orb_vocab.fbow -m /dataset/aist_living_lab_1/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --map-db map.msg +./run_video_slam -v /vocab/orb_vocab.fbow -m /dataset/aist_living_lab_1/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --map-db-out map.msg ``` After you see the window is stopped, press "Terminate" button in the left pane. @@ -66,7 +66,7 @@ You can use it for the below tracking demo. #### run localization ```shell -./run_video_slam --load-map --disable-mapping -v /vocab/orb_vocab.fbow -m /dataset/aist_living_lab_2/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --map-db map.msg +./run_video_slam --disable-mapping -v /vocab/orb_vocab.fbow -m /dataset/aist_living_lab_2/video.mp4 -c ../example/aist/equirectangular.yaml --frame-skip 3 --no-sleep --map-db-in map.msg ``` ## Run stella_vslam on your own video diff --git a/src/stella_vslam/data/map_database.cc b/src/stella_vslam/data/map_database.cc index 4f382b541..91dcbd651 100644 --- a/src/stella_vslam/data/map_database.cc +++ b/src/stella_vslam/data/map_database.cc @@ -243,17 +243,6 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p const nlohmann::json& json_keyfrms, const nlohmann::json& json_landmarks) { std::lock_guard lock(mtx_map_access_); - // Step 1. delete all the data in map database - for (auto& lm : landmarks_) { - lm.second = nullptr; - } - - for (auto& keyfrm : keyframes_) { - keyfrm.second = nullptr; - } - - landmarks_.clear(); - keyframes_.clear(); // When loading the map, leave last_inserted_keyfrm_ as nullptr. last_inserted_keyfrm_ = nullptr; local_landmarks_.clear(); @@ -262,7 +251,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // If the object does not exist at this step, the corresponding pointer is set as nullptr. spdlog::info("decoding {} keyframes to load", json_keyfrms.size()); for (const auto& json_id_keyfrm : json_keyfrms.items()) { - const auto id = std::stoi(json_id_keyfrm.key()); + const auto id = std::stoi(json_id_keyfrm.key()) + next_keyframe_id_; assert(0 <= id); const auto json_keyfrm = json_id_keyfrm.value(); @@ -273,7 +262,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // If the object does not exist at this step, the corresponding pointer is set as nullptr. spdlog::info("decoding {} landmarks to load", json_landmarks.size()); for (const auto& json_id_landmark : json_landmarks.items()) { - const auto id = std::stoi(json_id_landmark.key()); + const auto id = std::stoi(json_id_landmark.key()) + next_landmark_id_; assert(0 <= id); const auto json_landmark = json_id_landmark.value(); @@ -283,7 +272,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // Step 4. Register graph information spdlog::info("registering essential graph"); for (const auto& json_id_keyfrm : json_keyfrms.items()) { - const auto id = std::stoi(json_id_keyfrm.key()); + const auto id = std::stoi(json_id_keyfrm.key()) + next_keyframe_id_; assert(0 <= id); const auto json_keyfrm = json_id_keyfrm.value(); @@ -293,7 +282,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // Step 5. Register association between keyframs and 3D points spdlog::info("registering keyframe-landmark association"); for (const auto& json_id_keyfrm : json_keyfrms.items()) { - const auto id = std::stoi(json_id_keyfrm.key()); + const auto id = std::stoi(json_id_keyfrm.key()) + next_keyframe_id_; assert(0 <= id); const auto json_keyfrm = json_id_keyfrm.value(); @@ -303,7 +292,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // find root node std::unordered_set already_found_root_ids; for (const auto& json_id_keyfrm : json_keyfrms.items()) { - const auto id = std::stoi(json_id_keyfrm.key()); + const auto id = std::stoi(json_id_keyfrm.key()) + next_keyframe_id_; auto keyfrm = keyframes_.at(id); auto root = keyfrm->graph_node_->get_spanning_root(); if (already_found_root_ids.count(root->id_)) { @@ -317,7 +306,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // Step 6. Update graph spdlog::info("updating covisibility graph"); for (const auto& json_id_keyfrm : json_keyfrms.items()) { - const auto id = std::stoi(json_id_keyfrm.key()); + const auto id = std::stoi(json_id_keyfrm.key()) + next_keyframe_id_; assert(0 <= id); assert(keyframes_.count(id)); @@ -330,7 +319,7 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p // Step 7. Update geometry spdlog::info("updating landmark geometry"); for (const auto& json_id_landmark : json_landmarks.items()) { - const auto id = std::stoi(json_id_landmark.key()); + const auto id = std::stoi(json_id_landmark.key()) + next_landmark_id_; assert(0 <= id); assert(landmarks_.count(id)); @@ -400,9 +389,9 @@ void map_database::register_keyframe(camera_database* cam_db, orb_params_databas } void map_database::register_landmark(const unsigned int id, const nlohmann::json& json_landmark) { - const auto first_keyfrm_id = json_landmark.at("1st_keyfrm").get(); + const auto first_keyfrm_id = json_landmark.at("1st_keyfrm").get() + next_keyframe_id_; const auto pos_w = Vec3_t(json_landmark.at("pos_w").get>().data()); - const auto ref_keyfrm_id = json_landmark.at("ref_keyfrm").get(); + const auto ref_keyfrm_id = json_landmark.at("ref_keyfrm").get() + next_keyframe_id_; const auto ref_keyfrm = keyframes_.at(ref_keyfrm_id); const auto num_visible = json_landmark.at("n_vis").get(); const auto num_found = json_landmark.at("n_fnd").get(); @@ -421,15 +410,15 @@ void map_database::register_graph(const unsigned int id, const nlohmann::json& j const auto loop_edge_ids = json_keyfrm.at("loop_edges").get>(); assert(keyframes_.count(id)); - assert(spanning_parent_id == -1 || keyframes_.count(spanning_parent_id)); - keyframes_.at(id)->graph_node_->set_spanning_parent((spanning_parent_id == -1) ? nullptr : keyframes_.at(spanning_parent_id)); + assert(spanning_parent_id == -1 || keyframes_.count(spanning_parent_id + next_keyframe_id_)); + keyframes_.at(id)->graph_node_->set_spanning_parent((spanning_parent_id == -1) ? nullptr : keyframes_.at(spanning_parent_id + next_keyframe_id_)); for (const auto spanning_child_id : spanning_children_ids) { assert(keyframes_.count(spanning_child_id)); - keyframes_.at(id)->graph_node_->add_spanning_child(keyframes_.at(spanning_child_id)); + keyframes_.at(id)->graph_node_->add_spanning_child(keyframes_.at(spanning_child_id + next_keyframe_id_)); } for (const auto loop_edge_id : loop_edge_ids) { assert(keyframes_.count(loop_edge_id)); - keyframes_.at(id)->graph_node_->add_loop_edge(keyframes_.at(loop_edge_id)); + keyframes_.at(id)->graph_node_->add_loop_edge(keyframes_.at(loop_edge_id + next_keyframe_id_)); } } @@ -442,10 +431,11 @@ void map_database::register_association(const unsigned int keyfrm_id, const nloh assert(keyframes_.count(keyfrm_id)); auto keyfrm = keyframes_.at(keyfrm_id); for (unsigned int idx = 0; idx < num_keypts; ++idx) { - const auto lm_id = landmark_ids.at(idx); + auto lm_id = landmark_ids.at(idx); if (lm_id < 0) { continue; } + lm_id += next_landmark_id_; if (!landmarks_.count(lm_id)) { spdlog::warn("landmark {}: not found in the database", lm_id); continue; @@ -494,17 +484,6 @@ bool map_database::from_db(sqlite3* db, bow_vocabulary* bow_vocab) { std::lock_guard lock(mtx_map_access_); - // Step 1. delete all the data in map database - for (auto& lm : landmarks_) { - lm.second = nullptr; - } - - for (auto& keyfrm : keyframes_) { - keyfrm.second = nullptr; - } - - landmarks_.clear(); - keyframes_.clear(); // When loading the map, leave last_inserted_keyfrm_ as nullptr. last_inserted_keyfrm_ = nullptr; local_landmarks_.clear(); @@ -636,11 +615,11 @@ bool map_database::load_keyframes_from_db(sqlite3* db, // Compute BoW data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec); auto keyfrm = data::keyframe::make_keyframe( - id, timestamp, pose_cw, camera, orb_params, + id + next_keyframe_id_, timestamp, pose_cw, camera, orb_params, frm_obs, bow_vec, bow_feat_vec); // Append to map database - assert(!keyframes_.count(id)); + assert(!keyframes_.count(id + next_keyframe_id_)); keyframes_[keyfrm->id_] = keyfrm; } @@ -674,10 +653,10 @@ bool map_database::load_landmarks_from_db(sqlite3* db) { auto num_found = sqlite3_column_int64(stmt, column_id); column_id++; - auto ref_keyfrm = keyframes_.at(ref_keyfrm_id); + auto ref_keyfrm = keyframes_.at(ref_keyfrm_id + next_keyframe_id_); auto lm = std::make_shared( - id, first_keyfrm_id, pos_w, ref_keyfrm, + id + next_landmark_id_, first_keyfrm_id + next_keyframe_id_, pos_w, ref_keyfrm, num_visible, num_found); assert(!landmarks_.count(id)); landmarks_[lm->id_] = lm; @@ -719,24 +698,25 @@ bool map_database::load_associations_from_db(sqlite3* db) { std::memcpy(loop_edge_ids.data(), p, sqlite3_column_bytes(stmt, column_id)); column_id++; - assert(spanning_parent_id == -1 || keyframes_.count(spanning_parent_id)); - keyframes_.at(keyfrm_id)->graph_node_->set_spanning_parent((spanning_parent_id == -1LL) ? nullptr : keyframes_.at(spanning_parent_id)); + assert(spanning_parent_id == -1 || keyframes_.count(spanning_parent_id + next_keyframe_id_)); + keyframes_.at(keyfrm_id)->graph_node_->set_spanning_parent((spanning_parent_id == -1LL) ? nullptr : keyframes_.at(spanning_parent_id + next_keyframe_id_)); for (const auto spanning_child_id : spanning_children_ids) { - assert(keyframes_.count(spanning_child_id)); - keyframes_.at(keyfrm_id)->graph_node_->add_spanning_child(keyframes_.at(spanning_child_id)); + assert(keyframes_.count(spanning_child_id + next_keyframe_id_)); + keyframes_.at(keyfrm_id)->graph_node_->add_spanning_child(keyframes_.at(spanning_child_id + next_keyframe_id_)); } for (const auto loop_edge_id : loop_edge_ids) { - assert(keyframes_.count(loop_edge_id)); - keyframes_.at(keyfrm_id)->graph_node_->add_loop_edge(keyframes_.at(loop_edge_id)); + assert(keyframes_.count(loop_edge_id + next_keyframe_id_)); + keyframes_.at(keyfrm_id)->graph_node_->add_loop_edge(keyframes_.at(loop_edge_id + next_keyframe_id_)); } assert(keyframes_.count(keyfrm_id)); auto keyfrm = keyframes_.at(keyfrm_id); for (unsigned int idx = 0; idx < lm_ids.size(); ++idx) { - const auto lm_id = lm_ids.at(idx); + auto lm_id = lm_ids.at(idx); if (lm_id < 0) { continue; } + lm_id += next_landmark_id_; if (!landmarks_.count(lm_id)) { spdlog::warn("landmark {}: not found in the database", lm_id); continue; diff --git a/src/stella_vslam/io/map_database_io_msgpack.cc b/src/stella_vslam/io/map_database_io_msgpack.cc index f08df894b..77b20572c 100644 --- a/src/stella_vslam/io/map_database_io_msgpack.cc +++ b/src/stella_vslam/io/map_database_io_msgpack.cc @@ -81,9 +81,6 @@ void map_database_io_msgpack::load(const std::string& path, const auto json = nlohmann::json::from_msgpack(msgpack); - // load next ID - map_db->next_keyframe_id_ = json.at("keyframe_next_id").get(); - map_db->next_landmark_id_ = json.at("landmark_next_id").get(); // load database const auto json_cameras = json.at("cameras"); cam_db->from_json(json_cameras); @@ -92,6 +89,11 @@ void map_database_io_msgpack::load(const std::string& path, const auto json_keyfrms = json.at("keyframes"); const auto json_landmarks = json.at("landmarks"); map_db->from_json(cam_db, orb_params_db, bow_vocab, json_keyfrms, json_landmarks); + // load next ID + map_db->next_keyframe_id_ += json.at("keyframe_next_id").get(); + map_db->next_landmark_id_ += json.at("landmark_next_id").get(); + + // update bow database const auto keyfrms = map_db->get_all_keyframes(); for (const auto& keyfrm : keyfrms) { bow_db->add_keyframe(keyfrm); diff --git a/src/stella_vslam/io/map_database_io_sqlite3.cc b/src/stella_vslam/io/map_database_io_sqlite3.cc index a97f06f91..280d7b10a 100644 --- a/src/stella_vslam/io/map_database_io_sqlite3.cc +++ b/src/stella_vslam/io/map_database_io_sqlite3.cc @@ -63,9 +63,11 @@ void map_database_io_sqlite3::load(const std::string& path, } // load from database - bool ok = load_stats(db, map_db); - ok = ok && cam_db->from_db(db); + bool ok = cam_db->from_db(db); ok = ok && map_db->from_db(db, cam_db, orb_params_db, bow_vocab); + ok = ok && load_stats(db, map_db); + + // update bow database if (ok) { const auto keyfrms = map_db->get_all_keyframes(); for (const auto& keyfrm : keyfrms) { diff --git a/src/stella_vslam/system.cc b/src/stella_vslam/system.cc index f5e3dda3e..c157335ea 100644 --- a/src/stella_vslam/system.cc +++ b/src/stella_vslam/system.cc @@ -202,14 +202,14 @@ void system::save_keyframe_trajectory(const std::string& path, const std::string void system::load_map_database(const std::string& path) const { pause_other_threads(); - map_db_->clear(); - bow_db_->clear(); + spdlog::debug("load_map_database: {}", path); map_database_io_->load(path, cam_db_, orb_params_db_, map_db_, bow_db_, bow_vocab_); resume_other_threads(); } void system::save_map_database(const std::string& path) const { pause_other_threads(); + spdlog::debug("save_map_database: {}", path); map_database_io_->save(path, cam_db_, orb_params_db_, map_db_); resume_other_threads(); } diff --git a/src/stella_vslam/system.h b/src/stella_vslam/system.h index 69b1760e5..d44a33d48 100644 --- a/src/stella_vslam/system.h +++ b/src/stella_vslam/system.h @@ -79,10 +79,10 @@ class system { //! Save the keyframe trajectory in the specified format void save_keyframe_trajectory(const std::string& path, const std::string& format) const; - //! Load the map database from the MessagePack file + //! Load the map database from file void load_map_database(const std::string& path) const; - //! Save the map database to the MessagePack file + //! Save the map database to file void save_map_database(const std::string& path) const; //! Get the map publisher