├── .clang-format ├── .gitignore ├── .gitmodules ├── 3rdparty └── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindGflags.cmake └── FindGlog.cmake ├── include ├── depth_generator_from_map.h ├── dsl_common.h ├── full_system │ ├── coarse_distance_map.h │ ├── coarse_tracker.h │ ├── distance_initializer.h │ ├── ef_struct.h │ ├── full_system.h │ ├── hessian_blocks.h │ ├── immature_point.h │ ├── marginalization_residual.h │ ├── optimization_call_back.h │ ├── pixel_selector.h │ ├── residual.h │ └── residual_projection.h ├── map_render │ ├── core │ │ ├── global_model.h │ │ ├── gpu_texture.h │ │ └── index_map.h │ ├── csv.h │ ├── data_loader.h │ ├── gui.h │ ├── map_render.h │ ├── pangolin_utils.h │ ├── pcl_utils.h │ ├── shaders │ │ ├── color.glsl │ │ ├── combo_splat_cata.frag │ │ ├── data.frag │ │ ├── data.geom │ │ ├── data.vert │ │ ├── depth_splat_cata.frag │ │ ├── draw_feedback.frag │ │ ├── draw_feedback.vert │ │ ├── draw_surface.frag │ │ ├── draw_surface.geom │ │ ├── draw_surface.vert │ │ ├── empty.vert │ │ ├── feedback_buffer.h │ │ ├── fov.glsl │ │ ├── geometry.glsl │ │ ├── index_global_map.frag │ │ ├── index_global_map.vert │ │ ├── index_global_map_cata.geom │ │ ├── index_map.frag │ │ ├── index_map_cata.vert │ │ ├── init_unstable.vert │ │ ├── intensity_splat_cata.frag │ │ ├── parse.h │ │ ├── quad.geom │ │ ├── shaders.h │ │ ├── splat_cata.vert │ │ ├── surfels.glsl │ │ ├── uniform.h │ │ ├── update_with_pose.vert │ │ ├── vertex.h │ │ └── visualise_textures.frag │ └── yaml_loader.h ├── optimization │ ├── ab_local_parameterization.h │ ├── homo_cost_functor.h │ ├── marginalization_factor.h │ ├── matrix_accumulators.h │ ├── parameter_map.h │ ├── photo_cost_functor.h │ ├── se3_local_parameterization.h │ └── se3_prior_cost_functor.h ├── relocalization │ ├── boost_archiver.h │ ├── converter.h │ ├── desc_map.h │ ├── desc_map_viewer.h │ ├── feature_extractor.h │ ├── feature_matcher.h │ ├── pnp_solver.h │ ├── reloc_optimization │ │ ├── optimizer.h │ │ ├── reprojection_functor.h │ │ └── surfel_reprojection_functor.h │ ├── relocalization_config.h │ ├── relocalization_struct.h │ ├── sp_extractor.h │ ├── sp_matcher.h │ ├── struct │ │ ├── clique.h │ │ ├── frame.h │ │ ├── key_frame_database.h │ │ ├── map_point.h │ │ ├── motion_estimator.h │ │ ├── net_vlad_utils.h │ │ ├── virtual_frame.h │ │ └── vlad_database.h │ ├── visualization │ │ └── drawer.h │ └── vocabulary_binary.h ├── tool │ ├── cv_helper.h │ ├── dataset_converter.h │ ├── euroc_reader.h │ ├── habitat_sim_reader.h │ └── new_tsukuba_reader.h └── util │ ├── frame_shell.h │ ├── global_calib.h │ ├── global_funcs.h │ ├── graph_utils.h │ ├── image_and_exposure.h │ ├── index_thread_reduce.h │ ├── minimal_image.h │ ├── num_type.h │ ├── settings.h │ ├── timing.h │ └── util_common.h ├── scripts ├── extract_images_timestamp.py ├── pre_process_hkust.py └── tum2csv.py ├── src ├── CMakeLists.txt ├── colorizer.cc ├── depth_generator.cc ├── depth_generator_from_map.cc ├── desc_map_builder.cc ├── desc_map_clique.cc ├── desc_map_keyframe.cc ├── dsl_full_main.cc ├── dsl_main.cc ├── full_system │ ├── CMakeLists.txt │ ├── coarse_distance_map.cc │ ├── coarse_tracker.cc │ ├── distance_initializer.cc │ ├── ef_struct.cc │ ├── full_system.cc │ ├── full_system_activate_points.cc │ ├── full_system_marginalize.cc │ ├── full_system_optimize.cc │ ├── full_system_tracing.cc │ ├── full_system_tracking.cc │ ├── hessian_blocks.cc │ ├── immature_point.cc │ ├── optimization_call_back.cc │ ├── pixel_selector.cc │ └── residual.cc ├── map_render │ ├── CMakeLists.txt │ ├── core │ │ ├── global_model.cc │ │ ├── gpu_texture.cc │ │ └── index_map.cc │ ├── data_loader.cc │ ├── gui.cc │ ├── pangolin_utils.cc │ ├── pcl_utils.cc │ ├── shaders │ │ ├── feedback_buffer.cc │ │ ├── parse.cc │ │ └── vertex.cc │ └── yaml_loader.cc ├── optimization │ ├── CMakeLists.txt │ ├── homo_cost_functor.cc │ ├── marginalization_factor.cc │ ├── parameter_map.cc │ ├── photo_cost_functor.cc │ └── se3_prior_cost_functor.cc ├── relocalization │ ├── CMakeLists.txt │ ├── converter.cc │ ├── desc_map.cc │ ├── desc_map_optimizer.cc │ ├── desc_map_unused.cc │ ├── desc_map_updater.cc │ ├── desc_map_viewer.cc │ ├── feature_extractor.cc │ ├── feature_matcher.cc │ ├── pnp_solver.cc │ ├── reloc_optimization │ │ ├── optimizer.cc │ │ ├── reprojection_functor.cc │ │ └── surfel_reprojection_functor.cc │ ├── relocalization_config.cc │ ├── sp_extractor.cc │ ├── sp_matcher.cc │ ├── struct │ │ ├── clique.cc │ │ ├── frame.cc │ │ ├── key_frame_database.cc │ │ ├── map_point.cc │ │ ├── motion_estimator.cc │ │ ├── net_vlad_utils.cc │ │ ├── virtual_frame.cc │ │ └── vlad_database.cc │ ├── visualization │ │ └── drawer.cc │ └── vocabulary_binary.cc ├── tool │ ├── CMakeLists.txt │ ├── cv_helper.cc │ ├── dataset_converter.cc │ ├── euroc_reader.cc │ ├── habitat_sim_reader.cc │ └── new_tsukuba_reader.cc └── util │ ├── CMakeLists.txt │ ├── global_calib.cc │ ├── graph_utils.cc │ ├── settings.cc │ └── timing.cc ├── support_files ├── .gitignore └── superpoint.pt └── test ├── CMakeLists.txt ├── ceres_test.cc ├── coarse_tracker_test.cc ├── dbow2_test.cc ├── decomposition_test.cc ├── desc_map_vis.cc ├── euroc_test.cc ├── front_end_test.cc ├── img_test.cc ├── mt_test.cc ├── netvlad_test.cc ├── optimization_test.cc ├── ref_test.cc ├── render_test.cc └── sp_test.cc /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlinesLeft: true 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: All 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | BeforeCatch: false 33 | BeforeElse: false 34 | IndentBraces: false 35 | BreakBeforeBinaryOperators: None 36 | BreakBeforeBraces: Attach 37 | BreakBeforeTernaryOperators: true 38 | BreakConstructorInitializersBeforeComma: false 39 | ColumnLimit: 80 40 | CommentPragmas: '^ IWYU pragma:' 41 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 42 | ConstructorInitializerIndentWidth: 4 43 | ContinuationIndentWidth: 4 44 | Cpp11BracedListStyle: true 45 | DerivePointerAlignment: true 46 | DisableFormat: false 47 | ExperimentalAutoDetectBinPacking: false 48 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 49 | IncludeCategories: 50 | - Regex: '^<.*\.h>' 51 | Priority: 1 52 | - Regex: '^<.*' 53 | Priority: 2 54 | - Regex: '.*' 55 | Priority: 3 56 | IndentCaseLabels: true 57 | IndentWidth: 2 58 | IndentWrappedFunctionNames: false 59 | KeepEmptyLinesAtTheStartOfBlocks: false 60 | MacroBlockBegin: '' 61 | MacroBlockEnd: '' 62 | MaxEmptyLinesToKeep: 1 63 | NamespaceIndentation: None 64 | ObjCBlockIndentWidth: 2 65 | ObjCSpaceAfterProperty: false 66 | ObjCSpaceBeforeProtocolList: false 67 | PenaltyBreakBeforeFirstCallParameter: 1 68 | PenaltyBreakComment: 300 69 | PenaltyBreakFirstLessLess: 120 70 | PenaltyBreakString: 1000 71 | PenaltyExcessCharacter: 1000000 72 | PenaltyReturnTypeOnItsOwnLine: 200 73 | PointerAlignment: Left 74 | ReflowComments: true 75 | SortIncludes: true 76 | SpaceAfterCStyleCast: false 77 | SpaceBeforeAssignmentOperators: true 78 | SpaceBeforeParens: ControlStatements 79 | SpaceInEmptyParentheses: false 80 | SpacesBeforeTrailingComments: 2 81 | SpacesInAngles: false 82 | SpacesInContainerLiterals: true 83 | SpacesInCStyleCastParentheses: false 84 | SpacesInParentheses: false 85 | SpacesInSquareBrackets: false 86 | Standard: Auto 87 | TabWidth: 8 88 | UseTab: Never 89 | ... 90 | 91 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .idea 3 | build* 4 | cmake-* 5 | .DS_Store 6 | 7 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "3rdparty/googletest"] 2 | path = 3rdparty/googletest 3 | url = https://github.com/google/googletest.git 4 | [submodule "3rdparty/Sophus"] 5 | path = 3rdparty/Sophus 6 | url = https://github.com/strasdat/Sophus.git 7 | [submodule "3rdparty/nanoflann"] 8 | path = 3rdparty/nanoflann 9 | url = https://github.com/jlblancoc/nanoflann.git 10 | [submodule "3rdparty/DBoW2"] 11 | path = 3rdparty/DBoW2 12 | url = https://github.com/dorian3d/DBoW2.git 13 | [submodule "3rdparty/fmt"] 14 | path = 3rdparty/fmt 15 | url = https://github.com/fmtlib/fmt.git 16 | -------------------------------------------------------------------------------- /3rdparty/.gitignore: -------------------------------------------------------------------------------- 1 | libtorch/* 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | 3 | project(dsl) 4 | 5 | set(CMAKE_CXX_STANDARD 17) 6 | 7 | add_definitions("-DENABLE_SSE") 8 | #set(CMAKE_CXX_FLAGS 9 | #"${SSE_FLAGS} -g -march=native" 10 | ## "${SSE_FLAGS} -O3 -g -std=c++0x -fno-omit-frame-pointer" 11 | #) 12 | 13 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") 14 | 15 | find_package(Gflags REQUIRED) 16 | find_package(Glog REQUIRED) 17 | include_directories(${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS}) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | include_directories(${EIGEN3_INCLUDE_DIR}) 21 | 22 | find_package(Boost REQUIRED COMPONENTS system filesystem thread) 23 | include_directories(${Boost_INCLUDE_DIRS}) 24 | 25 | # list(APPEND CMAKE_PREFIX_PATH "/mnt/HDD/Softwares/opencv-3.4.11/install/share/OpenCV") 26 | find_package(OpenCV REQUIRED) 27 | find_package(Ceres REQUIRED) 28 | find_package(PCL REQUIRED) 29 | include_directories(${OpenCV_INCLUDE_DIRS}) 30 | include_directories(${CERES_INCLUDE_DIRS}) 31 | include_directories(${PCL_INCLUDE_DIRS}) 32 | 33 | find_package(Pangolin 0.4 REQUIRED) 34 | include_directories(${Pangolin_INCLUDE_DIRS}) 35 | 36 | find_package(Boost REQUIRED COMPONENTS system) 37 | include_directories(${Boost_INCLUDE_DIRS}) 38 | 39 | find_package(CUDA REQUIRED) 40 | include_directories(${CUDA_INCLUDE_DIRS}) 41 | 42 | # list(APPEND CMAKE_PREFIX_PATH "${PROJECT_SOURCE_DIR}/3rdparty/libtorch/") 43 | # find_package(Torch REQUIRED) 44 | # include_directories(${TORCH_INCLUDE_DIRS}) 45 | # message(${TORCH_INCLUDE_DIRS}) 46 | 47 | set(3rdparty_INCLUDE_DIRS 48 | 3rdparty/googletest/googletest/include 49 | 3rdparty/googletest/googlemock/include 50 | 3rdparty/Sophus 51 | 3rdparty/nanoflann/include 52 | 3rdparty/DBoW2/include 53 | 3rdparty/fmt/include 54 | ) 55 | 56 | include_directories(${3rdparty_INCLUDE_DIRS}) 57 | include_directories(include) 58 | include_directories(${PROJECT_SOURCE_DIR}) 59 | 60 | add_subdirectory(3rdparty/googletest) 61 | set(Google_LIBRARIES 62 | ${GFLAGS_LIBRARIES} 63 | ${GLOG_LIBRARIES} 64 | gmock 65 | gtest gtest_main 66 | ) 67 | 68 | add_subdirectory(3rdparty/DBoW2) 69 | add_subdirectory(3rdparty/fmt) 70 | 71 | set(ThreeParty_LIBS 72 | DBoW2 73 | fmt 74 | ) 75 | 76 | set(Ext_LIBRARIES 77 | ${ThreeParty_LIBS} 78 | ${CERES_LIBRARIES} 79 | ${GFLAGS_LIBRARIES} 80 | ${OpenCV_LIBRARIES} 81 | ${PCL_LIBRARIES} 82 | ${Pangolin_LIBRARIES} 83 | ${CUDA_LIBRARIES} 84 | # ${TORCH_LIBRARIES} 85 | ) 86 | 87 | enable_testing() 88 | 89 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;") 90 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false") 91 | 92 | set(SHADER_DIR "${PROJECT_SOURCE_DIR}/include/map_render/shaders" CACHE PATH "Where the shaders live") 93 | set(CMAKE_CXX_FLAGS ${ADDITIONAL_CMAKE_CXX_FLAGS} "-DSHADER_DIR=${SHADER_DIR}") 94 | 95 | set(CUDA_ARCH_BIN "30 35 50 52 61" CACHE STRING "Specify 'real' GPU arch to build binaries for, BIN(PTX) format is supported. Example: 1.3 2.1(1.3) or 13 21(13)") 96 | set(CUDA_ARCH_PTX "" CACHE STRING "Specify 'virtual' PTX arch to build PTX intermediate code for. Example: 1.0 1.2 or 10 12") 97 | 98 | add_subdirectory(src) 99 | # add_subdirectory(test) 100 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DSL 2 | Project page: https://sites.google.com/view/dsl-ram-lab/ 3 | 4 | ## Monocular Direct Sparse Localization in a Prior 3D Surfel Map 5 | #### Authors: Haoyang Ye, Huaiyang Huang, and Ming Liu from [RAM-LAB](https://ram-lab.com/). 6 | 7 | ## Paper and Video 8 | Related publications: 9 | ``` 10 | @inproceedings{ye2020monocular, 11 | title={Monocular direct sparse localization in a prior 3d surfel map}, 12 | author={Ye, Haoyang and Huang, Huaiyang and Liu, Ming}, 13 | booktitle={2020 IEEE International Conference on Robotics and Automation (ICRA)}, 14 | pages={8892--8898}, 15 | year={2020}, 16 | organization={IEEE} 17 | } 18 | @inproceedings{ye20213d, 19 | title={3D Surfel Map-Aided Visual Relocalization with Learned Descriptors}, 20 | author={Ye, Haoyang and Huang, Huaiyang and Hutter, Marco and Sandy, Timothy and Liu, Ming}, 21 | booktitle={2021 International Conference on Robotics and Automation (ICRA)}, 22 | pages={5574-5581}, 23 | year={2021}, 24 | organization={IEEE} 25 | } 26 | ``` 27 | Video: 28 | https://www.youtube.com/watch?v=LTihCBGcURo 29 | 30 | ## Dependency 31 | 1. [Pangolin](https://github.com/stevenlovegrove/Pangolin). 32 | 2. [CUDA](https://developer.nvidia.com/cuda-downloads?target_os=Linux&target_arch=x86_64&target_distro=Ubuntu&target_version=1804&target_type=deblocal). 33 | 3. [Ceres-solver](http://ceres-solver.org/installation.html#linux). 34 | 4. [PCL](http://www.pointclouds.org/downloads/), the default version accompanying by ROS. 35 | 5. [OpenCV](https://docs.opencv.org/master/d7/d9f/tutorial_linux_install.html), the default version accompanying by ROS. 36 | 37 | ## Build 38 | 0. `git submodule update --init --recursive` 39 | 1. `mkdir build && cd build` 40 | 2. `cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo` 41 | 3. `make -j8` 42 | 43 | ## Example 44 | The sample config file can be downloaded from [this link](https://hkustconnect-my.sharepoint.com/:u:/g/personal/hyeab_connect_ust_hk/EXDlfYJgCclHuxvTiEcfpbIBJmNAwdE1soquwUeHGuaItw?e=63oiEP). 45 | 46 | To run the example: 47 | ```shell 48 | [path_to_build]/src/dsl_main --path "[path_to_dataset]/left_pinhole" 49 | ``` 50 | 51 | ## Preparing Your Own Data 52 | 1. Collect LiDAR and camera data. 53 | 2. Build LiDAR map and obtain LiDAR poses (the poses are not necessary). 54 | 3. Pre-process LiDAR map to make the `[path_to_dataset]/*.pcd` map file contains `normal_x, normal_y, normal_z` fields (downsample & normal estimation). 55 | 4. Extract and undistort images into `[path_to_dataset]/images`. 56 | 5. Set the first camera pose to `initial_pose` and other camera parameters in `[path_to_dataset]/config.yaml`. 57 | 58 | ## Note 59 | This implementation of DSL takes [Ceres Solver](http://ceres-solver.org/) as backend, which is different from the the implementation of the original paper with DSO-backend. This leads to different performance, i.e., speed and accuracy, compared to the reported results. 60 | 61 | ## Credits 62 | This work is inspired from several open-source projects, such as [DSO](https://github.com/JakobEngel/dso), [DSM](https://github.com/jzubizarreta/dsm), [Elastic-Fusion](https://github.com/mp3guy/ElasticFusion), [SuperPoint](https://github.com/magicleap/SuperPointPretrainedNetwork), [DBoW2](https://github.com/dorian3d/DBoW2), [NetVlad](https://www.di.ens.fr/willow/research/netvlad/), [LIO-mapping](https://github.com/hyye/lio-mapping) and etc. 63 | 64 | ## Licence 65 | The source code is released under [GPL-3.0](https://www.gnu.org/licenses/). 66 | -------------------------------------------------------------------------------- /include/depth_generator_from_map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 5/13/20. 28 | // 29 | 30 | #ifndef DSL_DEPTH_GENERATOR_FROM_MAP_H_ 31 | #define DSL_DEPTH_GENERATOR_FROM_MAP_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | void LoadTumToPoses(std::string pose_path, std::vector> &all_poses) { 39 | all_poses.clear(); 40 | 41 | std::ifstream pose_infile(pose_path); 42 | 43 | std::string line; 44 | while (std::getline(pose_infile, line)) { 45 | std::istringstream iss(line); 46 | Eigen::Matrix vec_in; 47 | int i = 0; 48 | std::string s; 49 | 50 | while (iss >> s) { 51 | vec_in[i] = std::stod(s); 52 | std::cout << vec_in[i] << " "; 53 | ++i; 54 | if (iss.peek() == ' ') iss.ignore(); 55 | } 56 | std::cout << std::endl; 57 | Eigen::Quaterniond q(vec_in.segment<4>(4)); 58 | q.normalize(); 59 | std::cout << q.coeffs() << std::endl; 60 | 61 | Sophus::SE3 pose_in; 62 | pose_in.translation() = vec_in.segment<3>(1); 63 | pose_in.setRotationMatrix(q.toRotationMatrix()); 64 | 65 | all_poses.emplace_back(pose_in); 66 | } 67 | } 68 | 69 | #endif //DSL_DEPTH_GENERATOR_FROM_MAP_H_ 70 | -------------------------------------------------------------------------------- /include/dsl_common.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/5/19. 28 | // 29 | 30 | #ifndef DSL_DSL_COMMON_H_ 31 | #define DSL_DSL_COMMON_H_ 32 | 33 | #include 34 | #include 35 | #include "util/settings.h" 36 | #include "util/num_type.h" 37 | #include "util/global_calib.h" 38 | #include "util/global_funcs.h" 39 | #include "util/index_thread_reduce.h" 40 | #include "util/minimal_image.h" 41 | #include "util/image_and_exposure.h" 42 | #include "util/frame_shell.h" 43 | #include "util/timing.h" 44 | #include "optimization/parameter_map.h" 45 | 46 | // really want to get rid of these 47 | #define SCALE_IDIST 1.0f // scales internal value to idist. 48 | #define SCALE_XI_ROT 1.0f 49 | #define SCALE_XI_TRANS 0.5f 50 | #define SCALE_F 50.0f 51 | #define SCALE_C 50.0f 52 | #define SCALE_W 1.0f 53 | #define SCALE_A 10.0f 54 | #define SCALE_B 1000.0f 55 | 56 | namespace dsl { 57 | 58 | struct ImmaturePoint; 59 | struct FrameHessian; 60 | struct PointHessian; 61 | struct PointFrameResidual; 62 | struct EfFrame; 63 | struct EfPoint; 64 | struct EfResidual; 65 | class EnergyFunction; 66 | 67 | struct CalibHessian; 68 | 69 | enum ResLocation { ACTIVE = 0, LINEARIZED, MARGINALIZED, NONE }; 70 | enum ResState { IN = 0, OOB, OUTLIER }; 71 | // TODO: SCALES 72 | 73 | } 74 | 75 | 76 | 77 | #endif // DSL_DSL_COMMON_H_ 78 | -------------------------------------------------------------------------------- /include/full_system/coarse_distance_map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/8/19. 28 | // 29 | 30 | #ifndef DSL_COARSE_DISTANCE_MAP_H_ 31 | #define DSL_COARSE_DISTANCE_MAP_H_ 32 | 33 | #include "dsl_common.h" 34 | #include "hessian_blocks.h" 35 | 36 | namespace dsl { 37 | 38 | class CoarseDistanceMap { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | CoarseDistanceMap(int _w, int _h); 43 | ~CoarseDistanceMap() {} 44 | 45 | void MakeDistanceMap( 46 | std::vector> &frame_hessians, 47 | FrameHessian &frame); 48 | 49 | // TODO: is it necessary to have a copy here? 50 | void MakeK(CalibHessian &HCalib); 51 | 52 | void AddIntoDistFinal(int u, int v); 53 | 54 | void GrowDistBFS(/*int num_bfs1*/); 55 | 56 | std::vector fwd_warped_dist_final; 57 | std::vector bfs_list1; 58 | std::vector bfs_list2; 59 | 60 | Mat33f K[PYR_LEVELS]; 61 | Mat33f Ki[PYR_LEVELS]; 62 | float fx[PYR_LEVELS]; 63 | float fy[PYR_LEVELS]; 64 | float fxi[PYR_LEVELS]; 65 | float fyi[PYR_LEVELS]; 66 | float cx[PYR_LEVELS]; 67 | float cy[PYR_LEVELS]; 68 | float cxi[PYR_LEVELS]; 69 | float cyi[PYR_LEVELS]; 70 | float xi; 71 | int w[PYR_LEVELS]; 72 | int h[PYR_LEVELS]; 73 | }; 74 | 75 | 76 | } 77 | 78 | #endif // DSL_COARSE_DISTANCE_MAP_H_ 79 | -------------------------------------------------------------------------------- /include/full_system/immature_point.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/6/19. 28 | // 29 | 30 | #ifndef DSL_IMMATURE_POINT_H_ 31 | #define DSL_IMMATURE_POINT_H_ 32 | 33 | #include "dsl_common.h" 34 | #include "util/num_type.h" 35 | 36 | namespace dsl { 37 | 38 | struct ImmaturePointTemporaryResidual { 39 | public: 40 | ResState res_state; 41 | double res_energy; 42 | ResState new_res_state; 43 | double new_res_energy; 44 | FrameHessian* target; 45 | }; 46 | 47 | enum class ImmaturePointStatus { 48 | IPS_GOOD = 0, // traced well and good 49 | IPS_OOB, // OOB: end tracking & marginalize! 50 | IPS_OUTLIER, // energy too high: if happens again: outlier! 51 | IPS_SKIPPED, // traced well and good (but not actually traced). 52 | IPS_BADCONDITION, // not traced because of bad condition. 53 | IPS_UNINITIALIZED 54 | }; // not even traced once. 55 | 56 | class ImmaturePoint { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 59 | 60 | ImmaturePointStatus last_trace_status; 61 | Vec2f last_trace_uv; 62 | float last_trace_pixel_interval; 63 | 64 | // static values 65 | float color[MAX_RES_PER_POINT]; 66 | float weights[MAX_RES_PER_POINT]; 67 | 68 | Mat22f gradH; 69 | Vec2f gradH_ev; 70 | Mat22f gradH_eig; 71 | float energy_th; 72 | float u, v; 73 | // on the unit sphere 74 | float x, y, z; 75 | FrameHessian* host; 76 | int idx_in_immature_points; 77 | 78 | /// non-max suppression 79 | float quality; 80 | 81 | float my_type; 82 | 83 | float idist_min; 84 | float idist_max; 85 | 86 | ImmaturePoint(int _u, int _v, FrameHessian* _host, float type, 87 | CalibHessian& HCalib); 88 | double LinearizeResidual(CalibHessian& HCalib, const float outlier_th_slack, 89 | ImmaturePointTemporaryResidual& tmp_res, float& Hdd, 90 | float& bd, float idist); 91 | ImmaturePointStatus TraceOn(FrameHessian& frame, 92 | const Mat33f& host_to_frame_KRKi, 93 | const Mat33f& host_to_frame_R, 94 | const Vec3f& host_to_frame_t, 95 | const Vec2f& host_to_frame_aff, 96 | CalibHessian& HCalib); 97 | }; 98 | 99 | } // namespace dsl 100 | 101 | #endif // DSL_IMMATURE_POINT_H_ 102 | -------------------------------------------------------------------------------- /include/full_system/marginalization_residual.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 1/7/20. 28 | // 29 | 30 | #ifndef DSL_MARGINALIZATION_RESIDUAL_H_ 31 | #define DSL_MARGINALIZATION_RESIDUAL_H_ 32 | 33 | #include "optimization/marginalization_factor.h" 34 | #include "optimization/parameter_map.h" 35 | 36 | namespace dsl { 37 | 38 | struct MarginalizationResidual { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | MarginalizationResidual(MarginalizationInfo* marginalization_info) { 42 | cost_function = 43 | std::make_unique(marginalization_info); 44 | } 45 | std::unique_ptr cost_function; 46 | std::unique_ptr res_blk_spec = 47 | std::make_unique(); 48 | }; 49 | 50 | } // namespace dsl 51 | 52 | #endif // DSL_MARGINALIZATION_RESIDUAL_H_ 53 | -------------------------------------------------------------------------------- /include/full_system/optimization_call_back.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 2/10/20. 28 | // 29 | 30 | #ifndef DSL_OPTIMIZATION_CALL_BACK_H_ 31 | #define DSL_OPTIMIZATION_CALL_BACK_H_ 32 | 33 | #include 34 | #include "full_system/full_system.h" 35 | 36 | namespace dsl { 37 | 38 | class OptimizationCallBack : public ceres::IterationCallback { 39 | public: 40 | OptimizationCallBack(FullSystem &full_system) : full_system_(full_system) { 41 | for (auto &&fh : full_system_.frame_hessians) { 42 | // poses_.push_back(fh->parameter_pose.GetEstimate()); 43 | // abs_.push_back(fh->parameter_ab.GetEstimate()); 44 | } 45 | } 46 | 47 | void Backup(); 48 | bool CanBreak(); 49 | 50 | virtual ceres::CallbackReturnType operator()( 51 | const ceres::IterationSummary &summary); 52 | 53 | private: 54 | FullSystem &full_system_; 55 | // std::vector poses_; 56 | // std::vector abs_; 57 | }; 58 | 59 | } // namespace dsl 60 | 61 | #endif // DSL_OPTIMIZATION_CALL_BACK_H_ 62 | -------------------------------------------------------------------------------- /include/full_system/pixel_selector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/11/19. 28 | // 29 | 30 | #ifndef DSL_PIXEL_SELECTOR_H_ 31 | #define DSL_PIXEL_SELECTOR_H_ 32 | 33 | #include "hessian_blocks.h" 34 | #include "util/global_calib.h" 35 | #include "util/global_funcs.h" 36 | #include "util/num_type.h" 37 | 38 | namespace dsl { 39 | 40 | enum PixelSelectorStatus { PIXSEL_VOID = 0, PIXSEL_1, PIXSEL_2, PIXSEL_3 }; 41 | 42 | class PixelSelector { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | /** 46 | * 47 | * @param fh input frame_hessian 48 | * @param map_out the PixelSelectorStatus, selected in three levels as n[0], 49 | * n[1], n[2], (n2,n3,n4) 50 | * @param density 51 | * @param recursions_left 52 | * @param th_factor 53 | * @return 54 | */ 55 | int MakeMaps(const FrameHessian& fh, std::vector& map_out, 56 | float density, int recursions_left = 1, float th_factor = 1); 57 | 58 | PixelSelector(int w, int h); 59 | ~PixelSelector() {} 60 | int current_potential; 61 | 62 | void MakeHists(const FrameHessian& fh); 63 | 64 | private: 65 | Eigen::Vector3i Select(const FrameHessian& fh, std::vector& map_out, 66 | int pot, float th_factor = 1); 67 | 68 | std::vector random_pattern; 69 | 70 | std::vector grad_hist; 71 | std::vector ths; 72 | std::vector ths_smoothed; 73 | int ths_step; 74 | const FrameHessian* grad_hist_frame; 75 | }; 76 | 77 | } // namespace dsl 78 | 79 | #endif // DSL_PIXEL_SELECTOR_H_ 80 | -------------------------------------------------------------------------------- /include/full_system/residual.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/13/19. 28 | // 29 | 30 | #ifndef DSL_RESIDUAL_H_ 31 | #define DSL_RESIDUAL_H_ 32 | 33 | #include 34 | #include "dsl_common.h" 35 | #include "optimization/parameter_map.h" 36 | 37 | namespace dsl { 38 | 39 | struct PointFrameResidual { 40 | public: 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | 43 | ResState res_state; 44 | ResState new_res_state; 45 | double res_energy; 46 | double new_res_energy; 47 | double new_res_energy_with_outiler; 48 | 49 | double res_raw; 50 | double new_res_raw; 51 | static int instanceCounter; 52 | 53 | bool is_new; 54 | 55 | void ResetOOB(); 56 | double Linearize(CalibHessian &HCalib); 57 | 58 | void SetState(ResState s) { res_state = s; } 59 | 60 | void ApplyRes(bool copy_jacobians); 61 | 62 | EfResidual *ef_residual; 63 | PointHessian *point; 64 | FrameHessian *host; 65 | FrameHessian *target; 66 | 67 | std::unique_ptr cost_function; 68 | std::unique_ptr res_blk_spec = 69 | std::make_unique(); 70 | 71 | Vec2f projected_to[MAX_RES_PER_POINT]; 72 | Vec3f center_projected_to; 73 | Vec3f center_projected_to_backup; 74 | float ph_idist = -1, ph_idist_backup; 75 | 76 | Vec4f plane_coeff; 77 | bool idist_converged = false; 78 | bool valid_plane = false; 79 | 80 | Eigen::Matrix Jd; 81 | Eigen::Matrix Jd_backup; 82 | 83 | PointFrameResidual(PointHessian *_point, FrameHessian *_host, 84 | FrameHessian *_target, CalibHessian *_HCalib = 0); 85 | 86 | ~PointFrameResidual() { --instanceCounter; } 87 | 88 | void SetResPlane(); 89 | }; 90 | 91 | } // namespace dsl 92 | 93 | #endif // DSL_RESIDUAL_H_ 94 | -------------------------------------------------------------------------------- /include/map_render/core/global_model.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #ifndef DSL_GLOBAL_MODEL_H_ 31 | #define DSL_GLOBAL_MODEL_H_ 32 | 33 | #include 34 | #include "map_render/shaders/shaders.h" 35 | #include "map_render/shaders/uniform.h" 36 | #include "map_render/shaders/feedback_buffer.h" 37 | #include "gpu_texture.h" 38 | 39 | namespace dsl { 40 | 41 | class GlobalModel { 42 | 43 | public: 44 | GlobalModel(); 45 | virtual ~GlobalModel(); 46 | 47 | static const int TEXTURE_DIMENSION; 48 | static const int MAX_VERTICES; 49 | 50 | void Initialization(const GLuint &in_vbo, const GLuint &in_fid); 51 | void Initialization(const pangolin::GlBuffer &vbo); 52 | void RenderPointCloud(pangolin::OpenGlMatrix mvp, 53 | const bool draw_normals = false, 54 | const bool draw_colors = true); 55 | void Fuse(const Eigen::Matrix4f &pose, 56 | GPUTexture *rgb, 57 | GPUTexture *index_map, 58 | GPUTexture *vert_conf_map, 59 | GPUTexture *color_time_map, 60 | GPUTexture *norm_rad_map, 61 | const float depth_cutoff); 62 | const std::pair &Model(); 63 | 64 | unsigned int GetCount(); 65 | 66 | Eigen::Vector4f *DownloadMap(); 67 | 68 | private: 69 | const int buffer_size_; 70 | 71 | //First is the vbo, second is the fid 72 | std::pair *vbos_; 73 | GLuint *vaos_; 74 | int target_, render_; 75 | unsigned int count_; 76 | GLuint count_query_; 77 | std::shared_ptr init_prog_; 78 | std::shared_ptr draw_surfel_prog_; 79 | std::shared_ptr data_prog_; 80 | std::shared_ptr update_prog_; 81 | 82 | GLuint new_unstable_vbo_, new_unstable_fid_; 83 | 84 | pangolin::GlRenderBuffer render_buffer_; 85 | pangolin::GlFramebuffer framebuffer_; 86 | 87 | GPUTexture update_map_verts_confs_; 88 | GPUTexture update_map_colors_time_; 89 | GPUTexture update_map_norms_radii_; 90 | 91 | GLuint uvo_; 92 | int uv_size_; 93 | 94 | }; 95 | 96 | } 97 | 98 | #endif // DSL_GLOBAL_MODEL_H_ 99 | -------------------------------------------------------------------------------- /include/map_render/core/gpu_texture.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #ifndef DSL_GPU_TEXTURE_H_ 31 | #define DSL_GPU_TEXTURE_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace dsl { 39 | 40 | class GPUTexture { 41 | public: 42 | GPUTexture(const int width, const int height, const GLenum internalFormat, 43 | const GLenum format, const GLenum dataType, 44 | const bool draw = false, const bool cuda = false); 45 | 46 | virtual ~GPUTexture(); 47 | 48 | static const std::string RGB, DEPTH_RAW, DEPTH_FILTERED, DEPTH_METRIC, 49 | DEPTH_METRIC_FILTERED, DEPTH_NORM, VERTEX_MAP, REF_RGB, PREDICT_DEPTH; 50 | 51 | pangolin::GlTexture *texture; 52 | 53 | cudaGraphicsResource *cudaRes; 54 | 55 | const bool draw; 56 | 57 | private: 58 | GPUTexture() 59 | : texture(0), 60 | cudaRes(0), 61 | draw(false), 62 | width(0), 63 | height(0), 64 | internalFormat(0), 65 | format(0), 66 | dataType(0) {} 67 | const int width; 68 | const int height; 69 | const GLenum internalFormat; 70 | const GLenum format; 71 | const GLenum dataType; 72 | }; 73 | 74 | } 75 | 76 | #endif // DSL_GPU_TEXTURE_H_ 77 | -------------------------------------------------------------------------------- /include/map_render/map_render.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/30/19. 28 | // 29 | 30 | #ifndef DSL_MAP_RENDER_H_ 31 | #define DSL_MAP_RENDER_H_ 32 | 33 | #include "map_render/core/global_model.h" 34 | #include "map_render/core/index_map.h" 35 | #include "map_render/data_loader.h" 36 | #include "map_render/gui.h" 37 | #include "map_render/yaml_loader.h" 38 | 39 | #endif // DSL_MAP_RENDER_H_ 40 | -------------------------------------------------------------------------------- /include/map_render/pangolin_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/28/19. 28 | // 29 | 30 | #ifndef DSL_PANGOLIN_UTILS_H_ 31 | #define DSL_PANGOLIN_UTILS_H_ 32 | 33 | #include 34 | #include 35 | #include "util/util_common.h" 36 | 37 | namespace dsl { 38 | 39 | class PangolinUtils { 40 | public: 41 | PangolinUtils(){}; 42 | bool SetVertexNormal(pangolin::GlTexture *vertex_tex, 43 | pangolin::GlTexture *normal_tex); 44 | bool GetPoint(float x, float y, pcl::PointXYZINormal &pcl_point); 45 | static void SaveFigures(std::vector textures, 46 | std::vector flags, int current_frame = 0, 47 | std::string saving_path = "/tmp"); 48 | 49 | pangolin::TypedImage vertex_img, normal_img; 50 | int height, width; 51 | 52 | private: 53 | pangolin::GlTexture *vertex_tex_, *normal_tex_; 54 | bool vertex_available_, normal_available_; 55 | }; 56 | 57 | } // namespace dsl 58 | 59 | #endif // DSL_PANGOLIN_UTILS_H_ 60 | -------------------------------------------------------------------------------- /include/map_render/pcl_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/28/19. 28 | // 29 | 30 | #ifndef DSL_PCL_UTILS_H_ 31 | #define DSL_PCL_UTILS_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | //#include 40 | #include 41 | #include 42 | #include "core/global_model.h" 43 | #include "pangolin_utils.h" 44 | 45 | namespace dsl { 46 | 47 | typedef pcl::PointXYZINormal PointT; 48 | typedef pcl::PointXYZRGBNormal RGBPointT; 49 | typedef pcl::PointCloud PointCloud; 50 | typedef pcl::PointCloud RGBPointCloud; 51 | 52 | class PclUtils { 53 | public: 54 | PclUtils(); 55 | void LoadPcd(std::string file_name, double random_sample = 1, 56 | bool estimate_normal = false); 57 | void SavePcd(std::string file_name); 58 | void SaveRGBPcd(std::string file_name); 59 | PointCloud::Ptr &GetPointCloud() { return cloud_ptr_; }; 60 | RGBPointCloud::Ptr &GetRGBPointCloud() { return rgb_cloud_ptr_; }; 61 | void SetPointCloud(PointCloud::Ptr &in_ptr) { cloud_ptr_ = in_ptr; }; 62 | void PangolinToPcd(pangolin::GlTexture *vertex_tex, 63 | pangolin::GlTexture *normal_tex = NULL); 64 | void GlobalModelToPcd(GlobalModel &global_model); 65 | 66 | private: 67 | pcl::PCDReader pcd_reader_; 68 | pcl::PCDWriter pcd_writer_; 69 | PointCloud::Ptr cloud_ptr_; 70 | RGBPointCloud::Ptr rgb_cloud_ptr_; 71 | PangolinUtils pgl_utils_; 72 | }; 73 | 74 | } // namespace dsl 75 | 76 | #endif // DSL_PCL_UTILS_H_ 77 | -------------------------------------------------------------------------------- /include/map_render/shaders/color.glsl: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | float encodeColor(vec3 c) 20 | { 21 | int rgb = int(round(c.x * 255.0f)); 22 | rgb = (rgb << 8) + int(round(c.y * 255.0f)); 23 | rgb = (rgb << 8) + int(round(c.z * 255.0f)); 24 | return float(rgb); 25 | } 26 | 27 | vec3 decodeColor(float c) 28 | { 29 | vec3 col; 30 | col.x = float(int(c) >> 16 & 0xFF) / 255.0f; 31 | col.y = float(int(c) >> 8 & 0xFF) / 255.0f; 32 | col.z = float(int(c) & 0xFF) / 255.0f; 33 | return col; 34 | } 35 | -------------------------------------------------------------------------------- /include/map_render/shaders/combo_splat_cata.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | uniform vec4 cam; //cx, cy, fx, fy 22 | uniform float maxDepth; 23 | uniform float xi; 24 | uniform float max_half_fov; 25 | 26 | in vec4 position; 27 | in vec4 normRad; 28 | in vec4 colTime; 29 | 30 | layout(location = 0) out vec4 image; 31 | layout(location = 1) out vec4 vertex; 32 | layout(location = 2) out vec4 normal; 33 | //layout(location = 3) out uint time; 34 | 35 | #include "color.glsl" 36 | 37 | void main() 38 | { 39 | // vec3 l = normalize(vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f)); 40 | vec3 mu = vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f); 41 | float d2 = mu.x * mu.x + mu.y * mu.y; 42 | float factor = (xi + sqrt(1 + (1 - xi * xi) * d2)) / (d2 + 1); 43 | vec3 l = normalize(vec3(vec2(mu.xy) * factor, factor - xi)); 44 | 45 | vec3 corrected_pos = (dot(position.xyz, normRad.xyz) / dot(l, normRad.xyz)) * l; 46 | 47 | //check if the intersection is inside the surfel 48 | float sqrRad = pow(normRad.w, 2); 49 | vec3 diff = corrected_pos - position.xyz; 50 | 51 | if(dot(diff, diff) > sqrRad/*|| corrected_pos.z < 0.1*/) 52 | { 53 | discard; 54 | } 55 | //// TODO: FIXME: 56 | // vec3 corrected_pos = position.xyz; 57 | 58 | 59 | // vec3 corrected_pos = position.xyz; 60 | 61 | // if (colTime.y == 1) { 62 | // image = vec4(decodeColor(colTime.x), 1); 63 | // } else { 64 | // image = vec4(1, 0, 0, 1); 65 | // } 66 | 67 | image = vec4(decodeColor(colTime.x), 1); 68 | 69 | float z = corrected_pos.z; 70 | 71 | vertex = vec4((gl_FragCoord.x - cam.x) * z * (1.f / cam.z), (gl_FragCoord.y - cam.y) * z * (1.f / cam.w), z, position.w); 72 | 73 | normal = normRad; 74 | 75 | // time = uint(colTime.z); 76 | 77 | gl_FragDepth = (corrected_pos.z / (2 * maxDepth)) + 0.5f; 78 | } 79 | -------------------------------------------------------------------------------- /include/map_render/shaders/data.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | in vec4 vPosition0; 22 | in vec4 vColor0; 23 | in vec4 vNormRad0; 24 | flat in int updateId0; 25 | 26 | layout(location = 0) out vec4 vPosition1; 27 | layout(location = 1) out vec4 vColor1; 28 | layout(location = 2) out vec4 vNormRad1; 29 | 30 | void main() 31 | { 32 | //If we have a point to update in the existing model, store that 33 | if(updateId0 == 1) 34 | { 35 | vPosition1 = vPosition0; 36 | vColor1 = vColor0; 37 | vNormRad1 = vNormRad0; 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /include/map_render/shaders/data.geom: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(points, max_vertices = 1) out; 23 | 24 | in vec4 vPosition[]; 25 | in vec4 vColor[]; 26 | in vec4 vNormRad[]; 27 | flat in int updateId[]; 28 | 29 | out vec4 vPosition0; 30 | out vec4 vColor0; 31 | out vec4 vNormRad0; 32 | flat out int updateId0; 33 | 34 | void main() 35 | { 36 | //Emit a vertex if either we have an update to store, or a new unstable vertex to store 37 | if(updateId[0] > 0) 38 | { 39 | vPosition0 = vPosition[0]; 40 | vColor0 = vColor[0]; 41 | vNormRad0 = vNormRad[0]; 42 | updateId0 = updateId[0]; 43 | 44 | //This will be -10, -10 (offscreen) for new unstable vertices, so they don't show in the fragment shader 45 | gl_Position = gl_in[0].gl_Position; 46 | 47 | EmitVertex(); 48 | EndPrimitive(); 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /include/map_render/shaders/depth_splat_cata.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | #include "fov.glsl" 21 | 22 | uniform vec4 cam; //cx, cy, fx, fy 23 | uniform float xi; 24 | uniform float max_half_fov; 25 | uniform float maxDepth; 26 | 27 | in vec4 position; 28 | in vec4 normRad; 29 | 30 | out float FragColor; 31 | 32 | void main() 33 | { 34 | // vec3 l = normalize(vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f)); 35 | vec3 mu = vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f); 36 | float d2 = mu.x * mu.x + mu.y * mu.y; 37 | float factor = (xi + sqrt(1 + (1 - xi * xi) * d2)) / (d2 + 1); 38 | vec3 l = normalize(vec3(vec2(mu.xy) * factor, factor - xi)); 39 | 40 | vec3 corrected_pos = (dot(position.xyz, normRad.xyz) / dot(l, normRad.xyz)) * l; 41 | 42 | //check if the intersection is inside the surfel 43 | float sqrRad = pow(normRad.w, 2); 44 | vec3 diff = corrected_pos - position.xyz; 45 | 46 | if(dot(diff, diff) > sqrRad || !ValidFov(corrected_pos, max_half_fov)) 47 | { 48 | discard; 49 | } 50 | 51 | // vec3 corrected_pos = position.xyz; 52 | 53 | // FragColor = corrected_pos.z; 54 | FragColor = length(corrected_pos.xyz); 55 | 56 | gl_FragDepth = (corrected_pos.z / (2 * maxDepth)) + 0.5f; 57 | } 58 | -------------------------------------------------------------------------------- /include/map_render/shaders/draw_feedback.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | in vec4 vColor; 22 | 23 | out vec4 FragColor; 24 | 25 | void main() 26 | { 27 | FragColor = vColor; 28 | } 29 | -------------------------------------------------------------------------------- /include/map_render/shaders/draw_feedback.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | layout (location = 0) in vec4 position; 22 | layout (location = 1) in vec4 color; 23 | layout (location = 2) in vec4 normal; 24 | 25 | uniform mat4 MVP; 26 | uniform mat4 pose; 27 | uniform float threshold; 28 | uniform int colorType; 29 | 30 | out vec4 vColor; 31 | 32 | #include "color.glsl" 33 | 34 | void main() 35 | { 36 | if(position.w > threshold) 37 | { 38 | if(colorType == 1) 39 | { 40 | vColor = vec4(normal.xyz, 1.0); 41 | } 42 | else if(colorType == 2) 43 | { 44 | vColor = vec4(decodeColor(color.x), 1.0); 45 | } 46 | else 47 | { 48 | vColor = vec4((vec3(.5f, .5f, .5f) * abs(dot(normal.xyz, vec3(1.0, 1.0, 1.0)))) + vec3(0.1f, 0.1f, 0.1f), 1.0f); 49 | } 50 | gl_Position = MVP * pose * vec4(position.xyz, 1.0); 51 | // gl_Position = MVP * vec4(position.xyz, 1.0); 52 | } 53 | else 54 | { 55 | gl_Position = vec4(-10, -10, 0, 1); 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /include/map_render/shaders/draw_surface.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | in vec3 vColor0; 22 | in vec2 texcoord; 23 | in float radius; 24 | flat in int unstablePoint; 25 | 26 | out vec4 FragColor; 27 | 28 | void main() 29 | { 30 | if(dot(texcoord, texcoord) > 1.0) 31 | discard; 32 | 33 | if (vColor0.x == .0f && vColor0.y == 0 && vColor0.z == 0) { 34 | FragColor = vec4(.0f, .0f, .0f, .0f); 35 | } else { 36 | FragColor = vec4(vColor0, 0.8f); 37 | } 38 | // FragColor = vec4(vColor0, 0.8f); 39 | 40 | gl_FragDepth = gl_FragCoord.z; 41 | } 42 | -------------------------------------------------------------------------------- /include/map_render/shaders/draw_surface.geom: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(triangle_strip, max_vertices = 4) out; 23 | 24 | uniform float threshold; 25 | uniform float signMult; 26 | 27 | in vec4 vColor[]; 28 | in vec4 vPosition[]; 29 | in vec4 vNormRad[]; 30 | in mat4 vMVP[]; 31 | in int colorType0[]; 32 | 33 | out vec3 vColor0; 34 | out vec2 texcoord; 35 | out float radius; 36 | flat out int unstablePoint; 37 | 38 | #include "color.glsl" 39 | 40 | void main() 41 | { 42 | vec3 v; 43 | vec3 n; 44 | 45 | vec4 localPosition = vMVP[0] * vec4(vPosition[0].xyz, 1.0); 46 | if (localPosition.z >= 0.5) { 47 | if(colorType0[0] == 1) { 48 | vColor0 = vNormRad[0].xyz; 49 | if (vColor0.z < 0) { 50 | vColor0 = -vColor0; 51 | } 52 | } else if(colorType0[0] == 2) { 53 | if (vColor[0].y == 1) { 54 | vColor0 = decodeColor(vColor[0].x); 55 | // vColor0 = vec3(1.0f, 0, 0); 56 | } else { 57 | vColor0 = vec3(.0f, .0f, .0f); 58 | } 59 | } else { 60 | // WARNING: temporal use for intensity // * 2 for visualization only 61 | vColor0 = vec3(vColor[0].w) / 255.0; 62 | // vColor0 = (vec3(.5f, .5f, .5f) * abs(dot(vNormRad[0].xyz, vec3(1.0, 1.0, 1.0)))) + vec3(0.1f, 0.1f, 0.1f); 63 | } 64 | // vec4 vNormRad[1]; 65 | // vNormRad[0].xyz = normalize(vPosition[0].xyz); 66 | radius = vNormRad[0].w; 67 | 68 | vec3 x = normalize(vec3((vNormRad[0].y - vNormRad[0].z), -vNormRad[0].x, vNormRad[0].x)) * vNormRad[0].w * 1.41421356; 69 | 70 | vec3 y = cross(vNormRad[0].xyz, x); 71 | 72 | unstablePoint = 1; 73 | n = vNormRad[0].xyz; 74 | 75 | texcoord = vec2(-1.0, -1.0); 76 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz + x, 1.0); 77 | v = vPosition[0].xyz + x; 78 | EmitVertex(); 79 | 80 | texcoord = vec2(1.0, -1.0); 81 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz + y, 1.0); 82 | v = vPosition[0].xyz + y; 83 | EmitVertex(); 84 | 85 | texcoord = vec2(-1.0, 1.0); 86 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz - y, 1.0); 87 | v = vPosition[0].xyz - y; 88 | EmitVertex(); 89 | 90 | texcoord = vec2(1.0, 1.0); 91 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz - x, 1.0); 92 | v = vPosition[0].xyz - x; 93 | EmitVertex(); 94 | EndPrimitive(); 95 | } 96 | 97 | } 98 | -------------------------------------------------------------------------------- /include/map_render/shaders/draw_surface.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | layout (location = 0) in vec4 position; 22 | layout (location = 1) in vec4 color; 23 | layout (location = 2) in vec4 normal; 24 | 25 | uniform mat4 MVP; 26 | uniform int colorType; 27 | 28 | out vec4 vColor; 29 | out vec4 vPosition; 30 | out vec4 vNormRad; 31 | out mat4 vMVP; 32 | out int colorType0; 33 | 34 | void main() 35 | { 36 | colorType0 = colorType; 37 | vColor = color; 38 | vPosition = position; 39 | vNormRad = normal; 40 | // vNormRad.w = 0.01; // default surfel radius to draw 41 | vMVP = MVP; 42 | gl_Position = MVP * vec4(position.xyz, 1.0); // world -> camera 43 | } 44 | -------------------------------------------------------------------------------- /include/map_render/shaders/empty.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 20 | 21 | void main() 22 | { 23 | } 24 | -------------------------------------------------------------------------------- /include/map_render/shaders/feedback_buffer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #ifndef DSL_FEEDBACK_BUFFER_H_ 31 | #define DSL_FEEDBACK_BUFFER_H_ 32 | 33 | #include 34 | #include 35 | #include "map_render/shaders/shaders.h" 36 | #include "map_render/shaders/uniform.h" 37 | #include "map_render/shaders/vertex.h" 38 | #include "util/global_calib.h" 39 | 40 | namespace dsl { 41 | 42 | class FeedbackBuffer { 43 | public: 44 | FeedbackBuffer(std::shared_ptr program); 45 | virtual ~FeedbackBuffer(); 46 | 47 | std::shared_ptr program; 48 | 49 | void compute(pangolin::GlTexture *color, pangolin::GlTexture *depth, 50 | const int &time, const float depthCutoff); 51 | 52 | void render(pangolin::OpenGlMatrix mvp, const Eigen::Matrix4f &pose, 53 | const bool drawNormals, const bool drawColors); 54 | 55 | static const std::string RAW, FILTERED, PREDICT; 56 | 57 | GLuint vbo; 58 | GLuint fid; 59 | 60 | private: 61 | std::shared_ptr drawProgram; 62 | GLuint uvo; 63 | GLuint countQuery; 64 | const int bufferSize; 65 | unsigned int count; 66 | }; 67 | 68 | } // namespace dsl 69 | 70 | #endif // DSL_FEEDBACK_BUFFER_H_ 71 | -------------------------------------------------------------------------------- /include/map_render/shaders/fov.glsl: -------------------------------------------------------------------------------- 1 | bool ValidFov(vec3 p, float max_half_fov) { 2 | float s = sqrt(p.x * p.x + p.y * p.y); 3 | float z = p.z; 4 | return abs(atan(s, z)) < max_half_fov; 5 | } -------------------------------------------------------------------------------- /include/map_render/shaders/geometry.glsl: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | //Central difference on floating point depth maps 20 | //Cam is //cx, cy, 1 / fx, 1 / fy 21 | vec3 getVertex(vec2 texCoord, float x, float y, vec4 cam, sampler2D depth) 22 | { 23 | float z = float(textureLod(depth, texCoord, 0.0)); 24 | return vec3((x - cam.x) * z * cam.z, (y - cam.y) * z * cam.w, z); 25 | } 26 | 27 | //Cam is //cx, cy, 1 / fx, 1 / fy 28 | vec3 getNormal(vec3 vPosition, vec2 texCoord, float x, float y, vec4 cam, sampler2D depth) 29 | { 30 | vec3 vPosition_xf = getVertex(vec2(texCoord.x + (1.0 / cols), texCoord.y), x + 1, y, cam, depth); 31 | vec3 vPosition_xb = getVertex(vec2(texCoord.x - (1.0 / cols), texCoord.y), x - 1, y, cam, depth); 32 | 33 | vec3 vPosition_yf = getVertex(vec2(texCoord.x, texCoord.y + (1.0 / rows)), x, y + 1, cam, depth); 34 | vec3 vPosition_yb = getVertex(vec2(texCoord.x, texCoord.y - (1.0 / rows)), x, y - 1, cam, depth); 35 | 36 | vec3 del_x = ((vPosition_xb + vPosition) / 2) - ((vPosition_xf + vPosition) / 2); 37 | vec3 del_y = ((vPosition_yb + vPosition) / 2) - ((vPosition_yf + vPosition) / 2); 38 | 39 | return normalize(cross(del_x, del_y)); 40 | } 41 | 42 | //Forward difference on raw depth maps still in ushort mm 43 | //Cam is //cx, cy, 1 / fx, 1 / fy 44 | vec3 getVertex(vec2 texcoord, int x, int y, vec4 cam, usampler2D depth) 45 | { 46 | float z = float(textureLod(depth, texcoord, 0.0)) / 1000.0f; 47 | return vec3((x - cam.x) * z * cam.z, (y - cam.y) * z * cam.w, z); 48 | } 49 | 50 | //Cam is //cx, cy, 1 / fx, 1 / fy 51 | vec3 getNormal(vec3 vPosition, vec2 texcoord, int x, int y, vec4 cam, usampler2D depth) 52 | { 53 | vec3 vPosition_x = getVertex(vec2(texcoord.x + (1.0 / cols), texcoord.y), x + 1, y, cam, depth); 54 | vec3 vPosition_y = getVertex(vec2(texcoord.x, texcoord.y + (1.0 / rows)), x, y + 1, cam, depth); 55 | 56 | vec3 del_x = vPosition_x - vPosition; 57 | vec3 del_y = vPosition_y - vPosition; 58 | 59 | return normalize(cross(del_x, del_y)); 60 | } 61 | -------------------------------------------------------------------------------- /include/map_render/shaders/index_global_map.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | // redundant 20 | 21 | #version 330 core 22 | 23 | // in vec4 vPosition0; 24 | // in vec4 vColorTime0; 25 | // in vec4 vNormRad0; 26 | // flat in int vertexId; 27 | 28 | in vec2 texcoord; 29 | flat in vec4 vPosition1; 30 | flat in vec4 vColorTime1; 31 | flat in vec4 vNormRad1; 32 | flat in int vertexId1; 33 | 34 | layout(location = 0) out int FragColor; 35 | layout(location = 1) out vec4 vPosition2; 36 | layout(location = 2) out vec4 vColorTime2; 37 | layout(location = 3) out vec4 vNormRad2; 38 | 39 | void main() 40 | { 41 | if(dot(texcoord, texcoord) > 1.0) 42 | discard; 43 | vPosition2 = vPosition1; 44 | vColorTime2 = vColorTime1; 45 | vNormRad2 = vNormRad1; 46 | FragColor = vertexId1; 47 | } 48 | -------------------------------------------------------------------------------- /include/map_render/shaders/index_global_map.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | // wasteful and redundant 20 | 21 | #version 330 core 22 | 23 | layout (location = 0) in vec4 vPosition; 24 | layout (location = 1) in vec4 vColorTime; 25 | layout (location = 2) in vec4 vNormRad; 26 | 27 | out vec4 vPosition0; 28 | out vec4 vColorTime0; 29 | out vec4 vNormRad0; 30 | flat out int vertexId; 31 | 32 | uniform mat4 t_inv; 33 | uniform vec4 cam; //cx, cy, fx, fy 34 | uniform float cols; 35 | uniform float rows; 36 | uniform float maxDepth; 37 | //uniform int time; 38 | //uniform int timeDelta; 39 | 40 | void main() 41 | { 42 | // vec4 vPosHome = t_inv * vec4(vPosition.xyz, 1.0); 43 | 44 | // float x = 0; 45 | // float y = 0; 46 | 47 | // if(vPosHome.z > maxDepth || vPosHome.z < 0 /*|| time - vColorTime.w > timeDelta*/) 48 | // { 49 | // x = -10; 50 | // y = -10; 51 | // vertexId = 0; 52 | // } 53 | // else 54 | // { 55 | // x = ((((cam.z * vPosHome.x) / vPosHome.z) + cam.x) - (cols * 0.5)) / (cols * 0.5); 56 | // y = ((((cam.w * vPosHome.y) / vPosHome.z) + cam.y) - (rows * 0.5)) / (rows * 0.5); 57 | // vertexId = gl_VertexID; 58 | // } 59 | 60 | // gl_Position = vec4(x, y, vPosHome.z / maxDepth, 1.0); 61 | 62 | vPosition0 = vec4(vPosition.xyz, vPosition.w); 63 | vColorTime0 = vColorTime; 64 | vNormRad0 = vec4(normalize(vNormRad.xyz), vNormRad.w); 65 | vertexId = gl_VertexID; 66 | 67 | // vNormRad0.w = 0.01; // set default surfel radius here; 68 | } 69 | -------------------------------------------------------------------------------- /include/map_render/shaders/index_global_map_cata.geom: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | #include "fov.glsl" 21 | 22 | layout(points) in; 23 | layout(triangle_strip, max_vertices = 4) out; 24 | 25 | in vec4 vPosition0[]; 26 | in vec4 vColorTime0[]; 27 | in vec4 vNormRad0[]; 28 | flat in int vertexId[]; 29 | 30 | out vec2 texcoord; 31 | flat out vec4 vPosition1; 32 | flat out vec4 vColorTime1; 33 | flat out vec4 vNormRad1; 34 | flat out int vertexId1; 35 | 36 | uniform mat4 t_inv; 37 | uniform vec4 cam; //cx, cy, fx, fy 38 | uniform float xi; 39 | uniform float max_half_fov; 40 | uniform float cols; 41 | uniform float rows; 42 | uniform float maxDepth; 43 | 44 | vec4 PointToPixel(vec4 vPosHome) { 45 | float x = 0, y = 0; 46 | // if(vPosHome.z > maxDepth || vPosHome.z < 0) 47 | // { 48 | // x = -10; 49 | // y = -10; 50 | // } 51 | // else 52 | // { 53 | // x = ((((cam.z * vPosHome.x) / vPosHome.z) + cam.x) - (cols * 0.5)) / (cols * 0.5); 54 | // y = ((((cam.w * vPosHome.y) / vPosHome.z) + cam.y) - (rows * 0.5)) / (rows * 0.5); 55 | vec3 ps = normalize(vPosHome.xyz); 56 | float deno = ps.z + xi; 57 | float xs = ps.x; 58 | float ys = ps.y; 59 | float xm = xs / deno; 60 | float ym = ys / deno; 61 | x = (((cam.z * xm) + cam.x) - (cols * 0.5)) / (cols * 0.5); 62 | y = (((cam.w * ym) + cam.y) - (rows * 0.5)) / (rows * 0.5); 63 | // } 64 | return vec4(x, y, vPosHome.z / maxDepth, 1.0); 65 | } 66 | 67 | void main() 68 | { 69 | vec4 v1, v2, v3, v4; 70 | vec4 n; 71 | 72 | vec3 x = normalize(vec3((vNormRad0[0].y - vNormRad0[0].z), -vNormRad0[0].x, vNormRad0[0].x)) * vNormRad0[0].w * 1.41421356; 73 | vec3 y = cross(vNormRad0[0].xyz, x); 74 | 75 | v1 = t_inv * vec4(vPosition0[0].xyz + x, 1.0); 76 | v2 = t_inv * vec4(vPosition0[0].xyz + y, 1.0); 77 | v3 = t_inv * vec4(vPosition0[0].xyz - y, 1.0); 78 | v4 = t_inv * vec4(vPosition0[0].xyz - x, 1.0); 79 | 80 | if(v1.z <= maxDepth && ValidFov(v1.xyz, max_half_fov) && 81 | v2.z <= maxDepth && ValidFov(v2.xyz, max_half_fov) && 82 | v3.z <= maxDepth && ValidFov(v3.xyz, max_half_fov) && 83 | v4.z <= maxDepth && ValidFov(v4.xyz, max_half_fov)) { 84 | 85 | n = vec4(vNormRad0[0].xyz, 1.0); 86 | 87 | vPosition1 = vPosition0[0]; 88 | vNormRad1 = vNormRad0[0]; 89 | vColorTime1 = vColorTime0[0]; 90 | vertexId1 = vertexId[0]; 91 | 92 | texcoord = vec2(-1.0, -1.0); 93 | gl_Position = PointToPixel(v1); 94 | EmitVertex(); 95 | 96 | texcoord = vec2(1.0, -1.0); 97 | gl_Position = PointToPixel(v2); 98 | EmitVertex(); 99 | 100 | texcoord = vec2(-1.0, 1.0); 101 | gl_Position = PointToPixel(v3); 102 | EmitVertex(); 103 | 104 | texcoord = vec2(1.0, 1.0); 105 | gl_Position = PointToPixel(v4); 106 | EmitVertex(); 107 | EndPrimitive(); 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /include/map_render/shaders/index_map.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | in vec4 vPosition0; 22 | in vec4 vColorTime0; 23 | in vec4 vNormRad0; 24 | flat in int vertexId; 25 | 26 | layout(location = 0) out int FragColor; 27 | layout(location = 1) out vec4 vPosition1; 28 | layout(location = 2) out vec4 vColorTime1; 29 | layout(location = 3) out vec4 vNormRad1; 30 | 31 | void main() 32 | { 33 | vPosition1 = vPosition0; 34 | vColorTime1 = vColorTime0; 35 | vNormRad1 = vNormRad0; 36 | FragColor = vertexId; 37 | } 38 | -------------------------------------------------------------------------------- /include/map_render/shaders/index_map_cata.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | #include "fov.glsl" 21 | 22 | layout (location = 0) in vec4 vPosition; 23 | layout (location = 1) in vec4 vColorTime; 24 | layout (location = 2) in vec4 vNormRad; 25 | 26 | out vec4 vPosition0; 27 | out vec4 vColorTime0; 28 | out vec4 vNormRad0; 29 | flat out int vertexId; 30 | 31 | uniform mat4 t_inv; 32 | uniform vec4 cam; //cx, cy, fx, fy 33 | uniform float xi; 34 | uniform float max_half_fov; 35 | uniform float cols; 36 | uniform float rows; 37 | uniform float maxDepth; 38 | //uniform int time; 39 | //uniform int timeDelta; 40 | 41 | void main() 42 | { 43 | vec4 vPosHome = t_inv * vec4(vPosition.xyz, 1.0); 44 | 45 | float x = 0; 46 | float y = 0; 47 | 48 | if(vPosHome.z > maxDepth || !ValidFov(vPosHome.xyz, max_half_fov) /*|| time - vColorTime.w > timeDelta*/) 49 | { 50 | x = -10; 51 | y = -10; 52 | vertexId = 0; 53 | } 54 | else 55 | { 56 | // x = ((((cam.z * vPosHome.x) / vPosHome.z) + cam.x) - (cols * 0.5)) / (cols * 0.5); 57 | // y = ((((cam.w * vPosHome.y) / vPosHome.z) + cam.y) - (rows * 0.5)) / (rows * 0.5); 58 | // vertexId = gl_VertexID; 59 | vec3 ps = normalize(vPosHome.xyz); 60 | float deno = ps.z + xi; 61 | float xs = ps.x; 62 | float ys = ps.y; 63 | float xm = xs / deno; 64 | float ym = ys / deno; 65 | x = (((cam.z * xm) + cam.x) - (cols * 0.5)) / (cols * 0.5); 66 | y = (((cam.w * ym) + cam.y) - (rows * 0.5)) / (rows * 0.5); 67 | vertexId = gl_VertexID; 68 | } 69 | 70 | gl_Position = vec4(x, y, vPosHome.z / maxDepth, 1.0); 71 | 72 | vPosition0 = vec4(vPosHome.xyz, vPosition.w); 73 | vColorTime0 = vColorTime; 74 | vNormRad0 = vec4(normalize(mat3(t_inv) * vNormRad.xyz), vNormRad.w); 75 | } 76 | -------------------------------------------------------------------------------- /include/map_render/shaders/init_unstable.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | layout (location = 0) in vec4 vPosition; 22 | layout (location = 1) in vec4 vColor; 23 | layout (location = 2) in vec4 vNormRad; 24 | 25 | out vec4 vPosition0; 26 | out vec4 vColor0; 27 | out vec4 vNormRad0; 28 | 29 | void main() 30 | { 31 | vPosition0 = vPosition; 32 | vColor0 = vColor; 33 | // vColor0.y = -1; // color unset 34 | vColor0.z = 1; // This sets the vertex's initialisation time 35 | vNormRad0 = vNormRad; 36 | } 37 | -------------------------------------------------------------------------------- /include/map_render/shaders/intensity_splat_cata.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of li_cam. 3 | * 4 | * Copyright (C) 2019 HKUST 5 | * 6 | */ 7 | 8 | #version 330 core 9 | #include "fov.glsl" 10 | 11 | uniform vec4 cam; //cx, cy, fx, fy 12 | uniform float xi; 13 | uniform float max_half_fov; 14 | uniform float maxDepth; 15 | 16 | in vec4 position; 17 | in vec4 normRad; 18 | in vec4 colTime; 19 | 20 | layout(location = 0) out vec4 intensity; 21 | 22 | void main() 23 | { 24 | // vec3 l = normalize(vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f)); 25 | vec3 mu = vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f); 26 | float d2 = mu.x * mu.x + mu.y * mu.y; 27 | float factor = (xi + sqrt(1 + (1 - xi * xi) * d2)) / (d2 + 1); 28 | vec3 l = normalize(vec3(vec2(mu.xy) * factor, factor - xi)); 29 | 30 | vec3 corrected_pos = (dot(position.xyz, normRad.xyz) / dot(l, normRad.xyz)) * l; 31 | 32 | //check if the intersection is inside the surfel 33 | float sqrRad = pow(normRad.w, 2); 34 | vec3 diff = corrected_pos - position.xyz; 35 | 36 | if(dot(diff, diff) > sqrRad || !ValidFov(corrected_pos, max_half_fov)) 37 | { 38 | discard; 39 | } 40 | 41 | 42 | intensity = vec4(colTime.w / 255.0); 43 | intensity.w = 1.0; 44 | 45 | gl_FragDepth = (corrected_pos.z / (2 * maxDepth)) + 0.5f; 46 | } 47 | -------------------------------------------------------------------------------- /include/map_render/shaders/parse.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #ifndef DSL_PARSE_H_ 31 | #define DSL_PARSE_H_ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #define XSTR(x) #x 44 | #define STR(x) XSTR(x) 45 | 46 | namespace dsl { 47 | 48 | class Parse { 49 | public: 50 | static const Parse &Get(); 51 | 52 | int Arg(int argc, char **argv, const char *str, std::string &val) const; 53 | 54 | int Arg(int argc, char **argv, const char *str, float &val) const; 55 | 56 | int Arg(int argc, char **argv, const char *str, int &val) const; 57 | 58 | std::string ShaderDir() const; 59 | 60 | std::string BaseDir() const; 61 | 62 | private: 63 | Parse(); 64 | 65 | int FindArg(int argc, char **argv, const char *argument_name) const; 66 | }; 67 | 68 | } 69 | 70 | #endif // DSL_PARSE_H_ 71 | -------------------------------------------------------------------------------- /include/map_render/shaders/quad.geom: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(triangle_strip, max_vertices = 4) out; 23 | 24 | out vec2 texcoord; 25 | 26 | void main() 27 | { 28 | gl_Position = vec4(1.0, 1.0, 0.0, 1.0); 29 | texcoord = vec2(1.0, 1.0); 30 | EmitVertex(); 31 | 32 | gl_Position = vec4(-1.0, 1.0, 0.0, 1.0); 33 | texcoord = vec2(0.0, 1.0); 34 | EmitVertex(); 35 | 36 | gl_Position = vec4(1.0,-1.0, 0.0, 1.0); 37 | texcoord = vec2(1.0, 0.0); 38 | EmitVertex(); 39 | 40 | gl_Position = vec4(-1.0,-1.0, 0.0, 1.0); 41 | texcoord = vec2(0.0, 0.0); 42 | EmitVertex(); 43 | 44 | EndPrimitive(); 45 | } 46 | -------------------------------------------------------------------------------- /include/map_render/shaders/surfels.glsl: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | float getRadius(float depth, float norm_z) 20 | { 21 | float meanFocal = ((1.0 / abs(cam.z)) + (1.0 / abs(cam.w))) / 2.0; 22 | 23 | const float sqrt2 = 1.41421356237f; 24 | 25 | float radius = (depth / meanFocal) * sqrt2; 26 | 27 | float radius_n = radius; 28 | 29 | radius_n = radius_n / abs(norm_z); 30 | 31 | radius_n = min(2.0f * radius, radius_n); 32 | 33 | return radius_n; 34 | } 35 | 36 | float confidence(float x, float y, float weighting) 37 | { 38 | const float maxRadDist = 400; //sqrt((width * 0.5)^2 + (height * 0.5)^2) 39 | const float twoSigmaSquared = 0.72; //2*(0.6^2) from paper "Real-time 3D Rreconstruction in Dynamic Scenes using Point-based Fusion", for RGB-D camera 40 | 41 | vec2 pixelPosCentered = vec2(x, y) - cam.xy; 42 | 43 | float radialDist = sqrt(dot(pixelPosCentered, pixelPosCentered)) / maxRadDist; 44 | 45 | return exp((-(radialDist * radialDist) / twoSigmaSquared)) * weighting; 46 | } 47 | -------------------------------------------------------------------------------- /include/map_render/shaders/uniform.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #ifndef DSL_UNIFORM_H_ 31 | #define DSL_UNIFORM_H_ 32 | 33 | #include 34 | #include 35 | 36 | class Uniform { 37 | public: 38 | Uniform(const std::string& id, const int& v) : id(id), i(v), t(INT) {} 39 | 40 | Uniform(const std::string& id, const float& v) : id(id), f(v), t(FLOAT) {} 41 | 42 | Uniform(const std::string& id, const Eigen::Vector2f& v) 43 | : id(id), v2(v), t(VEC2) {} 44 | 45 | Uniform(const std::string& id, const Eigen::Vector3f& v) 46 | : id(id), v3(v), t(VEC3) {} 47 | 48 | Uniform(const std::string& id, const Eigen::Vector4f& v) 49 | : id(id), v4(v), t(VEC4) {} 50 | 51 | Uniform(const std::string& id, const Eigen::Matrix4f& v) 52 | : id(id), m4(v), t(MAT4) {} 53 | 54 | std::string id; 55 | 56 | int i; 57 | float f; 58 | Eigen::Vector2f v2; 59 | Eigen::Vector3f v3; 60 | Eigen::Vector4f v4; 61 | Eigen::Matrix4f m4; 62 | 63 | enum Type { INT, FLOAT, VEC2, VEC3, VEC4, MAT4, NONE }; 64 | 65 | Type t; 66 | }; 67 | 68 | #endif // DSL_UNIFORM_H_ 69 | -------------------------------------------------------------------------------- /include/map_render/shaders/vertex.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #ifndef DSL_VERTEX_H_ 31 | #define DSL_VERTEX_H_ 32 | 33 | #include 34 | 35 | namespace dsl { 36 | 37 | class Vertex { 38 | public: 39 | static const int SIZE; 40 | 41 | private: 42 | Vertex() {} 43 | }; 44 | 45 | } 46 | 47 | #endif // DSL_VERTEX_H_ 48 | -------------------------------------------------------------------------------- /include/map_render/shaders/visualise_textures.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #version 330 core 20 | 21 | uniform sampler2D texVerts; 22 | uniform float maxDepth; 23 | 24 | in vec2 texcoord; 25 | 26 | out vec4 FragColor; 27 | 28 | void main() 29 | { 30 | vec4 vertex = texture2D(texVerts, texcoord); 31 | 32 | // should be vertex.x if it is only depth 33 | // GL_LUMINANCE32F_ARB, // GL_R32F 34 | // GL_LUMINANCE, // GL_RED 35 | if(vertex.z > maxDepth || vertex.z <= 0) 36 | { 37 | discard; 38 | } 39 | else 40 | { 41 | FragColor = 1.0f - vec4(vertex.z / maxDepth); 42 | FragColor.w = 1.0f; 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /include/map_render/yaml_loader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/28/19. 28 | // 29 | 30 | #ifndef DSL_YAML_LOADER_H_ 31 | #define DSL_YAML_LOADER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace dsl { 39 | class YamlLoader { 40 | public: 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | YamlLoader(std::string yaml_file); 43 | 44 | Eigen::Matrix3d eigen_R; 45 | Eigen::Vector3d eigen_T; 46 | std::string csv_file, pcd_file, mask_file; 47 | double fx = 0, fy = 0, cx = 0, cy = 0; 48 | double gamma1 = 0, gamma2 = 0, u0 = 0, v0 = 0, xi = 0; 49 | int image_width = 0, image_height = 0; 50 | Eigen::Matrix3f R_lc = Eigen::Matrix3f::Identity(); 51 | Eigen::Vector3f t_lc = Eigen::Vector3f(0, 0, 0); 52 | Eigen::Affine3f T_lc = Eigen::Affine3f::Identity(); 53 | Eigen::Affine3f initial_pose = Eigen::Affine3f::Identity(); 54 | double intensity_scalar = 1.0; 55 | double rgb_scalar = 1.0; 56 | bool dso_baseline = false; 57 | int tracking_plot_level = 0; 58 | int fast_forward = 0; 59 | double retrack_threshold = 1.25; 60 | bool equal_hist = false; 61 | double surfel_size = 0.01; // default surfel 62 | std::string output_folder = ""; 63 | // double init_translation_noise = 0.0; 64 | // double init_rotation_noise = 0.0; 65 | }; 66 | } // namespace dsl 67 | 68 | #endif // DSL_YAML_LOADER_H_ 69 | -------------------------------------------------------------------------------- /include/optimization/ab_local_parameterization.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 1/18/20. 28 | // 29 | 30 | #ifndef DSL_AB_LOCAL_PARAMETERIZATION_H_ 31 | #define DSL_AB_LOCAL_PARAMETERIZATION_H_ 32 | 33 | #include "util/num_type.h" 34 | #include "dsl_common.h" 35 | #include 36 | 37 | namespace dsl { 38 | 39 | struct AbLocalParameterization : public ceres::LocalParameterization { 40 | virtual bool Plus(const double* x, const double* delta, 41 | double* x_plus_delta) const { 42 | Eigen::Map tangent(x); 43 | 44 | Eigen::Map d_tangent(delta); 45 | 46 | Eigen::Map tangent_plus(x_plus_delta); 47 | 48 | Vec2 scaled_dx = d_tangent; 49 | if (!settigNoScalingAtOpt) { 50 | scaled_dx.x() *= SCALE_A; 51 | scaled_dx.y() *= SCALE_B; 52 | } 53 | 54 | tangent_plus = tangent + scaled_dx; 55 | 56 | return true; 57 | } 58 | 59 | virtual bool ComputeJacobian(const double* x, double* jacobian) const { 60 | Eigen::Map> j(jacobian); 61 | j.setIdentity(); 62 | return true; 63 | } 64 | 65 | virtual int GlobalSize() const { return 2; }; 66 | virtual int LocalSize() const { return 2; }; 67 | }; 68 | 69 | } 70 | 71 | #endif // DSL_AB_LOCAL_PARAMETERIZATION_H_ 72 | -------------------------------------------------------------------------------- /include/optimization/homo_cost_functor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/28/19. 28 | // 29 | 30 | #ifndef DSL_HOMOCOSTFUNCTOR_H_ 31 | #define DSL_HOMOCOSTFUNCTOR_H_ 32 | 33 | #include 34 | #include "dsl_common.h" 35 | #include "util/num_type.h" 36 | #include "optimization/photo_cost_functor.h" 37 | 38 | namespace dsl { 39 | 40 | class HomoCostFunctor : public PhotoCostFunctor { 41 | public: 42 | HomoCostFunctor(PointFrameResidual* residual, CalibHessian& HCalib, 43 | bool baseline = false); 44 | 45 | Vec3f* dI; 46 | 47 | virtual bool Evaluate(double const* const* parameters, double* residuals, 48 | double** jacobians) const; 49 | mutable float idist; 50 | 51 | }; 52 | 53 | } 54 | 55 | #endif // DSL_HOMOCOSTFUNCTOR_H_ 56 | -------------------------------------------------------------------------------- /include/optimization/photo_cost_functor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/18/19. 28 | // 29 | 30 | #ifndef DSL_PHOTO_COST_FUNCTOR_H_ 31 | #define DSL_PHOTO_COST_FUNCTOR_H_ 32 | 33 | #include 34 | #include "dsl_common.h" 35 | #include "util/num_type.h" 36 | 37 | namespace dsl { 38 | 39 | class PhotoCostFunctor : public ceres::SizedCostFunction<8, 6, 2, 6, 2, 1> { 40 | public: 41 | PhotoCostFunctor(PointFrameResidual* residual, CalibHessian& HCalib, 42 | bool baseline = false); 43 | 44 | Vec3f* dI; 45 | 46 | virtual bool Evaluate(double const* const* parameters, double* residuals, 47 | double** jacobians) const; 48 | 49 | void SetEmpty(double* residuals, double** jacobians) const; 50 | 51 | void SetResidualFromNew(PointFrameResidual *pfr) const; 52 | void SetOOB(double* residuals, double** jacobians, PointFrameResidual *pfr) const; 53 | 54 | double CalcHdd(const SE3f& host_to_target, double idist); 55 | 56 | // protected: 57 | PointHessian* point_; 58 | FrameHessian* host_; 59 | FrameHessian* target_; 60 | PointFrameResidual* pfr_; 61 | CalibHessian& HCalib_; 62 | 63 | mutable bool first_eval = true; 64 | 65 | bool baseline_; 66 | // mutable bool idist_converged_ = false; 67 | }; 68 | 69 | } // namespace dsl 70 | 71 | #endif // DSL_PHOTO_COST_FUNCTOR_H_ 72 | -------------------------------------------------------------------------------- /include/optimization/se3_local_parameterization.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/18/19. 28 | // 29 | 30 | #ifndef DSL_SE3_LOCAL_PARAMETERIZATION_H_ 31 | #define DSL_SE3_LOCAL_PARAMETERIZATION_H_ 32 | 33 | #include "util/num_type.h" 34 | #include "ceres/ceres.h" 35 | 36 | namespace dsl { 37 | 38 | struct SE3LocalParameterization : public ceres::LocalParameterization { 39 | virtual bool Plus(const double* x, const double* delta, 40 | double* x_plus_delta) const { 41 | Eigen::Map tangent(x); 42 | 43 | Eigen::Map d_tangent(delta); 44 | 45 | Eigen::Map tangent_plus(x_plus_delta); 46 | 47 | tangent_plus = (SE3::exp(d_tangent) * SE3::exp(tangent)).log(); 48 | 49 | return true; 50 | } 51 | 52 | virtual bool ComputeJacobian(const double* x, double* jacobian) const { 53 | Eigen::Map> j(jacobian); 54 | j.setIdentity(); 55 | return true; 56 | } 57 | 58 | virtual int GlobalSize() const { return 6; }; 59 | virtual int LocalSize() const { return 6; }; 60 | }; 61 | 62 | } 63 | #endif // DSL_SE3_LOCAL_PARAMETERIZATION_H_ 64 | -------------------------------------------------------------------------------- /include/optimization/se3_prior_cost_functor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 1/15/20. 28 | // 29 | 30 | #ifndef DSL_SE3_COST_FUNCTOR_H_ 31 | #define DSL_SE3_COST_FUNCTOR_H_ 32 | 33 | #include 34 | #include "dsl_common.h" 35 | #include "util/num_type.h" 36 | 37 | namespace dsl { 38 | 39 | class SE3PriorCostFunctor : public ceres::SizedCostFunction<6, 6> { 40 | public: 41 | typedef Eigen::Matrix information_t; 42 | SE3PriorCostFunctor(const SE3& measurement, const information_t& information); 43 | 44 | void SetMeasurement(const SE3& measurement); 45 | const SE3& GetMeasurement() { return measurement_; } 46 | 47 | void SetInformation(const information_t& information); 48 | const information_t& GetInformation() { return information_; } 49 | 50 | virtual bool Evaluate(double const* const* parameters, double* residuals, 51 | double** jacobians) const; 52 | 53 | private: 54 | SE3 measurement_; 55 | SE3 measurement_inv_; 56 | information_t information_; 57 | information_t sqrt_information_; 58 | }; 59 | 60 | } // namespace dsl 61 | 62 | #endif // DSL_SE3_COST_FUNCTOR_H_ 63 | -------------------------------------------------------------------------------- /include/relocalization/converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/3/20. 28 | // 29 | 30 | #ifndef DSL_CONVERTER_H 31 | #define DSL_CONVERTER_H 32 | 33 | #include 34 | #include "util/num_type.h" 35 | 36 | namespace dsl { 37 | 38 | namespace relocalization { 39 | 40 | class Converter { 41 | public: 42 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 43 | 44 | static SE3 toSE3Quat(const cv::Mat &cvT); 45 | 46 | static cv::Mat toCvMat(const SE3 &SE3); 47 | static cv::Mat toCvMat(const Sim3 &sim3); 48 | static cv::Mat toCvMat(const Eigen::Matrix &m); 49 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 50 | static cv::Mat toCvMat(const Eigen::Matrix &m); 51 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 52 | 53 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 54 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 55 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 56 | 57 | static std::vector toQuaternion(const cv::Mat &M); 58 | }; 59 | 60 | } 61 | 62 | } 63 | 64 | #endif // DSL_CONVERTER_H 65 | -------------------------------------------------------------------------------- /include/relocalization/desc_map_viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/7/20. 28 | // 29 | 30 | #ifndef DSL_DESC_MAP_VIEWER_H 31 | #define DSL_DESC_MAP_VIEWER_H 32 | 33 | #include "map_render/gui.h" 34 | #include "map_render/core/global_model.h" 35 | 36 | namespace dsl::relocalization { 37 | 38 | // TODO: 39 | class DescMapViewer : public GUI { 40 | public: 41 | DescMapViewer(); 42 | void Run(); 43 | void DrawAll(); 44 | bool GetPauseFlag(); 45 | bool GetStepFlag(); 46 | bool CheckFinish(); 47 | bool pause_flag = false, step_flag = false; 48 | bool finish_flag = false; 49 | GlobalModel *global_model; 50 | std::mutex gui_mutex; 51 | }; 52 | 53 | } // namespace dsl::relocalization 54 | 55 | 56 | 57 | #endif // DSL_DESC_MAP_VIEWER_H 58 | -------------------------------------------------------------------------------- /include/relocalization/reloc_optimization/optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/14/20. 28 | // 29 | 30 | #ifndef DSL_OPTIMIZER_H 31 | #define DSL_OPTIMIZER_H 32 | 33 | #include 34 | #include "sophus/se3.hpp" 35 | #include "relocalization/relocalization_struct.h" 36 | 37 | namespace dsl::relocalization { 38 | 39 | // NOTE: this is only for relocalization 40 | class Optimizer { 41 | public: 42 | static int PoseOptimization(Frame *pFrame); 43 | }; 44 | 45 | } // namespace dsl::relocalization 46 | 47 | #endif // DSL_OPTIMIZER_H 48 | -------------------------------------------------------------------------------- /include/relocalization/reloc_optimization/reprojection_functor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/14/20. 28 | // 29 | 30 | #ifndef DSL_REPROJECTION_FUNCTOR_H 31 | #define DSL_REPROJECTION_FUNCTOR_H 32 | 33 | #include 34 | #include "dsl_common.h" 35 | 36 | namespace dsl::relocalization { 37 | 38 | class ReprojectionFunctor : public ceres::SizedCostFunction<2, 6> { 39 | public: 40 | ReprojectionFunctor() {} 41 | ReprojectionFunctor(ReprojectionFunctor &functor); 42 | virtual bool Evaluate(double const *const *parameters, double *residuals, 43 | double **jacobians) const; 44 | void Check(double const *const *parameters); 45 | 46 | Eigen::Vector2d CamProject(const Eigen::Vector3d &trans_xyz) const { 47 | Eigen::Vector2d proj; 48 | proj.x() = trans_xyz.x() / trans_xyz.z(); 49 | proj.y() = trans_xyz.y() / trans_xyz.z(); 50 | 51 | Eigen::Vector2d res; 52 | res.x() = proj.x() * fx + cx; 53 | res.y() = proj.y() * fy + cy; 54 | return res; 55 | } 56 | 57 | void SetInformation(const Eigen::Matrix2d &_information) { 58 | information = _information; 59 | Eigen::LLT llt_of_information(information); 60 | sqrt_information = llt_of_information.matrixL().transpose(); 61 | } 62 | 63 | void ComputeError(const double *const *parameters); 64 | double GetChi2() { return error_.dot(information * error_); }; 65 | Eigen::Vector2d GetError() { return error_; } 66 | 67 | void SetFixed(bool fixed, const double *const *parameters) { 68 | fixed_ = fixed; 69 | if (parameters) ComputeError(parameters); 70 | } 71 | 72 | Eigen::Vector3d Xw; 73 | Eigen::Vector2d measurement; 74 | double fx, fy, cx, cy; 75 | Eigen::Matrix2d information = Eigen::Matrix2d::Identity(); 76 | Eigen::Matrix2d sqrt_information = Eigen::Matrix2d::Identity(); 77 | 78 | private: 79 | Eigen::Vector2d error_; 80 | bool fixed_ = false; 81 | }; 82 | 83 | } // namespace dsl::relocalization 84 | 85 | #endif // DSL_REPROJECTION_FUNCTOR_H 86 | -------------------------------------------------------------------------------- /include/relocalization/reloc_optimization/surfel_reprojection_functor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 8/25/20. 28 | // 29 | 30 | #ifndef DSL_SURFEL_REPROJECTION_FUNCTOR_H 31 | #define DSL_SURFEL_REPROJECTION_FUNCTOR_H 32 | 33 | #include 34 | #include "dsl_common.h" 35 | 36 | namespace dsl::relocalization { 37 | 38 | /// \brief on unit image plane 39 | class SurfelReprojectionFunctor : public ceres::SizedCostFunction<2, 6, 6> { 40 | public: 41 | SurfelReprojectionFunctor(const Eigen::Vector3d &_x0, 42 | const Eigen::Vector3d &_x1, 43 | const Eigen::Vector3d &_n_w, 44 | double _d_w); 45 | virtual bool Evaluate(double const *const *parameters, double *residuals, 46 | double **jacobians) const; 47 | void Check(double const *const *parameters); 48 | 49 | void ComputeError(const double *const *parameters); 50 | 51 | Eigen::Vector3d x0_bar, x1_bar; 52 | Eigen::Vector3d n_w; 53 | double d_w; 54 | double error; 55 | }; 56 | 57 | } // namespace dsl::relocalization 58 | 59 | #endif // DSL_SURFEL_REPROJECTION_FUNCTOR_H 60 | -------------------------------------------------------------------------------- /include/relocalization/relocalization_config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/10/20. 28 | // 29 | 30 | #ifndef DSL_RELOCALIZATION_CONFIG_H 31 | #define DSL_RELOCALIZATION_CONFIG_H 32 | 33 | #include "map_render/yaml_loader.h" 34 | 35 | namespace dsl::relocalization { 36 | 37 | class DescMapConfig { 38 | public: 39 | bool fcheck = false; 40 | bool grid_filter = false; 41 | bool fcheck_cov_kf = false; 42 | bool active_search = false; 43 | bool active_search_3d2d = false; 44 | 45 | int levelsup = 2; 46 | bool enhanced_points = false; 47 | bool use_vlad = true; 48 | bool use_enhanced_points_in_bf_search = false; 49 | bool use_enhanced_points_in_active_search = false; 50 | bool use_wide_enhanced_points_in_active_search = false; 51 | bool use_all_points_in_active_search = false; 52 | bool use_good_neighbor_points_in_active_search = true; 53 | bool use_covisible_kf_points = true; 54 | bool use_covisible_kf_points_dbg = true; 55 | int min_nGood = 50; 56 | int min_nMatched = 50; 57 | int max_num_kf = 3; 58 | 59 | double translation_threshold = 0.3; 60 | double eval_translation_threshold = 0.3; 61 | 62 | std::string Print(); 63 | }; 64 | 65 | class RelocalizationConfig : public YamlLoader { 66 | public: 67 | RelocalizationConfig(std::string filename); 68 | int max_num_features = 1000; 69 | int gt_projection = 0; 70 | int best_cliques = 3; 71 | int min_common_weight = 2; 72 | int num_features = 500; 73 | double scale_factor = 1.2; 74 | int num_levels = 5; 75 | 76 | bool use_superpoint = false; 77 | std::string superpoint_path; 78 | std::string pytorch_device = "CUDA"; 79 | float superpoint_conf_thresh = 0.007; 80 | 81 | std::string database_dataset_path; 82 | std::string database_map_path; 83 | std::string database_voc_path; 84 | 85 | std::string query_dataset_path; 86 | std::string query_gt_traj_path; 87 | 88 | std::string database_vlad_path; 89 | std::string query_vlad_path; 90 | 91 | DescMapConfig desc_map_config; 92 | 93 | std::string Print(); 94 | }; 95 | 96 | } // namespace dsl::relocalization 97 | 98 | #endif // DSL_RELOCALIZATION_CONFIG_H 99 | -------------------------------------------------------------------------------- /include/relocalization/relocalization_struct.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/2/20. 28 | // 29 | 30 | #ifndef DSL_RELOCALIZATION_STRUCT_H 31 | #define DSL_RELOCALIZATION_STRUCT_H 32 | 33 | #include "relocalization/struct/map_point.h" 34 | #include "relocalization/struct/frame.h" 35 | #include "relocalization/struct/virtual_frame.h" 36 | #include "relocalization/struct/clique.h" 37 | 38 | /** 39 | * Returns a random int in the range [min..max] 40 | * @param min 41 | * @param max 42 | * @return random int in [min..max] 43 | */ 44 | inline static int RandomInt(int min, int max){ 45 | int d = max - min + 1; 46 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 47 | } 48 | 49 | #endif // DSL_RELOCALIZATION_STRUCT_H 50 | -------------------------------------------------------------------------------- /include/relocalization/sp_matcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 8/8/20. 28 | // 29 | 30 | #ifndef DSL_SP_MATCHER_H 31 | #define DSL_SP_MATCHER_H 32 | 33 | #include "relocalization/feature_matcher.h" 34 | 35 | namespace dsl::relocalization { 36 | 37 | class SPMatcher : public ORBmatcher { 38 | public: 39 | SPMatcher(float nnratio = 0.6); 40 | 41 | static float DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 42 | int SearchByProjection(Frame &F, const std::vector &vpMapPoints, const float th = 3) override; 43 | // int SearchByProjection(Frame &F, 44 | // const std::vector &vpMapPoints, 45 | // std::vector &vpOutMPs, 46 | // const float th = 3); 47 | 48 | // Project MapPoints seen in KeyFrame into the Frame and search matches. 49 | // Used in relocalisation (Tracking) 50 | // int SearchByProjection(Frame &CurrentFrame, Frame *pKF, const std::set &sAlreadyFound, 51 | // const float th, const int ORBdist); 52 | 53 | int SearchByProjection(Frame &CurrentFrame, 54 | const std::vector &vpMapPoints, 55 | const std::set &sAlreadyFound, 56 | const float th, 57 | const float ORBdist) override; 58 | 59 | int SearchByBruteForce(const std::vector &vpMPs, 60 | Frame &F, 61 | std::vector &vpMapPointMatches, 62 | bool useEnhancedMPs = true) override; 63 | int SearchByBruteForce(Frame *pKF, Frame &F, std::vector &vpMapPointMatches, bool useEnhancedMPs = true) override; 64 | // int SearchByBruteForce(Frame *pKF, Frame &F, std::vector> &corres_id, bool useEnhancedMPs); 65 | 66 | int Fuse(Frame *pF, const std::vector &vpMapPoints, const float th = 3.0) override; 67 | int FuseNew(Frame *pF, std::vector &vpMapPoints, const float th = 3.0) override; 68 | 69 | public: 70 | static const float TH_LOW; 71 | static const float TH_HIGH; 72 | static const int HISTO_LENGTH; 73 | }; 74 | 75 | } // namespace 76 | 77 | #endif // DSL_SP_MATCHER_H 78 | -------------------------------------------------------------------------------- /include/relocalization/struct/clique.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/10/20. 28 | // 29 | 30 | #ifndef DSL_CLIQUE_H 31 | #define DSL_CLIQUE_H 32 | 33 | #include "relocalization/relocalization_struct.h" 34 | #include "relocalization/vocabulary_binary.h" 35 | #include "relocalization/boost_archiver.h" 36 | 37 | namespace dsl::relocalization { 38 | 39 | class Clique { 40 | public: 41 | std::vector mvNId; 42 | // Bag of Words Vector structures. 43 | DBoW2::BowVector mBowVec; 44 | DBoW2::FeatureVector mFeatVec; 45 | int mnRelocWords = 0; 46 | int mnRelocQuery = -1; 47 | float mRelocScore = 0; 48 | 49 | cv::Mat mMeanWorldPos; 50 | 51 | // ORB descriptor, each row associated to a keypoint. 52 | cv::Mat mDescriptors; 53 | private: 54 | friend class boost::serialization::access; 55 | template 56 | void serialize(Archive &ar, const unsigned int version); 57 | }; 58 | 59 | class CliqueMap { 60 | public: 61 | CliqueMap(OrbVocabularyBinary *voc); 62 | std::vector mvCliques; 63 | std::vector> mCliques; 64 | std::map mVertIdToNId; 65 | std::map mNIdToVertId; 66 | std::set msNId; 67 | OrbVocabularyBinary *mpVoc; 68 | std::vector > mvInvertedFile; 69 | void ComputeCliques(std::unordered_map> &all_map_points, 70 | int min_common_weight = 1); 71 | static void FindMaxCliqueIds(std::unordered_map> &all_map_points, 72 | unsigned long mp_nid, 73 | std::vector &max_clique_nids, 74 | int min_common_weight); 75 | void Save(std::string filename); 76 | bool Load(std::string filename); 77 | void BuildInvertedFile(); 78 | std::vector DetectRelocalizationCandidates(Frame *pF); 79 | 80 | private: 81 | friend class boost::serialization::access; 82 | template 83 | void serialize(Archive &ar, const unsigned int version); 84 | }; 85 | 86 | } // namespace dsl::relocalization 87 | 88 | 89 | #endif // DSL_CLIQUE_H 90 | -------------------------------------------------------------------------------- /include/relocalization/struct/key_frame_database.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/13/20. 28 | // 29 | 30 | #ifndef DSL_KEY_FRAME_DATABASE_H 31 | #define DSL_KEY_FRAME_DATABASE_H 32 | 33 | #include "relocalization/relocalization_struct.h" 34 | #include "relocalization/vocabulary_binary.h" 35 | 36 | namespace dsl::relocalization { 37 | 38 | class KeyFrameDatabase { 39 | public: 40 | KeyFrameDatabase(OrbVocabularyBinary *voc); 41 | void Add(Frame *pF); 42 | void Erase(Frame *pF); 43 | void Clear(); 44 | std::vector DetectRelocalizationCandidates(Frame *pF); 45 | 46 | void BuildInvertedFile(const std::map> &all_keyframes); 47 | 48 | KeyFrameDatabase() {} 49 | void SetORBvocabulary(OrbVocabularyBinary *porbv); 50 | protected: 51 | OrbVocabularyBinary *mpVoc; 52 | 53 | // Inverted file 54 | std::vector> mvInvertedFile; 55 | 56 | }; 57 | 58 | } // namespace dsl::relocalization 59 | 60 | #endif // DSL_KEY_FRAME_DATABASE_H 61 | -------------------------------------------------------------------------------- /include/relocalization/struct/motion_estimator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/28/20. 28 | // 29 | 30 | #ifndef DSL_MOTION_ESTIMATOR_H 31 | #define DSL_MOTION_ESTIMATOR_H 32 | 33 | #include "dsl_common.h" 34 | 35 | namespace dsl::relocalization { 36 | 37 | // Adapted from VINS-Mono 38 | 39 | class MotionEstimator { 40 | public: 41 | static bool solveRelativeRT(const std::vector> &corres, 42 | Eigen::Matrix3d &R, 43 | Eigen::Vector3d &T, 44 | cv::Mat &mask); 45 | }; 46 | 47 | } // dsl::relocalization 48 | 49 | #endif // DSL_MOTION_ESTIMATOR_H 50 | -------------------------------------------------------------------------------- /include/relocalization/struct/net_vlad_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/23/20. 28 | // 29 | 30 | #ifndef DSL_NET_VLAD_UTILS_H 31 | #define DSL_NET_VLAD_UTILS_H 32 | 33 | #include 34 | #include 35 | 36 | namespace dsl::relocalization { 37 | 38 | class NetVladUtils { 39 | public: 40 | static cv::Mat ReadVLADBinary(const std::string &file_name) { 41 | const int height = 1; 42 | const int width = 4096; 43 | // cout << file_name << endl; 44 | // allocate buffer 45 | const int buffer_size_ = sizeof(float) * height * width; 46 | char buffer_[buffer_size_]; 47 | // open filestream && read buffer 48 | std::ifstream fs_bin_(file_name.c_str(), std::ios::binary); 49 | fs_bin_.read(buffer_, buffer_size_); 50 | fs_bin_.close(); 51 | // construct depth map && return 52 | cv::Mat out = cv::Mat(cv::Size(width, height), CV_32FC1); 53 | std::memcpy(out.data, buffer_, buffer_size_); 54 | return out.clone(); 55 | } 56 | }; 57 | 58 | } 59 | 60 | #endif // DSL_NET_VLAD_UTILS_H 61 | -------------------------------------------------------------------------------- /include/relocalization/struct/virtual_frame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/6/20. 28 | // 29 | 30 | #ifndef DSL_VIRTUAL_FRAME_H 31 | #define DSL_VIRTUAL_FRAME_H 32 | 33 | #include 34 | #include "util/util_common.h" 35 | #include "relocalization/boost_archiver.h" 36 | 37 | namespace dsl::relocalization { 38 | 39 | class VirtualFrame { 40 | public: 41 | }; 42 | 43 | } // namespace dsl::relocalization 44 | 45 | #endif // DSL_VIRTUAL_FRAME_H 46 | -------------------------------------------------------------------------------- /include/relocalization/struct/vlad_database.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/24/20. 28 | // 29 | 30 | #ifndef DSL_VLAD_DATABASE_H 31 | #define DSL_VLAD_DATABASE_H 32 | 33 | #include "boost/filesystem.hpp" 34 | #include "relocalization/struct/net_vlad_utils.h" 35 | #include "relocalization/struct/frame.h" 36 | 37 | namespace dsl::relocalization { 38 | 39 | class VLADBinaryLoader{ 40 | public: 41 | VLADBinaryLoader() {}; 42 | VLADBinaryLoader(const std::string & path); 43 | cv::Mat Load(const std::string& timestamp); 44 | 45 | boost::filesystem::path path_; 46 | }; 47 | 48 | struct VLADKnnResult { 49 | std::vector frames; 50 | std::vector si; 51 | }; 52 | 53 | class VLADDatabase { 54 | public: 55 | VLADDatabase(const std::string &vlad_path, const std::vector &frames); 56 | void SetQueryDatabase(const std::string &query_vlad_path); 57 | VLADKnnResult QueryKnn(const cv::Mat &vlad_query, int knn = 20); 58 | std::vector DetectRelocalizationCandidates(Frame *pF); 59 | cv::Mat vlad_descs; 60 | std::unique_ptr flann_vlad_descs_ptr; 61 | VLADBinaryLoader vlad_binary_loader; 62 | VLADBinaryLoader query_vlad_binary_loader; 63 | 64 | VLADKnnResult result_knn; 65 | 66 | std::vector frames_; 67 | }; 68 | 69 | } // namespace dsl::relocalization 70 | 71 | #endif // DSL_VLAD_DATABASE_H 72 | -------------------------------------------------------------------------------- /include/relocalization/visualization/drawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/10/20. 28 | // 29 | 30 | #ifndef DSL_DRAWER_H 31 | #define DSL_DRAWER_H 32 | 33 | #include "relocalization/relocalization_struct.h" 34 | 35 | namespace dsl::relocalization { 36 | 37 | void AddTextToImage(const std::string &s, cv::Mat &im, const int r, const int g, const int b); 38 | 39 | void ConvertMatchesToKpts(Frame *const pF, 40 | const std::vector &vpMapPointMatches, 41 | cv::Mat &Tcw, 42 | std::vector &kpts); 43 | 44 | void ConvertMatches(Frame *const pF, 45 | Frame *const pKF, 46 | const std::vector &vpMapPointMatches, 47 | std::vector &kpts1, 48 | std::vector &kpts2, 49 | std::vector &matches1to2); 50 | 51 | void DrawMatchesPair(const std::vector> &kpt_pairs, 52 | const cv::Mat &img1, 53 | const cv::Mat &img2, 54 | cv::Mat &outImg); 55 | 56 | void DrawMapPointMatches(const Frame *const pF, 57 | const std::vector &vpMapPointMatches, 58 | const cv::Mat &inImg, 59 | cv::Mat &outImg); 60 | void DrawImagePair(const cv::Mat &img1, const cv::Mat &img2, cv::Mat &outImg); 61 | cv::Mat MakeCanvas(std::vector &vecMat, int windowHeight, int nRows); 62 | 63 | } // namespace dsl::relocalization 64 | 65 | 66 | #endif // DSL_DRAWER_H 67 | -------------------------------------------------------------------------------- /include/tool/cv_helper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/11/19. 28 | // 29 | 30 | #ifndef DSL_CV_HELPER_H_ 31 | #define DSL_CV_HELPER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace cv_helper { 38 | 39 | void DrawPairs(const cv::Mat &img1, const cv::Mat &img2, 40 | const std::vector &pvec1, 41 | const std::vector &pvec2, cv::Mat &stereo_img, 42 | bool draw_lines = true, int skip_count = 1, 43 | cv::Scalar color1 = cv::Scalar(255, 0, 0), 44 | cv::Scalar color2 = cv::Scalar(0, 255, 0)); 45 | 46 | void VisualizePairs(const cv::Mat &img1, const cv::Mat &img2, 47 | const std::vector &pvec1, 48 | const std::vector &pvec2, 49 | bool draw_lines = true, int skip_count = 1, 50 | cv::Scalar color1 = cv::Scalar(255, 0, 0), 51 | cv::Scalar color2 = cv::Scalar(0, 255, 0)); 52 | 53 | } // namespace cv_helper 54 | 55 | #endif // DSL_CV_HELPER_H_ 56 | -------------------------------------------------------------------------------- /include/tool/dataset_converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 6/30/20. 28 | // 29 | 30 | #ifndef DSL_DATASET_CONVERTER_H 31 | #define DSL_DATASET_CONVERTER_H 32 | 33 | #include 34 | #include "util/frame_shell.h" 35 | 36 | namespace dsl { 37 | 38 | struct FrameShellWithFn : FrameShell { 39 | std::string filename; 40 | }; 41 | 42 | // Format 'timestamp tx ty tz qx qy qz qw' 43 | std::vector LoadTum(std::string pose_path); 44 | 45 | std::string ToTum(std::string time, const SE3 &se3); 46 | 47 | } // namespace dsl 48 | 49 | #endif // DSL_DATASET_CONVERTER_H 50 | -------------------------------------------------------------------------------- /include/tool/euroc_reader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/19/19. 28 | // 29 | 30 | #ifndef DSL_EUROC_READER_H_ 31 | #define DSL_EUROC_READER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace dsl { 38 | 39 | class EurocReader { 40 | public: 41 | EurocReader(std::string _path, std::string config_filename="config.yaml"); 42 | 43 | void ReadImageAndDist(int idx); 44 | void ReadImage(int idx); 45 | int ReadImage(const std::string& filename); 46 | void ReadDepth(int idx); 47 | 48 | std::string path; 49 | std::string image_path; 50 | std::string depth_path; 51 | std::string config_path; 52 | std::vector filenames; 53 | std::string file_ext = ""; 54 | 55 | cv::Mat gray_image; 56 | cv::Mat dist_image; 57 | 58 | Eigen::Matrix3f K; 59 | int w, h; 60 | 61 | }; 62 | 63 | } 64 | 65 | #endif //DSL_EUROC_READER_H_ 66 | -------------------------------------------------------------------------------- /include/tool/habitat_sim_reader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 2/27/20. 28 | // 29 | 30 | #ifndef DSL_HABITAT_SIM_READER_H_ 31 | #define DSL_HABITAT_SIM_READER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace dsl { 42 | 43 | class HabitatSimReader { 44 | public: 45 | typedef Sophus::SE3 SE3; 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | enum CameraName { left = 0, right }; 48 | 49 | HabitatSimReader(std::string _path, CameraName camera_name = left); 50 | 51 | std::string path, image_path, depth_path, pose_path; 52 | cv::Mat color_image; 53 | cv::Mat gray_image; 54 | cv::Mat depth_image; 55 | SE3 cam_pose; 56 | 57 | const std::vector cameras = {"left", "right"}; 58 | int w = 640, h = 480; 59 | std::vector all_poses; 60 | Eigen::Matrix K; 61 | 62 | bool ReadImageAndDepth(int index); 63 | bool ReadImage(int index); 64 | }; 65 | 66 | } 67 | 68 | #endif // DSL_HABITAT_SIM_READER_H_ 69 | -------------------------------------------------------------------------------- /include/tool/new_tsukuba_reader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/8/19. 28 | // 29 | 30 | #ifndef DSL_NEW_TSUKUBA_READER_H_ 31 | #define DSL_NEW_TSUKUBA_READER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | namespace dsl { 42 | 43 | class NewTsukubaReader { 44 | public: 45 | typedef Sophus::SE3 SE3; 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | enum SceneName { daylight = 0, flashlight, fluorescent, lamps }; 48 | enum CameraName { left = 0, right }; 49 | 50 | NewTsukubaReader(std::string _path, SceneName scene_name = fluorescent, 51 | CameraName camera_name = left); 52 | 53 | std::string path, image_path, depth_path, pose_path; 54 | cv::Mat color_img; 55 | cv::Mat depth_img; 56 | SE3 cam_pose; 57 | const std::vector scenes = {"daylight", "flashlight", 58 | "fluorescent", "lamps"}; 59 | 60 | const std::vector cameras = {"left", "right"}; 61 | const std::vector LR = {"L", "R"}; 62 | const int w = 640, h = 480; 63 | std::vector all_poses; 64 | Eigen::Matrix K; 65 | 66 | bool ReadImageAndDepth(int index); 67 | }; 68 | 69 | } // namespace dsl 70 | 71 | #endif // DSL_NEW_TSUKUBA_READER_H_ 72 | -------------------------------------------------------------------------------- /include/util/frame_shell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/6/19. 28 | // 29 | 30 | #ifndef DSL_FRAME_SHELL_H_ 31 | #define DSL_FRAME_SHELL_H_ 32 | 33 | #include "num_type.h" 34 | 35 | namespace dsl { 36 | 37 | struct FrameShell { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | int id; 41 | int incoming_id; 42 | double timestamp; 43 | 44 | // set once after tracking 45 | SE3 cam_to_ref; 46 | FrameShell* tracking_ref; 47 | 48 | SE3 cam_to_world; 49 | AffLight aff_light; 50 | bool pose_valid; 51 | 52 | // statistics 53 | int statistics_outlier_res; 54 | int statistics_good_res; 55 | int marginalized_at; 56 | double moved_by_opt; 57 | 58 | FrameShell() { 59 | id = 0; 60 | pose_valid = true; 61 | cam_to_world = SE3(); 62 | timestamp = 0; 63 | marginalized_at = -1; 64 | moved_by_opt = statistics_outlier_res=statistics_good_res=0;; 65 | tracking_ref=0; 66 | cam_to_ref = SE3(); 67 | } 68 | 69 | }; 70 | 71 | } 72 | 73 | #endif // DSL_FRAME_SHELL_H_ 74 | -------------------------------------------------------------------------------- /include/util/global_calib.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/6/19. 28 | // 29 | 30 | #ifndef DSL_GLOBALCALIB_H_ 31 | #define DSL_GLOBALCALIB_H_ 32 | 33 | #include "settings.h" 34 | #include "num_type.h" 35 | #include 36 | #include 37 | #include 38 | 39 | namespace dsl { 40 | 41 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 42 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 43 | 44 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], cxiG[PYR_LEVELS], 45 | cyiG[PYR_LEVELS]; 46 | 47 | extern Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 48 | 49 | extern cv::Mat maskG[PYR_LEVELS]; 50 | 51 | extern float wM3G; 52 | extern float hM3G; 53 | 54 | extern float xiG; 55 | 56 | extern float max_half_fovG; 57 | 58 | void SetGlobalCalib(int w, int h, const Eigen::Matrix3f &K); 59 | void SetGlobalCalib(int w, int h, const Eigen::Matrix3f &K, float xi); 60 | void SetGlobalMask(std::string mask_file); 61 | 62 | namespace relocalization { 63 | // WARNING: config for relocalization only 64 | extern bool use_superpoint; 65 | } 66 | 67 | } // namespace dsl 68 | 69 | #endif // DSL_GLOBALCALIB_H_ 70 | -------------------------------------------------------------------------------- /include/util/graph_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/9/20. 28 | // 29 | 30 | #ifndef DSL_GRAPH_UTILS_H 31 | #define DSL_GRAPH_UTILS_H 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace dsl { 38 | 39 | class GraphUtils { 40 | typedef boost::adjacency_list_traits 42 | Traits; 43 | typedef boost::adjacency_list< 44 | boost::vecS, boost::vecS, boost::directedS, 45 | boost::property< 46 | boost::vertex_name_t, std::string, 47 | boost::property< 48 | boost::vertex_index_t, long, 49 | boost::property< 50 | boost::vertex_color_t, boost::default_color_type, 51 | boost::property< 52 | boost::vertex_distance_t, long, 53 | boost::property > > > >, 55 | 56 | boost::property< 57 | boost::edge_capacity_t, long, 58 | boost::property > > > 61 | Graph; 62 | 63 | typedef 64 | typename boost::graph_traits::vertex_descriptor vertex_descriptor; 65 | typedef typename boost::graph_traits::edge_descriptor edge_descriptor; 66 | using VertexColors = boost::property_map::type; 67 | 68 | public: 69 | void SetGraph(const std::map, long> &edge_values, long num_vertex); 70 | void CalcSourceSet(long source_idx, long target_idx, std::vector &source_set); 71 | bool IsInSourceSet(vertex_descriptor v) const; 72 | Graph g; 73 | std::vector verts; 74 | 75 | VertexColors colors; 76 | boost::default_color_type src_color; 77 | }; 78 | 79 | } // namespace dsl 80 | 81 | #endif // DSL_GRAPH_UTILS_H 82 | -------------------------------------------------------------------------------- /include/util/image_and_exposure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/6/19. 28 | // 29 | 30 | #ifndef DSL_IMAGEANDEXPOSURE_H_ 31 | #define DSL_IMAGEANDEXPOSURE_H_ 32 | 33 | #include "dsl_common.h" 34 | 35 | namespace dsl { 36 | 37 | struct ImageAndExposure { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | std::vector image; // irradiance. between 0 and 256 41 | int w, h; // width and height; 42 | double timestamp; 43 | float exposure_time; // exposure time in ms. 44 | inline ImageAndExposure(int _w, int _h, double _timestamp = 0) 45 | : w(_w), h(_h), timestamp(_timestamp) { 46 | image = std::vector(w * h); 47 | exposure_time = 1; 48 | } 49 | inline ~ImageAndExposure() {} 50 | 51 | inline void CopyMetaTo(ImageAndExposure &other) { 52 | other.exposure_time = exposure_time; 53 | } 54 | 55 | inline std::unique_ptr GetDeepCopy() { 56 | std::unique_ptr img = 57 | std::make_unique(w, h, timestamp); 58 | img->exposure_time = exposure_time; 59 | std::copy(image.begin(), image.end(), std::back_inserter(img->image)); 60 | return img; 61 | } 62 | }; 63 | 64 | } // namespace dsl 65 | 66 | #endif // DSL_IMAGEANDEXPOSURE_H_ 67 | -------------------------------------------------------------------------------- /include/util/util_common.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/7/19. 28 | // 29 | 30 | #ifndef DSL_UTIL_COMMON_H_ 31 | #define DSL_UTIL_COMMON_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #endif // DSL_UTIL_COMMON_H_ 41 | -------------------------------------------------------------------------------- /scripts/extract_images_timestamp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Copyright 2016 Massachusetts Institute of Technology 5 | 6 | """Extract images from a rosbag. 7 | """ 8 | 9 | import os 10 | import argparse 11 | 12 | import cv2 13 | 14 | import rosbag 15 | import numpy as np 16 | from sensor_msgs.msg import Image 17 | from cv_bridge import CvBridge 18 | 19 | def main(): 20 | """Extract a folder of images from a rosbag. 21 | """ 22 | parser = argparse.ArgumentParser(description="Extract images from a ROS bag.") 23 | parser.add_argument("--bag_file", help="Input ROS bag.") 24 | parser.add_argument("--output_dir", help="Output directory.") 25 | parser.add_argument("--image_topic", help="Image topic.") 26 | 27 | args = parser.parse_args() 28 | 29 | print "Extract images from %s on topic %s into %s" % (args.bag_file, 30 | args.image_topic, args.output_dir) 31 | 32 | bag = rosbag.Bag(args.bag_file, "r") 33 | bridge = CvBridge() 34 | count = 0 35 | for topic, msg, t in bag.read_messages(topics=[args.image_topic]): 36 | 37 | # import IPython 38 | # IPython.embed() 39 | # break 40 | 41 | if "Compressed" in str(type(msg)): 42 | np_arr = np.fromstring(msg.data, np.uint8) 43 | cv_img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) 44 | else: 45 | cv_img = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") 46 | 47 | # import IPython 48 | # IPython.embed() 49 | # break 50 | # cv_img = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") 51 | 52 | # output_img_path = str(os.path.join(args.output_dir, str(msg.header.stamp.secs) + "." + str(msg.header.stamp.nsecs).zfill(9) +".jpg")) 53 | output_img_path = str(os.path.join(args.output_dir, str(msg.header.stamp.to_nsec()) +".png")) 54 | cv2.imwrite(output_img_path, cv_img) 55 | print "Wrote image %i %s" % (count, output_img_path) 56 | 57 | count += 1 58 | 59 | bag.close() 60 | 61 | return 62 | 63 | if __name__ == '__main__': 64 | main() 65 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(util) 2 | add_subdirectory(map_render) 3 | add_subdirectory(tool) 4 | add_subdirectory(optimization) 5 | # add_subdirectory(relocalization) 6 | add_subdirectory(full_system) 7 | 8 | set(DSL_DEPENDENCIES 9 | dsl_full_system 10 | # dsl_relocalization 11 | dsl_cost_functor 12 | dsl_util 13 | dsl_tool 14 | dsl_map_render 15 | ${ThreeParty_LIBS} 16 | gmock gtest gtest_main 17 | ) 18 | 19 | set(DSL_LIBRARIES 20 | dsl_full_system 21 | # dsl_relocalization 22 | dsl_cost_functor 23 | dsl_util 24 | dsl_tool 25 | dsl_map_render 26 | ${ThreeParty_LIBS} 27 | ${Google_LIBRARIES} 28 | ${Ext_LIBRARIES} 29 | ) 30 | 31 | add_executable(dsl_main dsl_main.cc) 32 | add_dependencies(dsl_main ${DSL_DEPENDENCIES}) 33 | target_link_libraries(dsl_main 34 | ${DSL_LIBRARIES} 35 | ) 36 | 37 | # add_executable(dsl_full_main dsl_full_main.cc) 38 | # add_dependencies(dsl_full_main ${DSL_DEPENDENCIES}) 39 | # target_link_libraries(dsl_full_main 40 | # ${DSL_LIBRARIES} 41 | # ) 42 | 43 | # add_executable(colorizer colorizer.cc) 44 | # add_dependencies(colorizer ${DSL_DEPENDENCIES}) 45 | # target_link_libraries(colorizer 46 | # ${DSL_LIBRARIES} 47 | # ) 48 | 49 | # add_executable(depth_generator depth_generator.cc) 50 | # add_dependencies(depth_generator ${DSL_DEPENDENCIES}) 51 | # target_link_libraries(depth_generator 52 | # ${DSL_LIBRARIES} 53 | # ) 54 | 55 | # add_executable(depth_generator_from_map depth_generator_from_map.cc) 56 | # add_dependencies(depth_generator_from_map ${DSL_DEPENDENCIES}) 57 | # target_link_libraries(depth_generator_from_map 58 | # ${DSL_LIBRARIES} 59 | # ) 60 | 61 | # add_executable(desc_map_builder desc_map_builder.cc) 62 | # add_dependencies(desc_map_builder ${DSL_DEPENDENCIES}) 63 | # target_link_libraries(desc_map_builder 64 | # ${DSL_LIBRARIES} 65 | # ) 66 | 67 | # add_executable(desc_map_clique desc_map_clique.cc) 68 | # add_dependencies(desc_map_clique ${DSL_DEPENDENCIES}) 69 | # target_link_libraries(desc_map_clique 70 | # ${DSL_LIBRARIES} 71 | # ) 72 | 73 | # add_executable(desc_map_keyframe desc_map_keyframe.cc) 74 | # add_dependencies(desc_map_keyframe ${DSL_DEPENDENCIES}) 75 | # target_link_libraries(desc_map_keyframe 76 | # ${DSL_LIBRARIES} 77 | # ) -------------------------------------------------------------------------------- /src/full_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsl_full_system 2 | full_system.cc 3 | full_system_tracing.cc 4 | full_system_tracking.cc 5 | full_system_activate_points.cc 6 | full_system_optimize.cc 7 | full_system_marginalize.cc 8 | optimization_call_back.cc 9 | pixel_selector.cc 10 | distance_initializer.cc 11 | coarse_tracker.cc 12 | coarse_distance_map.cc 13 | ef_struct.cc 14 | hessian_blocks.cc 15 | immature_point.cc 16 | residual.cc 17 | ) -------------------------------------------------------------------------------- /src/full_system/residual.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/13/19. 28 | // 29 | 30 | #include "full_system/residual.h" 31 | #include "full_system/hessian_blocks.h" 32 | #include 33 | #include "optimization/homo_cost_functor.h" 34 | 35 | namespace dsl { 36 | 37 | void PointFrameResidual::SetResPlane() { 38 | if (point->valid_plane) { 39 | valid_plane = true; 40 | plane_coeff = point->plane_coeff; 41 | } 42 | } 43 | 44 | int PointFrameResidual::instanceCounter = 0; 45 | 46 | PointFrameResidual::PointFrameResidual(PointHessian* _point, 47 | FrameHessian* _host, 48 | FrameHessian* _target, 49 | CalibHessian* _HCalib) 50 | : point(_point), host(_host), target(_target) { 51 | ef_residual = nullptr; 52 | ResetOOB(); 53 | ++instanceCounter; 54 | 55 | is_new = true; 56 | 57 | SetResPlane(); 58 | 59 | cost_function = std::make_unique(this, *_HCalib, settingBaseline); 60 | } 61 | 62 | void PointFrameResidual::ResetOOB() { 63 | new_res_energy = res_energy = 0; 64 | new_res_raw = res_raw = 0; 65 | new_res_state = ResState::OUTLIER; 66 | 67 | SetState(ResState::IN); 68 | } 69 | 70 | double PointFrameResidual::Linearize(dsl::CalibHessian& HCalib) { 71 | // FIXME: Linearize 72 | } 73 | 74 | void PointFrameResidual::ApplyRes(bool copy_jacobians) { 75 | if (copy_jacobians) { 76 | if (res_state == ResState::OOB) { 77 | assert(!ef_residual->is_active_and_is_good_new); 78 | return; // can never go back from OOB 79 | } 80 | if (new_res_state == ResState::IN) { 81 | ef_residual->is_active_and_is_good_new = true; 82 | ef_residual->TakeDataF(); 83 | } else { 84 | ef_residual->is_active_and_is_good_new = false; 85 | } 86 | } 87 | 88 | SetState(new_res_state); 89 | res_energy = new_res_energy; 90 | res_raw = new_res_raw; 91 | } 92 | 93 | } // namespace dsl -------------------------------------------------------------------------------- /src/map_render/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsl_map_render 2 | gui.cc 3 | pangolin_utils.cc 4 | pcl_utils.cc 5 | data_loader.cc 6 | yaml_loader.cc 7 | shaders/parse.cc 8 | shaders/vertex.cc 9 | shaders/feedback_buffer.cc 10 | core/gpu_texture.cc 11 | core/index_map.cc 12 | core/global_model.cc 13 | ) -------------------------------------------------------------------------------- /src/map_render/core/gpu_texture.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #include "map_render/core/gpu_texture.h" 31 | 32 | namespace dsl { 33 | 34 | const std::string GPUTexture::RGB = "RGB"; 35 | const std::string GPUTexture::DEPTH_RAW = "DEPTH"; 36 | const std::string GPUTexture::DEPTH_FILTERED = "DEPTH_FILTERED"; 37 | const std::string GPUTexture::DEPTH_METRIC = "DEPTH_METRIC"; 38 | const std::string GPUTexture::DEPTH_METRIC_FILTERED = "DEPTH_METRIC_FILTERED"; 39 | const std::string GPUTexture::DEPTH_NORM = "DEPTH_NORM"; 40 | const std::string GPUTexture::VERTEX_MAP = "VERTEX_MAP"; 41 | const std::string GPUTexture::REF_RGB = "REF_RGB"; 42 | const std::string GPUTexture::PREDICT_DEPTH = "PREDICT_DEPTH"; 43 | 44 | GPUTexture::GPUTexture(const int width, const int height, 45 | const GLenum internalFormat, const GLenum format, 46 | const GLenum dataType, const bool draw, const bool cuda) 47 | : texture(new pangolin::GlTexture(width, height, internalFormat, draw, 0, 48 | format, dataType)), 49 | draw(draw), 50 | width(width), 51 | height(height), 52 | internalFormat(internalFormat), 53 | format(format), 54 | dataType(dataType) { 55 | if (cuda) { 56 | cudaGraphicsGLRegisterImage(&cudaRes, texture->tid, GL_TEXTURE_2D, 57 | cudaGraphicsRegisterFlagsReadOnly); 58 | } else { 59 | cudaRes = 0; 60 | } 61 | } 62 | 63 | GPUTexture::~GPUTexture() { 64 | if (texture) { 65 | delete texture; 66 | } 67 | 68 | if (cudaRes) { 69 | cudaGraphicsUnregisterResource(cudaRes); 70 | } 71 | } 72 | 73 | } // namespace dsl -------------------------------------------------------------------------------- /src/map_render/shaders/parse.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #include "map_render/shaders/parse.h" 31 | 32 | namespace dsl { 33 | 34 | Parse::Parse() {} 35 | 36 | const Parse &Parse::Get() { 37 | static const Parse instance; 38 | return instance; 39 | } 40 | 41 | int Parse::Arg(int argc, char **argv, const char *str, std::string &val) const { 42 | int index = FindArg(argc, argv, str) + 1; 43 | 44 | if (index > 0 && index < argc) { 45 | val = argv[index]; 46 | } 47 | 48 | return index - 1; 49 | } 50 | 51 | int Parse::Arg(int argc, char **argv, const char *str, float &val) const { 52 | int index = FindArg(argc, argv, str) + 1; 53 | 54 | if (index > 0 && index < argc) { 55 | val = atof(argv[index]); 56 | } 57 | 58 | return index - 1; 59 | } 60 | 61 | int Parse::Arg(int argc, char **argv, const char *str, int &val) const { 62 | int index = FindArg(argc, argv, str) + 1; 63 | 64 | if (index > 0 && index < argc) { 65 | val = atoi(argv[index]); 66 | } 67 | 68 | return index - 1; 69 | } 70 | 71 | std::string Parse::ShaderDir() const { 72 | std::string currentVal = STR(SHADER_DIR); 73 | 74 | assert(pangolin::FileExists(currentVal) && "Shader directory not found!"); 75 | 76 | return currentVal; 77 | } 78 | 79 | std::string Parse::BaseDir() const { 80 | char buf[256]; 81 | 82 | int length = readlink("/proc/self/exe", buf, sizeof(buf)); 83 | 84 | std::string currentVal; 85 | currentVal.append((char *)&buf, length); 86 | 87 | currentVal = currentVal.substr(0, currentVal.rfind("/build/")); 88 | 89 | return currentVal; 90 | } 91 | 92 | int Parse::FindArg(int argc, char **argv, const char *argument_name) const { 93 | for (int i = 1; i < argc; ++i) { 94 | // Search for the string 95 | if (strcmp(argv[i], argument_name) == 0) { 96 | return i; 97 | } 98 | } 99 | return -1; 100 | } 101 | 102 | } // namespace dsl -------------------------------------------------------------------------------- /src/map_render/shaders/vertex.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #include "map_render/shaders/vertex.h" 31 | 32 | namespace dsl { 33 | 34 | /* 35 | * OK this is the structure 36 | * 37 | *-------------------- 38 | * vec3 position 39 | * float confidence 40 | * 41 | * float color (encoded as a 24-bit integer) 42 | * float 43 | * float initTime 44 | * float timestamp 45 | * 46 | * vec3 normal 47 | * float radius 48 | *-------------------- 49 | 50 | * Which is three vec4s 51 | */ 52 | 53 | const int Vertex::SIZE = sizeof(Eigen::Vector4f) * 3; 54 | 55 | } -------------------------------------------------------------------------------- /src/optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsl_cost_functor 2 | photo_cost_functor.cc 3 | homo_cost_functor.cc 4 | marginalization_factor.cc 5 | se3_prior_cost_functor.cc 6 | parameter_map.cc 7 | ) -------------------------------------------------------------------------------- /src/optimization/se3_prior_cost_functor.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 1/15/20. 28 | // 29 | 30 | #include "optimization/se3_prior_cost_functor.h" 31 | 32 | namespace dsl { 33 | 34 | SE3PriorCostFunctor::SE3PriorCostFunctor(const SE3 &measurement, 35 | const information_t &information) { 36 | SetMeasurement(measurement); 37 | SetInformation(information); 38 | } 39 | 40 | void SE3PriorCostFunctor::SetMeasurement(const SE3 &measurement) { 41 | measurement_ = measurement; 42 | measurement_inv_ = measurement.inverse(); 43 | } 44 | 45 | void SE3PriorCostFunctor::SetInformation(const information_t &information) { 46 | information_ = information; 47 | Eigen::LLT llt_of_information(information_); 48 | sqrt_information_ = llt_of_information.matrixL().transpose(); 49 | } 50 | 51 | bool SE3PriorCostFunctor::Evaluate(const double *const *parameters, 52 | double *residuals, double **jacobians) const { 53 | Eigen::Map param0(parameters[0]); 54 | SE3 estimation = SE3::exp(param0); 55 | SE3 error = measurement_inv_ * estimation; 56 | Eigen::Map > weighted_error(residuals); 57 | weighted_error = sqrt_information_ * error.log(); 58 | 59 | if (jacobians != nullptr) { 60 | if (jacobians[0] != nullptr) { 61 | Eigen::Map > J0( 62 | jacobians[0]); 63 | J0 = sqrt_information_ * measurement_inv_.Adj(); 64 | } 65 | } 66 | return true; 67 | } 68 | 69 | } // namespace dsl -------------------------------------------------------------------------------- /src/relocalization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsl_relocalization 2 | feature_extractor.cc 3 | sp_extractor.cc 4 | feature_matcher.cc 5 | sp_matcher.cc 6 | vocabulary_binary.cc 7 | struct/map_point.cc 8 | struct/frame.cc 9 | struct/virtual_frame.cc 10 | struct/clique.cc 11 | struct/vlad_database.cc 12 | struct/key_frame_database.cc 13 | struct/motion_estimator.cc 14 | reloc_optimization/optimizer.cc 15 | reloc_optimization/reprojection_functor.cc 16 | reloc_optimization/surfel_reprojection_functor.cc 17 | visualization/drawer.cc 18 | relocalization_config.cc 19 | pnp_solver.cc 20 | converter.cc 21 | desc_map_updater.cc 22 | desc_map_optimizer.cc 23 | desc_map_unused.cc 24 | desc_map.cc 25 | ) 26 | 27 | add_dependencies( 28 | dsl_relocalization 29 | dsl_util 30 | DBoW2 31 | ) -------------------------------------------------------------------------------- /src/relocalization/desc_map_viewer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/7/20. 28 | // 29 | 30 | #include "relocalization/desc_map_viewer.h" 31 | 32 | namespace dsl::relocalization { 33 | 34 | DescMapViewer::DescMapViewer() { 35 | 36 | } 37 | 38 | void DescMapViewer::Run() { 39 | 40 | } 41 | 42 | } // namespace dsl::relocalization -------------------------------------------------------------------------------- /src/relocalization/struct/net_vlad_utils.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/23/20. 28 | // 29 | 30 | #include "relocalization/struct/net_vlad_utils.h" 31 | -------------------------------------------------------------------------------- /src/relocalization/struct/virtual_frame.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/6/20. 28 | // 29 | 30 | #include "relocalization/struct/virtual_frame.h" 31 | #include "relocalization/struct/map_point.h" 32 | #include "relocalization/struct/frame.h" 33 | #include "relocalization/converter.h" 34 | #include "util/global_calib.h" 35 | 36 | namespace dsl::relocalization { 37 | 38 | 39 | } // namespace dsl::relocalization -------------------------------------------------------------------------------- /src/tool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsl_tool 2 | new_tsukuba_reader.cc 3 | habitat_sim_reader.cc 4 | dataset_converter.cc 5 | cv_helper.cc 6 | euroc_reader.cc 7 | ) -------------------------------------------------------------------------------- /src/tool/cv_helper.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/11/19. 28 | // 29 | 30 | #include "tool/cv_helper.h" 31 | 32 | namespace cv_helper { 33 | 34 | void DrawPairs(const cv::Mat &img1, const cv::Mat &img2, 35 | const std::vector &pvec1, 36 | const std::vector &pvec2, cv::Mat &stereo_img, 37 | bool draw_lines, int skip_count, cv::Scalar color1, 38 | cv::Scalar color2) { 39 | assert(img1.size() == img2.size()); 40 | assert(pvec1.size() == pvec2.size()); 41 | 42 | for (int i = 0; i < pvec1.size(); i += skip_count) { 43 | cv::Point2f left = cv::Point2f(pvec1[i].x, pvec1[i].y); 44 | cv::Point2f right = (cv::Point2f(pvec2[i].x, pvec2[i].y) + 45 | cv::Point2f((float)img1.cols, 0.f)); 46 | if (draw_lines) { 47 | line(stereo_img, left, right, cv::Scalar(255, 0, 0)); 48 | } 49 | 50 | circle(stereo_img, left, 1, color1, -1, cv::FILLED); 51 | circle(stereo_img, right, 1, color2, -1, cv::FILLED); 52 | } 53 | } 54 | 55 | void VisualizePairs(const cv::Mat &img1, const cv::Mat &img2, 56 | const std::vector &pvec1, 57 | const std::vector &pvec2, bool draw_lines, 58 | int skip_count, cv::Scalar color1, cv::Scalar color2) { 59 | cv::Mat stereo_img( 60 | cv::Mat(img1.rows, img1.cols + img2.cols, CV_8UC3, cv::Scalar(0, 0, 0))); 61 | img1.copyTo(stereo_img(cv::Rect(0, 0, img1.cols, img1.rows))); 62 | img2.copyTo(stereo_img(cv::Rect(img2.cols, 0, img2.cols, img2.rows))); 63 | DrawPairs(img1, img2, pvec1, pvec2, stereo_img, draw_lines, skip_count, 64 | color1, color2); 65 | 66 | cv::namedWindow("vis", cv::WINDOW_NORMAL); 67 | cv::imshow("vis", stereo_img); 68 | cv::resizeWindow("vis", 640 * 2, 480); 69 | } 70 | 71 | } -------------------------------------------------------------------------------- /src/tool/dataset_converter.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 6/30/20. 28 | // 29 | 30 | #include "tool/dataset_converter.h" 31 | 32 | namespace dsl { 33 | 34 | std::vector LoadTum(std::string pose_path) { 35 | std::vector poses_with_fn; 36 | 37 | std::ifstream pose_infile(pose_path); 38 | 39 | std::string line; 40 | while (std::getline(pose_infile, line)) { 41 | std::istringstream iss(line); 42 | Eigen::Matrix vec_in; 43 | int i = 0; 44 | std::string s; 45 | std::string filename; 46 | 47 | while (iss >> s) { 48 | if (i == 0) { 49 | filename = s.substr(0, 10) + s.substr(11, 9); 50 | } 51 | vec_in[i] = std::stod(s); 52 | // std::cout << vec_in[i] << " "; 53 | ++i; 54 | if (iss.peek() == ' ') iss.ignore(); 55 | } 56 | // std::cout << std::endl; 57 | Eigen::Quaterniond q(vec_in.segment<4>(4)); 58 | q.normalize(); 59 | // std::cout << q.coeffs() << std::endl; 60 | 61 | SE3 pose_in; 62 | pose_in.translation() = vec_in.segment<3>(1); 63 | pose_in.setRotationMatrix(q.toRotationMatrix()); 64 | 65 | FrameShellWithFn frame_shell; 66 | frame_shell.timestamp = vec_in(0); 67 | frame_shell.cam_to_world = pose_in; 68 | frame_shell.filename = filename; 69 | 70 | poses_with_fn.push_back(frame_shell); 71 | } 72 | return poses_with_fn; 73 | } 74 | 75 | std::string ToTum(std::string time, const SE3 &se3) { 76 | std::stringstream ss; 77 | Eigen::Quaterniond q = se3.unit_quaternion(); 78 | Vec3 p = se3.translation(); 79 | std::string time_converted; 80 | if (time.size() >= 18) { 81 | time_converted = time.substr(0, 10) + "." + time.substr(10, 9); 82 | } else { 83 | time_converted = time; 84 | } 85 | ss << time_converted << " " << p.x() << " " 86 | << p.y() << " " << p.z() << " " << q.x() << " " << q.y() << " " << q.z() 87 | << " " << q.w() << std::endl; 88 | return ss.str(); 89 | } 90 | 91 | } -------------------------------------------------------------------------------- /src/tool/euroc_reader.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/19/19. 28 | // 29 | 30 | #include "tool/euroc_reader.h" 31 | #include 32 | 33 | #include 34 | 35 | namespace fs = boost::filesystem; 36 | 37 | namespace dsl { 38 | 39 | EurocReader::EurocReader(std::string _path, std::string config_filename) : path(_path) { 40 | image_path = path + "/" + "images"; 41 | depth_path = path + "/" + "depths"; 42 | 43 | for (const auto& entry : fs::directory_iterator(image_path)) { 44 | filenames.emplace_back(entry.path().stem().string()); 45 | if (file_ext == "") { 46 | file_ext = entry.path().extension().string(); 47 | } 48 | } 49 | 50 | std::sort(filenames.begin(), filenames.end()); 51 | 52 | config_path = path + "/" + config_filename; 53 | cv::FileStorage fs(config_path, cv::FileStorage::READ); 54 | float fx = fs["projection_parameters"]["fx"]; 55 | float fy = fs["projection_parameters"]["fy"]; 56 | float cx = fs["projection_parameters"]["cx"]; 57 | float cy = fs["projection_parameters"]["cy"]; 58 | K << fx, 0, cx, 0, fy, cy, 0, 0, 1; 59 | 60 | w = fs["image_width"]; 61 | h = fs["image_height"]; 62 | } 63 | 64 | void EurocReader::ReadImage(int idx) { 65 | std::string fn = image_path + "/" + filenames[idx] + file_ext; 66 | gray_image = cv::imread(fn, CV_LOAD_IMAGE_GRAYSCALE); 67 | } 68 | 69 | int EurocReader::ReadImage(const std::string& filename) { 70 | auto it = std::find(filenames.begin(), filenames.end(), filename); 71 | if (it != filenames.end()) { 72 | int idx_in_filenames = 73 | std::distance(filenames.begin(), it); 74 | 75 | std::string fn = image_path + "/" + filename + file_ext; 76 | gray_image = cv::imread(fn, CV_LOAD_IMAGE_GRAYSCALE); 77 | return idx_in_filenames; 78 | } else { 79 | return -1; 80 | } 81 | } 82 | 83 | void EurocReader::ReadDepth(int idx) { 84 | std::string fn = depth_path + "/" + filenames[idx] + ".xml"; 85 | std::cout << fn; 86 | dist_image = cv::imread(fn); 87 | cv::FileStorage fs(fn, cv::FileStorage::READ); 88 | fs["depth"] >> dist_image; 89 | } 90 | 91 | void EurocReader::ReadImageAndDist(int idx) { 92 | ReadImage(idx); 93 | ReadDepth(idx); 94 | } 95 | 96 | } // namespace dsl -------------------------------------------------------------------------------- /src/tool/habitat_sim_reader.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 2/27/20. 28 | // 29 | 30 | #include "tool/habitat_sim_reader.h" 31 | 32 | #include 33 | 34 | namespace dsl { 35 | 36 | HabitatSimReader::HabitatSimReader( 37 | std::string _path, dsl::HabitatSimReader::CameraName camera_name) 38 | : path(_path) { 39 | image_path = path + "/" + cameras[camera_name] + "/"; 40 | depth_path = path + "/depth/"; 41 | pose_path = path + "/trajectory.tum"; 42 | std::ifstream pose_infile(pose_path); 43 | 44 | std::string line; 45 | while (std::getline(pose_infile, line)) { 46 | std::istringstream iss(line); 47 | Eigen::Matrix vec_in; 48 | int i = 0; 49 | std::string s; 50 | 51 | while (iss >> s) { 52 | vec_in[i] = std::stod(s); 53 | std::cout << vec_in[i] << " "; 54 | ++i; 55 | if (iss.peek() == ' ') iss.ignore(); 56 | } 57 | std::cout << std::endl; 58 | Eigen::Quaterniond q(vec_in.segment<4>(4)); 59 | q.normalize(); 60 | std::cout << q.coeffs() << std::endl; 61 | 62 | SE3 pose_in; 63 | pose_in.translation() = vec_in.segment<3>(1); 64 | pose_in.setRotationMatrix(q.toRotationMatrix()); 65 | 66 | all_poses.emplace_back(pose_in); 67 | } 68 | 69 | K << 320, 0, 320, 0, 320, 240, 0, 0, 1; 70 | } 71 | 72 | bool HabitatSimReader::ReadImageAndDepth(int index) { 73 | std::stringstream ss; 74 | ss << std::setw(5) << std::setfill('0') << (index); 75 | std::string s = ss.str(); 76 | std::string color_fn = image_path + s + ".png"; 77 | std::string depth_fn = depth_path + s + ".xml"; 78 | std::cout <<"color_fn: " << color_fn << std::endl; 79 | color_image = cv::imread(color_fn); 80 | cv::cvtColor(color_image, gray_image, cv::COLOR_BGR2GRAY); 81 | cv::FileStorage fs; 82 | fs.open(depth_fn, cv::FileStorage::READ); 83 | fs["depth"] >> depth_image; 84 | cam_pose = all_poses[index]; 85 | } 86 | 87 | bool HabitatSimReader::ReadImage(int index) { 88 | std::stringstream ss; 89 | ss << std::setw(5) << std::setfill('0') << (index); 90 | std::string s = ss.str(); 91 | std::string color_fn = image_path + s + ".png"; 92 | std::cout <<"color_fn: " << color_fn << std::endl; 93 | color_image = cv::imread(color_fn); 94 | cv::cvtColor(color_image, gray_image, cv::COLOR_BGR2GRAY); 95 | cam_pose = all_poses[index]; 96 | } 97 | 98 | } // namespace dsl -------------------------------------------------------------------------------- /src/tool/new_tsukuba_reader.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/8/19. 28 | // 29 | 30 | #include "tool/new_tsukuba_reader.h" 31 | 32 | namespace dsl { 33 | 34 | NewTsukubaReader::NewTsukubaReader( 35 | std::string _path, dsl::NewTsukubaReader::SceneName scene_name, 36 | dsl::NewTsukubaReader::CameraName camera_name) 37 | : path(_path) { 38 | image_path = path + "/illumination/" + scenes[scene_name] + "/" + 39 | cameras[camera_name] + "/tsukuba_" + scenes[scene_name] + "_" + 40 | LR[camera_name] + "_"; 41 | depth_path = path + "/groundtruth/depth_maps/" + cameras[camera_name] + 42 | "/tsukuba_depth_" + LR[camera_name] + "_"; 43 | pose_path = path + "/groundtruth/camera_track.txt"; 44 | std::ifstream pose_infile(pose_path); 45 | 46 | std::string line; 47 | while (std::getline(pose_infile, line)) { 48 | std::istringstream iss(line); 49 | Eigen::Matrix vec_in; 50 | int i = 0; 51 | std::string s; 52 | 53 | while (iss >> s) { 54 | vec_in[i] = std::stod(s); 55 | // std::cout << vec_in[i] << " "; 56 | ++i; 57 | if (iss.peek() == ',') iss.ignore(); 58 | } 59 | // std::cout << std::endl; 60 | vec_in.head<3>() /= 100.0; 61 | vec_in[1] = -vec_in[1]; 62 | vec_in[2] = -vec_in[2]; 63 | vec_in.tail<3>() *= (M_PI / 180.0); 64 | 65 | Eigen::Matrix3d R(Eigen::AngleAxisd(-vec_in[5], Eigen::Vector3d::UnitZ()) * 66 | Eigen::AngleAxisd(-vec_in[4], Eigen::Vector3d::UnitY()) * 67 | Eigen::AngleAxisd(vec_in[3], Eigen::Vector3d::UnitX())); 68 | SE3 pose_in; 69 | pose_in.translation() = vec_in.head<3>(); 70 | pose_in.setRotationMatrix(R); 71 | 72 | all_poses.emplace_back(pose_in); 73 | } 74 | 75 | K << 615, 0, 320, 0, 615, 240, 0, 0, 1; 76 | } 77 | 78 | bool NewTsukubaReader::ReadImageAndDepth(int index) { 79 | std::stringstream ss; 80 | ss << std::setw(5) << std::setfill('0') << (index + 1); 81 | std::string s = ss.str(); 82 | std::string color_fn = image_path + s + ".png"; 83 | std::string depth_fn = depth_path + s + ".xml"; 84 | color_img = cv::imread(color_fn); 85 | cv::FileStorage fs; 86 | fs.open(depth_fn, cv::FileStorage::READ); 87 | fs["depth"] >> depth_img; 88 | depth_img /= 100.0; 89 | cam_pose = all_poses[index]; 90 | } 91 | 92 | } 93 | -------------------------------------------------------------------------------- /src/util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsl_util 2 | global_calib.cc 3 | settings.cc 4 | timing.cc 5 | ) -------------------------------------------------------------------------------- /src/util/graph_utils.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 7/9/20. 28 | // 29 | 30 | #include "util/graph_utils.h" 31 | #include "util/util_common.h" 32 | 33 | using namespace boost; 34 | namespace dsl { 35 | 36 | bool GraphUtils::IsInSourceSet(vertex_descriptor v) const { 37 | return (src_color == get(colors, v)); 38 | } 39 | 40 | void GraphUtils::SetGraph( 41 | const std::map, long> &edge_values, long num_vertex) { 42 | property_map::type capacity = get(edge_capacity, g); 43 | property_map::type residual_capacity = 44 | get(edge_residual_capacity, g); 45 | property_map::type rev = get(edge_reverse, g); 46 | 47 | LOG(INFO) << "num_vertex: " << num_vertex; 48 | LOG(INFO) << "edge_values: " << edge_values.size(); 49 | 50 | for (long vi = 0; vi < num_vertex; ++vi) { 51 | verts.push_back(add_vertex(g)); 52 | } 53 | 54 | for (auto &&edge : edge_values) { 55 | long t0 = edge.first.first; 56 | long t1 = edge.first.second; 57 | edge_descriptor e1, e2; 58 | bool in1, in2; 59 | boost::tie(e1, in1) = add_edge(verts[t0], verts[t1], g); // tail -> head 60 | boost::tie(e2, in2) = add_edge(verts[t1], verts[t0], g); 61 | 62 | // if (!in1 || !in2) { 63 | // LOG(ERROR) << "unable to add edge (" << t1 << "," << t0 << ")" 64 | // << std::endl; 65 | // return; 66 | // } 67 | capacity[e1] += edge.second; 68 | capacity[e2] += 0; 69 | rev[e1] = e2; 70 | rev[e2] = e1; 71 | // LOG(INFO) << t0 << " " << t1 << " " << edge.second; 72 | } 73 | std::vector color(num_vertices(g)); 74 | std::vector distance(num_vertices(g)); 75 | } 76 | 77 | void GraphUtils::CalcSourceSet(long source_idx, long target_idx, 78 | std::vector &source_set) { 79 | source_set.clear(); 80 | Traits::vertex_descriptor s, t; 81 | s = verts[source_idx]; 82 | t = verts[target_idx]; 83 | long flow = boykov_kolmogorov_max_flow(g, s, t); 84 | 85 | LOG(INFO) << "c The total flow:"; 86 | LOG(INFO) << "s " << flow; 87 | LOG(INFO) << "num_vertices(g) " << num_vertices(g); 88 | 89 | colors = get(boost::vertex_color, g); 90 | src_color = get(colors, s); 91 | graph_traits::vertex_iterator u_iter, u_end; 92 | graph_traits::out_edge_iterator ei, e_end; 93 | for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) { 94 | if (IsInSourceSet(*u_iter)) { 95 | source_set.push_back(*u_iter); 96 | } 97 | } 98 | } 99 | 100 | } // namespace dsl -------------------------------------------------------------------------------- /support_files/.gitignore: -------------------------------------------------------------------------------- 1 | vocabulary 2 | *.tgz 3 | -------------------------------------------------------------------------------- /support_files/superpoint.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hyye/dsl/36f956b24be136db2ee63a8e40a9583764e63369/support_files/superpoint.pt -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(Test_DEPENDENCIES 2 | dsl_full_system 3 | dsl_relocalization 4 | dsl_cost_functor 5 | dsl_util 6 | dsl_tool 7 | dsl_map_render 8 | ${ThreeParty_LIBS} 9 | gmock gtest gtest_main 10 | ) 11 | 12 | set(Test_LIBRARIES 13 | dsl_full_system 14 | dsl_relocalization 15 | dsl_cost_functor 16 | dsl_util 17 | dsl_tool 18 | dsl_map_render 19 | ${ThreeParty_LIBS} 20 | ${Google_LIBRARIES} 21 | ${Ext_LIBRARIES} 22 | ) 23 | 24 | add_executable(ref_test ref_test.cc) 25 | target_link_libraries(ref_test 26 | ${Test_LIBRARIES} 27 | ) 28 | add_dependencies(ref_test 29 | ${Test_DEPENDENCIES} 30 | ) 31 | 32 | add_executable(mt_test mt_test.cc) 33 | target_link_libraries(mt_test 34 | ${Test_LIBRARIES} 35 | ) 36 | add_dependencies(mt_test 37 | ${Test_DEPENDENCIES} 38 | ) 39 | 40 | add_executable(img_test img_test.cc) 41 | target_link_libraries(img_test 42 | ${Test_LIBRARIES} 43 | ) 44 | add_dependencies(img_test 45 | ${Test_DEPENDENCIES} 46 | ) 47 | 48 | add_executable(coarse_tracker_test coarse_tracker_test.cc) 49 | target_link_libraries(coarse_tracker_test 50 | ${Test_LIBRARIES} 51 | ) 52 | add_dependencies(coarse_tracker_test 53 | ${Test_DEPENDENCIES} 54 | ) 55 | 56 | add_executable(front_end_test front_end_test.cc) 57 | target_link_libraries(front_end_test 58 | ${Test_LIBRARIES} 59 | ) 60 | add_dependencies(front_end_test 61 | ${Test_DEPENDENCIES} 62 | ) 63 | 64 | add_executable(ceres_test ceres_test.cc) 65 | target_link_libraries(ceres_test 66 | ${Test_LIBRARIES} 67 | ) 68 | add_dependencies(ceres_test 69 | ${Test_DEPENDENCIES} 70 | ) 71 | 72 | add_executable(optimization_test optimization_test.cc) 73 | target_link_libraries(optimization_test 74 | ${Test_LIBRARIES} 75 | ) 76 | add_dependencies(optimization_test 77 | ${Test_DEPENDENCIES} 78 | ) 79 | 80 | add_executable(decomposition_test decomposition_test.cc) 81 | target_link_libraries(decomposition_test 82 | ${Test_LIBRARIES} 83 | ) 84 | add_dependencies(decomposition_test 85 | ${Test_DEPENDENCIES} 86 | ) 87 | 88 | add_executable(euroc_test euroc_test.cc) 89 | target_link_libraries(euroc_test 90 | ${Test_LIBRARIES} 91 | ${Boost_LIBRARIES} 92 | ) 93 | add_dependencies(euroc_test 94 | ${Test_DEPENDENCIES} 95 | ) 96 | 97 | add_executable(render_test render_test.cc) 98 | target_link_libraries(render_test 99 | ${Test_LIBRARIES} 100 | ${Boost_LIBRARIES} 101 | ) 102 | add_dependencies(render_test 103 | ${Test_DEPENDENCIES} 104 | ) 105 | 106 | add_executable(dbow2_test dbow2_test.cc) 107 | target_link_libraries(dbow2_test 108 | ${Test_LIBRARIES} 109 | ${Boost_LIBRARIES} 110 | DBoW2 111 | ) 112 | add_dependencies(dbow2_test 113 | ${Test_DEPENDENCIES} 114 | DBoW2 115 | ) 116 | 117 | add_executable(netvlad_test netvlad_test.cc) 118 | target_link_libraries(netvlad_test 119 | ${Test_LIBRARIES} 120 | ${Boost_LIBRARIES} 121 | DBoW2 122 | ) 123 | add_dependencies(netvlad_test 124 | ${Test_DEPENDENCIES} 125 | DBoW2 126 | ) 127 | 128 | add_executable(sp_test sp_test.cc) 129 | target_link_libraries(sp_test 130 | ${Test_LIBRARIES} 131 | ${Boost_LIBRARIES} 132 | ) 133 | add_dependencies(sp_test 134 | ${Test_DEPENDENCIES} 135 | ) 136 | 137 | add_executable(desc_map_vis desc_map_vis.cc) 138 | target_link_libraries(desc_map_vis 139 | ${Test_LIBRARIES} 140 | ${Boost_LIBRARIES} 141 | ) 142 | add_dependencies(desc_map_vis 143 | ${Test_DEPENDENCIES} 144 | ) -------------------------------------------------------------------------------- /test/mt_test.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 11/6/19. 28 | // 29 | 30 | #include "util/index_thread_reduce.h" 31 | #include 32 | 33 | using namespace dsl; 34 | 35 | DEFINE_int64(N, 20, "input number"); 36 | 37 | static std::vector global_v; 38 | 39 | void SumReductor(int min, int max, int &stats, int tid) { 40 | for (int i = min; i < max; ++i) { 41 | stats += global_v[i]; 42 | } 43 | } 44 | 45 | TEST(MTTest,IndexThreadReduceTest) { 46 | int N = FLAGS_N; 47 | for (int i = 0; i < N; ++i) { 48 | global_v.emplace_back(i * 1); 49 | } 50 | 51 | IndexThreadReduce thread_reduce; 52 | thread_reduce.Reduce( 53 | std::bind(&SumReductor, std::placeholders::_1, std::placeholders::_2, 54 | std::placeholders::_3, std::placeholders::_4), 55 | 0, 1, 0); 56 | int stats = 0; 57 | if (thread_reduce.GetStats(stats)) { 58 | // std::cout << "Successful" << std::endl; 59 | } else { 60 | // std::cout << "Unsuccessful" << std::endl; 61 | } 62 | LOG(INFO) << "stats: " << stats; 63 | 64 | thread_reduce.Reduce( 65 | std::bind(&SumReductor, std::placeholders::_1, std::placeholders::_2, 66 | std::placeholders::_3, std::placeholders::_4), 67 | 1, N, 3); 68 | 69 | stats = 0; 70 | if (thread_reduce.GetStats(stats)) { 71 | // std::cout << "Successful" << std::endl; 72 | } else { 73 | // std::cout << "Unsuccessful" << std::endl; 74 | } 75 | LOG(INFO) << "stats: " << stats; 76 | } 77 | 78 | int main(int argc, char **argv) { 79 | google::InitGoogleLogging(argv[0]); 80 | 81 | gflags::ParseCommandLineFlags(&argc, &argv, true); 82 | FLAGS_alsologtostderr = true; 83 | 84 | testing::InitGoogleTest(&argc, argv); 85 | 86 | return RUN_ALL_TESTS(); 87 | } 88 | -------------------------------------------------------------------------------- /test/render_test.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Direct Sparse Localization (DSL). 3 | * 4 | * Copyright (C) 2021 Haoyang Ye , 5 | * and Huaiyang Huang , 6 | * Robotics and Multiperception Lab (RAM-LAB ), 7 | * The Hong Kong University of Science and Technology 8 | * 9 | * For more information please see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * DSL is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSL is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSL. If not, see . 25 | */ 26 | // 27 | // Created by hyye on 12/27/19. 28 | // 29 | 30 | #include "map_render/core/global_model.h" 31 | #include "map_render/core/index_map.h" 32 | #include "map_render/data_loader.h" 33 | #include "map_render/gui.h" 34 | #include "map_render/yaml_loader.h" 35 | #include "tool/euroc_reader.h" 36 | 37 | using namespace dsl; 38 | 39 | DEFINE_string(path, "", "input path"); 40 | 41 | int main(int argc, char **argv) { 42 | google::InitGoogleLogging(argv[0]); 43 | 44 | gflags::ParseCommandLineFlags(&argc, &argv, true); 45 | FLAGS_alsologtostderr = true; 46 | 47 | EurocReader reader(FLAGS_path); 48 | YamlLoader yaml_loader(reader.config_path); 49 | MapLoader map_loader(yaml_loader.pcd_file); 50 | SetGlobalCalib(reader.w, reader.h, reader.K, 0); 51 | 52 | GUI gui(false, false); 53 | 54 | pangolin::GlBuffer *vbo = map_loader.GetVbo(); 55 | LOG(INFO) << vbo->SizeBytes() << " size bytes"; 56 | 57 | GlobalModel global_model; 58 | global_model.Initialization(*vbo); 59 | 60 | PangolinUtils new_vn; 61 | IndexMap index_map; 62 | Eigen::Affine3f trans; 63 | trans = yaml_loader.initial_pose; 64 | 65 | while (!pangolin::ShouldQuit()) { 66 | gui.PreCall(); 67 | 68 | index_map.PredictGlobalIndices(trans.matrix(), global_model.Model(), 100); 69 | new_vn.SetVertexNormal(index_map.VertGlobalTex()->texture, 70 | index_map.NormalGlobalTex()->texture); 71 | Vec4f *n_ptr = (Vec4f *)new_vn.normal_img.ptr; 72 | Vec4f *v_ptr = (Vec4f *)new_vn.vertex_img.ptr; 73 | cv::Mat normal_img(hG[0], wG[0], CV_32FC3); 74 | cv::Mat vertex_img(hG[0], wG[0], CV_32FC3); 75 | for (int x = 0; x < wG[0]; ++x) { 76 | for (int y = 0; y < hG[0]; ++y) { 77 | int idx = x + y * wG[0]; 78 | Vec4f n = n_ptr[idx]; 79 | Vec4f v = v_ptr[idx]; 80 | normal_img.at(y, x) = cv::Vec3f(n.x(), n.y(), n.z()); 81 | vertex_img.at(y, x) = cv::Vec3f(v.x(), v.y(), v.z()); 82 | } 83 | 84 | } 85 | 86 | cv::namedWindow("n", cv::WINDOW_NORMAL); 87 | cv::imshow("n", normal_img); 88 | 89 | cv::namedWindow("v", cv::WINDOW_NORMAL); 90 | cv::imshow("v", vertex_img / 10.0); 91 | cv::waitKey(1); 92 | 93 | if (gui.draw_global_model->Get()) { 94 | global_model.RenderPointCloud(gui.s_cam.GetProjectionModelViewMatrix(), 95 | gui.draw_normals->Get(), 96 | gui.draw_colors->Get()); 97 | } 98 | 99 | gui.PostCall(); 100 | } 101 | 102 | delete vbo; 103 | 104 | return 0; 105 | } --------------------------------------------------------------------------------