├── .vscode ├── c_cpp_properties.json ├── launch.json └── settings.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── calib │ ├── 00 │ │ ├── ba_calib.yml │ │ ├── he_calib.yml │ │ └── iba_calib_global.yml │ ├── 02 │ │ ├── he_calib.yml │ │ └── iba_calib_global.yml │ ├── 03 │ │ ├── he_calib.yml │ │ └── iba_calib_global.yml │ ├── 04 │ │ ├── he_calib.yml │ │ └── iba_calib_global.yml │ ├── 05 │ │ ├── he_calib.yml │ │ └── iba_calib_global.yml │ ├── 07 │ │ ├── he_calib.yml │ │ └── iba_calib_global.yml │ └── README ├── loam │ ├── KITTI.yml │ └── backend.yml ├── orb │ ├── save_localmap.yml │ └── store.yml ├── orb_ori │ ├── EuRoC.yaml │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ ├── KITTI19.yaml │ ├── TUM1.yaml │ ├── TUM2.yaml │ └── TUM3.yaml └── settings.sh ├── doc ├── abstract.jpg ├── ca_init_ann.png ├── ca_opt_ann.png ├── cba_init_ann.png ├── cba_opt_ann.png ├── proj_00_gt.png ├── proj_00_pred.png ├── proj_02_gt.png ├── proj_02_pred.png ├── proj_03_gt.png ├── proj_03_pred.png ├── proj_04_gt.png ├── proj_04_pred.png ├── proj_05_gt.png ├── proj_05_pred.png ├── proj_07_gt.png ├── proj_07_pred.png ├── traj_00.jpeg ├── traj_02.jpeg ├── traj_03.jpeg ├── traj_04.jpeg ├── traj_05.jpeg └── traj_07.jpeg ├── include ├── GPR.hpp ├── GeoCalib.h ├── HECalib.h ├── IBACalib.hpp ├── IBACalib2.hpp ├── IBAPlane.hpp ├── KDTreeVectorOfVectorsAdaptor.h ├── NLHECalib.hpp ├── argparse.hpp ├── backend_opt.h ├── color_gradient.h ├── cv_tools.hpp ├── g2o_tools.h ├── io_tools.h ├── kitti_tools.h ├── nanoflann.hpp └── pointcloud.h └── src ├── ba_demo.cpp ├── ba_demo_data.cpp ├── backend_opt.cpp ├── examples ├── ba_calib.cpp ├── floam_backend.cpp ├── floam_kitti.cpp ├── he_calib.cpp ├── iba_func.cpp ├── iba_global.cpp ├── iba_global_stable.cpp ├── iba_local.cpp ├── iba_single_frame.cpp ├── icp_calib.cpp ├── orb_kitti.cpp ├── orb_kitti_store.cpp ├── orb_restore.cpp └── orb_save_map.cpp ├── floam ├── CMakeLists.txt ├── include │ ├── floamClass.h │ ├── laserMappingClass.h │ ├── laserProcessingClass.h │ ├── lidar.h │ ├── lidarOptimization.h │ └── odomEstimationClass.h └── src │ ├── floamClass.cpp │ ├── laserMappingClass.cpp │ ├── laserProcessingClass.cpp │ ├── lidar.cpp │ ├── lidarOptimization.cpp │ └── odomEstimationClass.cpp ├── orb_slam ├── CMakeLists.txt ├── include │ ├── Converter.h │ ├── Frame.h │ ├── FrameDrawer.h │ ├── Initializer.h │ ├── KeyFrame.h │ ├── KeyFrameDatabase.h │ ├── LocalMapping.h │ ├── LoopClosing.h │ ├── Map.h │ ├── MapDrawer.h │ ├── MapPoint.h │ ├── ORBVocabulary.h │ ├── ORBextractor.h │ ├── ORBmatcher.h │ ├── Optimizer.h │ ├── Sim3Solver.h │ ├── System.h │ ├── Tracking.h │ └── Viewer.h └── src │ ├── Converter.cc │ ├── Frame.cc │ ├── FrameDrawer.cc │ ├── Initializer.cc │ ├── KeyFrame.cc │ ├── KeyFrameDatabase.cc │ ├── LocalMapping.cc │ ├── LoopClosing.cc │ ├── Map.cc │ ├── MapDrawer.cc │ ├── MapPoint.cc │ ├── ORBextractor.cc │ ├── ORBmatcher.cc │ ├── Optimizer.cc │ ├── Sim3Solver.cc │ ├── System.cc │ ├── Tracking.cc │ └── Viewer.cc ├── scancontext ├── CMakeLists.txt ├── KDTreeVectorOfVectorsAdaptor.h ├── Scancontext.cpp ├── Scancontext.h ├── nanoflann.hpp └── tic_toc.h └── test ├── test_gpr.cpp └── test_igl.cpp /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "${workspaceFolder}/include", 8 | "/usr/include/eigen3", 9 | "/usr/include/opencv4", 10 | "/usr/include/pcl-1.10", 11 | "/data/Pacakge/nomad/build/release/include/nomad", 12 | "/data/Pacakge/nomad/src", 13 | // "/home/bit/.local/lib/python3.8/site-packages/pybind11/include", 14 | // "/usr/include/python3.8" 15 | ], 16 | "defines": [], 17 | "compilerPath": "/usr/bin/clang", 18 | "cStandard": "c17", 19 | "cppStandard": "c++17", 20 | "intelliSenseMode": "linux-clang-x64" 21 | } 22 | ], 23 | "version": 4 24 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "environment": [ 4 | { 5 | "LD_LIBRARY_PATH": "" 6 | } 7 | ], 8 | "configurations": [ 9 | { 10 | "name": "(gdb) 启动", 11 | "type": "cppdbg", 12 | "request": "launch", 13 | "program": "${workspaceFolder}/build/iba_calib_test", 14 | "args": [ 15 | "${workspaceFolder}/config/calib/iba_calib_plane.yml", 16 | ], 17 | "stopAtEntry": false, 18 | "cwd": "${workspaceFolder}/build/", 19 | 20 | "externalConsole": false, 21 | "MIMode": "gdb", 22 | "setupCommands": [ 23 | { 24 | "description": "为 gdb 启用整齐打印", 25 | "text": "-enable-pretty-printing", 26 | "ignoreFailures": true 27 | }, 28 | { 29 | "description": "将反汇编风格设置为 Intel", 30 | "text": "-gdb-set disassembly-flavor intel", 31 | "ignoreFailures": true 32 | } 33 | ] 34 | }, 35 | { 36 | "name": "Python: File", 37 | "type": "python", 38 | "request": "launch", 39 | "program": "${file}", 40 | "justMyCode": true 41 | } 42 | ] 43 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "memory_resource": "cpp", 4 | "cctype": "cpp", 5 | "clocale": "cpp", 6 | "cmath": "cpp", 7 | "csignal": "cpp", 8 | "cstdarg": "cpp", 9 | "cstddef": "cpp", 10 | "cstdio": "cpp", 11 | "cstdlib": "cpp", 12 | "cstring": "cpp", 13 | "ctime": "cpp", 14 | "cwchar": "cpp", 15 | "cwctype": "cpp", 16 | "array": "cpp", 17 | "atomic": "cpp", 18 | "strstream": "cpp", 19 | "*.tcc": "cpp", 20 | "bitset": "cpp", 21 | "chrono": "cpp", 22 | "codecvt": "cpp", 23 | "complex": "cpp", 24 | "condition_variable": "cpp", 25 | "cstdint": "cpp", 26 | "deque": "cpp", 27 | "forward_list": "cpp", 28 | "list": "cpp", 29 | "unordered_map": "cpp", 30 | "unordered_set": "cpp", 31 | "vector": "cpp", 32 | "exception": "cpp", 33 | "algorithm": "cpp", 34 | "functional": "cpp", 35 | "iterator": "cpp", 36 | "map": "cpp", 37 | "memory": "cpp", 38 | "numeric": "cpp", 39 | "optional": "cpp", 40 | "random": "cpp", 41 | "ratio": "cpp", 42 | "set": "cpp", 43 | "string": "cpp", 44 | "string_view": "cpp", 45 | "system_error": "cpp", 46 | "tuple": "cpp", 47 | "type_traits": "cpp", 48 | "utility": "cpp", 49 | "fstream": "cpp", 50 | "future": "cpp", 51 | "initializer_list": "cpp", 52 | "iomanip": "cpp", 53 | "iosfwd": "cpp", 54 | "iostream": "cpp", 55 | "istream": "cpp", 56 | "limits": "cpp", 57 | "mutex": "cpp", 58 | "new": "cpp", 59 | "ostream": "cpp", 60 | "shared_mutex": "cpp", 61 | "sstream": "cpp", 62 | "stdexcept": "cpp", 63 | "streambuf": "cpp", 64 | "thread": "cpp", 65 | "cfenv": "cpp", 66 | "cinttypes": "cpp", 67 | "typeindex": "cpp", 68 | "typeinfo": "cpp", 69 | "valarray": "cpp", 70 | "variant": "cpp", 71 | "bit": "cpp", 72 | "*.ipp": "cpp" 73 | }, 74 | "cmake.configureOnOpen": false 75 | } -------------------------------------------------------------------------------- /config/calib/00/ba_calib.yml: -------------------------------------------------------------------------------- 1 | BaseDir: ../KITTI-00/ 2 | KeyFrameDir: KeyFrames/ 3 | VOFile: slam_res/Twc.txt 4 | LOFile: slam_res/floam_isam_00.txt 5 | init_sim3: calib_res/he_rb_calib_00.txt 6 | ORBVocabulary: ../data/Vocabulary/ORBvoc.txt 7 | ORBConfig: ../config/orb_ori/KITTI00-02.yaml 8 | VOIdFile: FrameId.yml 9 | MapFile: Map.yml 10 | ResFile: calib_res/ba_local_calib_00.txt 11 | method: local # global or local 12 | method_options: {"global":1,"local":2} -------------------------------------------------------------------------------- /config/calib/00/he_calib.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-00/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_00.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/he_calib_00.txt 9 | ResRBFile: calib_res/he_rb_calib_00.txt 10 | ResLPFile: calib_res/he_lp_calib_00.txt 11 | 12 | runtime: 13 | zero_translation: true 14 | regulation: true 15 | robust_kernel_size: 0.3 16 | regulation_weight: 1.0E-3 17 | inner_iter: 10 18 | ex_iter: 20 19 | init_mu: 64 20 | divide_factor: 1.4 21 | min_mu: 0.01 22 | verborse: true 23 | -------------------------------------------------------------------------------- /config/calib/00/iba_calib_global.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-00/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_00.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/iba_global_pl_00.txt 9 | init_sim3: calib_res/he_rb_calib_00.txt 10 | gt_sim3: calib_res/gt_calib_00.txt 11 | PointCloudDir: ../data/00/velodyne 12 | PointCloudskip: 1 13 | PointCloudOnlyPositiveX: true 14 | orb: 15 | Vocabulary: ../data/Vocabulary/ORBvoc.txt 16 | Config: ../config/orb_ori/KITTI00-02.yaml 17 | KeyFrameDir: ../KITTI-00/KeyFrames/ 18 | MapFile: ../KITTI-00/Map.yml 19 | 20 | runtime: 21 | max_pixel_dist: 1.5 22 | num_best_covis: 3 # set negative to use min_covis_weight 23 | min_covis_weight: 100 24 | min_rot_diff: 0.03 25 | kdtree2d_max_leaf_size: 10 26 | kdtree3d_max_leaf_size: 30 27 | corr_3d_2d_threshold: 40 28 | corr_3d_3d_threshold: 10 29 | norm_max_pts: 30 30 | norm_min_pts: 5 31 | norm_radius: 0.6 32 | norm_reg_threshold: 0.02 # 0.02 33 | min_diff_dist: 0.2 # 0.2 34 | he_threshold: 0.094 35 | err_weight: [1.0, 1.0] 36 | init_frame: [0.5,0.5,0.5,0.5,0.5,0.5,0.5] 37 | min_mesh: 1.0E-6 38 | lb: [-0.1,-0.1,-0.1,-0.3,-0.3,-0.3,-1.0] 39 | ub: [0.1,0.1,0.1,0.3,0.3,0.3,1.0] 40 | max_bbeval: 5000 41 | valid_rate: 0.95 42 | seed: 0 43 | use_vns: true 44 | direction_type: 2N 45 | use_cache: false 46 | cacheFile: ../KITTI-00/cache/cache_00_d1.5_fuse_wide.txt 47 | verborse: true 48 | use_plane: true # set to false for the ablation study -------------------------------------------------------------------------------- /config/calib/02/he_calib.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-02/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_02.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/he_calib_02.txt 9 | ResRBFile: calib_res/he_rb_calib_02.txt 10 | ResLPFile: calib_res/he_lp_calib_02.txt 11 | 12 | runtime: 13 | zero_translation: true 14 | regulation: true 15 | robust_kernel_size: 0.3 16 | regulation_weight: 1.0E-3 17 | inner_iter: 10 18 | ex_iter: 20 19 | init_mu: 64 20 | divide_factor: 1.4 21 | min_mu: 0.01 22 | verborse: true 23 | -------------------------------------------------------------------------------- /config/calib/02/iba_calib_global.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-02/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_02.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/iba_global_pl_02.txt 9 | init_sim3: calib_res/he_lp_calib_02.txt 10 | gt_sim3: calib_res/gt_calib_02.txt 11 | PointCloudDir: ../data/02/velodyne 12 | PointCloudskip: 1 13 | PointCloudOnlyPositiveX: true 14 | orb: 15 | Vocabulary: ../data/Vocabulary/ORBvoc.txt 16 | Config: ../config/orb_ori/KITTI00-02.yaml 17 | KeyFrameDir: ../KITTI-02/KeyFrames/ 18 | MapFile: ../KITTI-02/Map.yml 19 | 20 | runtime: 21 | max_pixel_dist: 1.5 22 | num_best_covis: 3 # set negative to use min_covis_weight 23 | min_covis_weight: 200 24 | kdtree2d_max_leaf_size: 10 25 | kdtree3d_max_leaf_size: 30 26 | corr_3d_2d_threshold: 40 27 | corr_3d_3d_threshold: 10 28 | norm_max_pts: 30 29 | norm_min_pts: 5 30 | norm_radius: 0.6 31 | norm_reg_threshold: 0.02 32 | min_diff_dist: 0.2 33 | he_threshold: 0.146 34 | err_weight: [1.0, 1.0] 35 | init_frame: [0.1,0.1,0.1,0.3,0.3,0.3,1.0] # [0.1,0.1,0.1,0.3,0.3,0.3,1.0] 36 | min_mesh: 1.0E-6 37 | lb: [-0.15,-0.15,-0.15,-0.3,-0.3,-0.3,-1.5] 38 | ub: [0.15,0.15,0.15,0.3,0.3,0.3,1.5] 39 | max_bbeval: 5000 40 | valid_rate: 0.95 41 | seed: 0 42 | use_vns: true 43 | direction_type: 2N 44 | use_cache: false 45 | cacheFile: ../KITTI-02/cache/cache_02_d1.5_fuse_wide.txt 46 | verborse: true 47 | use_plane: true # set to false for the ablation study -------------------------------------------------------------------------------- /config/calib/03/he_calib.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-03/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_raw_03.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/he_calib_03.txt 9 | ResRBFile: calib_res/he_rb_calib_03.txt 10 | ResLPFile: calib_res/he_lp_calib_03.txt 11 | 12 | runtime: 13 | zero_translation: true 14 | regulation: true 15 | robust_kernel_size: 0.15 16 | regulation_weight: 1.0E-3 17 | inner_iter: 10 18 | ex_iter: 20 19 | init_mu: 64 20 | divide_factor: 1.4 21 | min_mu: 0.01 22 | verborse: true 23 | -------------------------------------------------------------------------------- /config/calib/03/iba_calib_global.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-03/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_raw_03.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/iba_global_pl_03.txt 9 | init_sim3: calib_res/he_rb_calib_03.txt 10 | gt_sim3: calib_res/gt_calib_03.txt 11 | PointCloudDir: ../data/03/velodyne 12 | PointCloudskip: 1 13 | PointCloudOnlyPositiveX: true 14 | orb: 15 | Vocabulary: ../data/Vocabulary/ORBvoc.txt 16 | Config: ../config/orb_ori/KITTI03.yaml 17 | KeyFrameDir: ../KITTI-03/KeyFrames/ 18 | MapFile: ../KITTI-03/Map.yml 19 | 20 | runtime: 21 | max_pixel_dist: 1.5 22 | num_best_covis: 3 # set negative to use min_covis_weight 23 | min_covis_weight: 200 24 | kdtree2d_max_leaf_size: 10 25 | kdtree3d_max_leaf_size: 30 26 | corr_3d_2d_threshold: 40 27 | corr_3d_3d_threshold: 10 28 | norm_max_pts: 30 29 | norm_min_pts: 3 30 | norm_radius: 0.6 31 | norm_reg_threshold: 0.02 32 | min_diff_dist: 0.2 33 | he_threshold: 0.035 34 | err_weight: [1.0, 1.0] 35 | init_frame: [0.1,0.1,0.1,0.3,0.3,0.3,1.0] # ratio of anisotropic maximum mesh 36 | min_mesh: 1.0E-6 37 | lb: [-0.1,-0.1,-0.1,-0.3,-0.3,-0.3,-1.0] 38 | ub: [0.1,0.1,0.1,0.3,0.3,0.3,1.0] 39 | max_bbeval: 5000 40 | valid_rate: 0.95 41 | seed: 0 42 | use_vns: true 43 | direction_type: N+1 QUAD 44 | use_cache: true 45 | cacheFile: ../KITTI-03/cache/cache_pl.txt 46 | verborse: true 47 | use_plane: true # set to false for the ablation study -------------------------------------------------------------------------------- /config/calib/04/he_calib.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-04/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_raw_04.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/he_calib_04.txt 9 | ResRBFile: calib_res/he_rb_calib_04.txt 10 | ResLPFile: calib_res/he_lp_calib_04.txt 11 | 12 | runtime: 13 | zero_translation: true 14 | regulation: true 15 | robust_kernel_size: 0.15 16 | regulation_weight: 1.0E-3 17 | inner_iter: 10 18 | ex_iter: 20 19 | init_mu: 64 20 | divide_factor: 1.4 21 | min_mu: 0.01 22 | verborse: true 23 | -------------------------------------------------------------------------------- /config/calib/04/iba_calib_global.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-04/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_raw_04.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/iba_global_pl2_04.txt 9 | init_sim3: calib_res/he_rb_calib_04.txt 10 | gt_sim3: calib_res/gt_calib_04.txt 11 | PointCloudDir: ../data/04/velodyne 12 | PointCloudskip: 1 13 | PointCloudOnlyPositiveX: true 14 | orb: 15 | Vocabulary: ../data/Vocabulary/ORBvoc.txt 16 | Config: ../config/orb_ori/KITTI04-12.yaml 17 | KeyFrameDir: ../KITTI-04/KeyFrames/ 18 | MapFile: ../KITTI-04/Map.yml 19 | 20 | runtime: 21 | max_pixel_dist: 1.5 22 | num_best_covis: 3 # set negative to use min_covis_weight 23 | min_covis_weight: 200 24 | kdtree2d_max_leaf_size: 10 25 | kdtree3d_max_leaf_size: 30 26 | corr_3d_2d_threshold: 40 27 | corr_3d_3d_threshold: 10 28 | norm_max_pts: 30 29 | norm_min_pts: 5 30 | norm_radius: 0.6 31 | norm_reg_threshold: 0.02 32 | min_diff_dist: 0.2 33 | he_threshold: 0.03 34 | err_weight: [1.0, 1.0] 35 | init_frame: [0.5,0.5,0.5,0.5,0.5,0.5,0.5] # ratio of anisotropic maximum mesh 36 | min_mesh: 1.0E-6 37 | lb: [-0.1,-0.1,-0.1,-0.3,-0.3,-0.3,-1.0] 38 | ub: [0.1,0.1,0.1,0.3,0.3,0.3,1.0] 39 | max_bbeval: 5000 40 | valid_rate: 0.95 41 | seed: 0 42 | use_vns: true 43 | direction_type: 2N 44 | use_cache: true 45 | cacheFile: ../KITTI-04/cache/cache_pl.txt 46 | verborse: true 47 | use_plane: true # set to false for the ablation study -------------------------------------------------------------------------------- /config/calib/05/he_calib.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-05/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_05.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/he_calib_05.txt 9 | ResRBFile: calib_res/he_rb_calib_05.txt 10 | ResLPFile: calib_res/he_lp_calib_05.txt 11 | 12 | runtime: 13 | zero_translation: true 14 | regulation: true 15 | robust_kernel_size: 0.15 16 | regulation_weight: 1.0E-3 17 | inner_iter: 10 18 | ex_iter: 20 19 | init_mu: 64 20 | divide_factor: 1.4 21 | min_mu: 0.01 22 | verborse: true 23 | -------------------------------------------------------------------------------- /config/calib/05/iba_calib_global.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-05/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_05.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/iba_global_pl.txt 9 | init_sim3: calib_res/he_rb_calib_05.txt 10 | zero_translation: false 11 | gt_sim3: calib_res/gt_calib_05.txt 12 | PointCloudDir: ../data/05/velodyne 13 | PointCloudskip: 1 14 | PointCloudOnlyPositiveX: true 15 | orb: 16 | Vocabulary: ../data/Vocabulary/ORBvoc.txt 17 | Config: ../config/orb_ori/KITTI04-12.yaml 18 | KeyFrameDir: ../KITTI-05/KeyFrames/ 19 | MapFile: ../KITTI-05/Map.yml 20 | 21 | runtime: 22 | max_pixel_dist: 1.5 23 | num_min_corr2d_3d: 30 24 | num_best_covis: 3 # set negative to use min_covis_weight 25 | min_covis_weight: 100 26 | min_rot_diff: 0.03 27 | kdtree2d_max_leaf_size: 10 28 | kdtree3d_max_leaf_size: 30 29 | corr_3d_2d_threshold: 40 30 | corr_3d_3d_threshold: 10 31 | norm_max_pts: 30 32 | norm_min_pts: 5 33 | norm_radius: 0.6 34 | norm_reg_threshold: 0.02 35 | min_diff_dist: 0.2 36 | he_threshold: 0.07 37 | err_weight: [1.0, 1.0] 38 | init_frame: [0.5,0.5,0.5,0.5,0.5,0.5,0.5] # 0.5 ratio of anisotropic maximum mesh 39 | min_mesh: 1.0E-6 40 | lb: [-0.1,-0.1,-0.1,-0.35,-0.3,-0.3,-1.0] 41 | ub: [0.1,0.1,0.1,0.35,0.3,0.3,1.0] 42 | max_bbeval: 5000 43 | valid_rate: 0.95 44 | seed: 0 45 | use_vns: true 46 | direction_type: N+1 QUAD 47 | use_cache: false 48 | cacheFile: ../KITTI-05/cache/cache_05_d1.5_fuse_wide.txt 49 | verborse: true 50 | use_plane: true # set to false for the ablation study -------------------------------------------------------------------------------- /config/calib/07/he_calib.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-07/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_07.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/he_calib_07.txt 9 | ResRBFile: calib_res/he_rb_calib_07.txt 10 | ResLPFile: calib_res/he_lp_calib_07.txt 11 | 12 | runtime: 13 | zero_translation: true 14 | regulation: true 15 | robust_kernel_size: 0.15 16 | regulation_weight: 1.0E-3 17 | inner_iter: 10 18 | ex_iter: 20 19 | init_mu: 64 20 | divide_factor: 1.4 21 | min_mu: 0.01 22 | verborse: true 23 | -------------------------------------------------------------------------------- /config/calib/07/iba_calib_global.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | io: 4 | BaseDir: ../KITTI-07/ 5 | VOFile: slam_res/Twc.txt 6 | LOFile: slam_res/floam_isam_07.txt 7 | VOIdFile: FrameId.yml 8 | ResFile: calib_res/iba_global_pl_07.txt 9 | init_sim3: calib_res/he_rb_calib_07.txt 10 | zero_translation: true 11 | gt_sim3: calib_res/gt_calib_07.txt 12 | PointCloudDir: ../data/07/velodyne 13 | PointCloudskip: 1 14 | PointCloudOnlyPositiveX: true 15 | orb: 16 | Vocabulary: ../data/Vocabulary/ORBvoc.txt 17 | Config: ../config/orb_ori/KITTI04-12.yaml 18 | KeyFrameDir: ../KITTI-07/KeyFrames/ 19 | MapFile: ../KITTI-07/Map.yml 20 | 21 | runtime: 22 | max_pixel_dist: 1.5 23 | num_min_corr2d_3d: 30 24 | num_best_covis: 3 # set negative to use min_covis_weight 25 | min_covis_weight: 100 26 | min_rot_diff: 0.03 27 | kdtree2d_max_leaf_size: 10 28 | kdtree3d_max_leaf_size: 30 29 | corr_3d_2d_threshold: 40 30 | corr_3d_3d_threshold: 10 31 | norm_max_pts: 30 32 | norm_min_pts: 5 33 | norm_radius: 0.6 34 | norm_reg_threshold: 0.01 35 | min_diff_dist: 0.2 36 | he_threshold: 0.142 37 | err_weight: [1.0, 1.0] 38 | init_frame: [0.5,0.5,0.5,0.5,0.5,0.5,0.5] 39 | min_mesh: 1.0E-6 40 | lb: [-0.1,-0.1,-0.1,-0.35,-0.3,-0.3,-1.0] 41 | ub: [0.1,0.1,0.1,0.35,0.3,0.3,1.0] 42 | max_bbeval: 5000 43 | valid_rate: 0.95 44 | seed: 0 45 | use_vns: true 46 | direction_type: N+1 QUAD 47 | use_cache: false 48 | cacheFile: ../KITTI-07/cache/cache_07_d1.5_fuse_wide.txt 49 | verborse: true 50 | use_plane: true # set to false for the ablation study -------------------------------------------------------------------------------- /config/calib/README: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /config/loam/KITTI.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | scan_line: 64 4 | lidar_type: HDL64 5 | scan_period: 0.1 6 | minimum_range: 0.3 7 | 8 | voxel_size: [0.2, 0.2, 0.2] 9 | stack_size: 400000 10 | -------------------------------------------------------------------------------- /config/loam/backend.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | gui: 4 | verborse: true 5 | vis: false 6 | 7 | 8 | odom: 9 | voxel: 0.2 10 | o3d_voxel: 0.2 11 | keyframeMeterGap: 1.5 12 | keyframeRadGap: 0.15 13 | icp_corase_dist: 1.0 # 3.0 14 | icp_refine_dist: 0.3 15 | icp_max_iter: 30 16 | 17 | loopclosure: 18 | LCkeyframeMeterGap: 3 # 10 19 | LCkeyframeRadGap: 1 # 2 20 | LCSubmapSize: 25 # 10 21 | loopOverlapThre: 0.5 # 0.7 22 | loopInlierRMSEThre: 0.2 23 | 24 | multiway: 25 | use: false 26 | MRmaxIter: 100 27 | MRmaxCorrDist: 1.2 28 | MREdgePruneThre: 0.25 29 | RefineOdom: true 30 | 31 | io: 32 | isam_poses: ../KITTI-07/slam_res/floam_isam_07.txt 33 | mr_graph: ../KITTI-07/slam_res/floam_pg.json 34 | mr_poses: ../KITTI-07/slam_res/floam_mr_07.txt 35 | save_map: false 36 | map_path: ../KITTI-07/slam_res/floam_map_07.pcd -------------------------------------------------------------------------------- /config/orb/save_localmap.yml: -------------------------------------------------------------------------------- 1 | orb_setting: 2 | vocabulary: ../data/Vocabulary/ORBvoc.txt 3 | setting: ../config/orb_ori/KITTI00-02.yaml 4 | 5 | io: 6 | KeyFrameDir: ../KITTI-00/KeyFrames/ 7 | MapFile: ../KITTI-00/Map.yml 8 | SavePath: ../KITTI-00/slam_res/orb_map_00.pcd -------------------------------------------------------------------------------- /config/orb/store.yml: -------------------------------------------------------------------------------- 1 | root_path: /home/bit/CODE/Research/AutoCalib/ST_Calib/ 2 | io: 3 | seq_dir: data/00/ 4 | save_pose: KITTI_00/slam_res/Twc.txt 5 | keyframe_dir: KITTI_00/KeyFrames/ 6 | MapFile: KITTI_00/Map.yml 7 | orb: 8 | setting: config/orb_ori/KITTI00-02.yaml 9 | Vocabulary: data/Vocabulary/ORBvoc.txt 10 | image_dir: image_0/ 11 | timestamp: times.txt 12 | vis: true -------------------------------------------------------------------------------- /config/orb_ori/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 458.654 9 | Camera.fy: 457.296 10 | Camera.cx: 367.215 11 | Camera.cy: 248.375 12 | 13 | Camera.k1: -0.28340811 14 | Camera.k2: 0.07395907 15 | Camera.p1: 0.00019359 16 | Camera.p2: 1.76187114e-05 17 | 18 | # Camera frames per second 19 | Camera.fps: 20.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #--------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /config/orb_ori/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize: 2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /config/orb_ori/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize: 2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /config/orb_ori/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize: 2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /config/orb_ori/KITTI19.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.8560 9 | Camera.fy: 718.8560 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize: 2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /config/orb_ori/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize: 2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /config/orb_ori/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize: 2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /config/orb_ori/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize: 2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /config/settings.sh: -------------------------------------------------------------------------------- 1 | export LD_LIBRARY_PATH="" -------------------------------------------------------------------------------- /doc/abstract.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/abstract.jpg -------------------------------------------------------------------------------- /doc/ca_init_ann.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/ca_init_ann.png -------------------------------------------------------------------------------- /doc/ca_opt_ann.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/ca_opt_ann.png -------------------------------------------------------------------------------- /doc/cba_init_ann.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/cba_init_ann.png -------------------------------------------------------------------------------- /doc/cba_opt_ann.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/cba_opt_ann.png -------------------------------------------------------------------------------- /doc/proj_00_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_00_gt.png -------------------------------------------------------------------------------- /doc/proj_00_pred.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_00_pred.png -------------------------------------------------------------------------------- /doc/proj_02_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_02_gt.png -------------------------------------------------------------------------------- /doc/proj_02_pred.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_02_pred.png -------------------------------------------------------------------------------- /doc/proj_03_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_03_gt.png -------------------------------------------------------------------------------- /doc/proj_03_pred.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_03_pred.png -------------------------------------------------------------------------------- /doc/proj_04_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_04_gt.png -------------------------------------------------------------------------------- /doc/proj_04_pred.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_04_pred.png -------------------------------------------------------------------------------- /doc/proj_05_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_05_gt.png -------------------------------------------------------------------------------- /doc/proj_05_pred.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_05_pred.png -------------------------------------------------------------------------------- /doc/proj_07_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_07_gt.png -------------------------------------------------------------------------------- /doc/proj_07_pred.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/proj_07_pred.png -------------------------------------------------------------------------------- /doc/traj_00.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/traj_00.jpeg -------------------------------------------------------------------------------- /doc/traj_02.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/traj_02.jpeg -------------------------------------------------------------------------------- /doc/traj_03.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/traj_03.jpeg -------------------------------------------------------------------------------- /doc/traj_04.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/traj_04.jpeg -------------------------------------------------------------------------------- /doc/traj_05.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/traj_05.jpeg -------------------------------------------------------------------------------- /doc/traj_07.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/doc/traj_07.jpeg -------------------------------------------------------------------------------- /include/GeoCalib.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "orb_slam/include/KeyFrame.h" 4 | #include 5 | #include "cv_tools.hpp" 6 | 7 | void TransformPointCloudInplace(std::vector &pointCloud, const Eigen::Isometry3d &transformation){ 8 | for(std::uint32_t i = 0; i< (std::uint32_t) pointCloud.size(); ++i) 9 | pointCloud[i] = transformation * pointCloud[i]; 10 | } 11 | 12 | void TransformationPointCloud(const std::vector &srcPointCloud, std::vector &tgtPointCloud, const Eigen::Isometry3d &transformation){ 13 | tgtPointCloud.resize(srcPointCloud.size()); 14 | for(std::uint32_t i = 0; i *srcPointCloud, const std::vector *tgtPointCloud, 19 | std::vector> &corrSet, const double maxDistance = 0.05){ 20 | 21 | typedef nanoflann::KDTreeSingleIndexAdaptor>, std::vector, 3, std::uint32_t> KDTreeType; 22 | 23 | std::unique_ptr kdtree(new KDTreeType(3, *tgtPointCloud, {15})); // max_leaf_size = 15 24 | for(std::uint32_t i = 0; i < srcPointCloud->size(); ++i){ 25 | std::uint32_t num_res = 1; 26 | std::vector query_index(num_res); 27 | std::vector sq_dist(num_res, 1000); 28 | num_res = kdtree->knnSearch(srcPointCloud->at(i).data(), num_res, query_index.data(), sq_dist.data()); 29 | if(num_res > 0 && sq_dist[0] <= maxDistance) 30 | corrSet.push_back(std::make_pair(i, query_index[0])); 31 | } 32 | } 33 | 34 | void computeCorrespondenceList(const std::vector* > &srcPointCloudList, const std::vector *tgtPointCloud, 35 | std::vector>* > &corrSetList, const double maxDistance = 0.05){ 36 | 37 | typedef nanoflann::KDTreeSingleIndexAdaptor>, std::vector, 3, std::uint32_t> KDTreeType; 38 | 39 | std::unique_ptr kdtree(new KDTreeType(3, *tgtPointCloud, {15})); // max_leaf_size = 15 40 | corrSetList.reserve(srcPointCloudList.size()); 41 | for(std::uint32_t cloud_i = 0; cloud_i < srcPointCloudList.size(); ++ cloud_i){ 42 | std::vector>* corrSet(new std::vector>); 43 | for(std::uint32_t i = 0; i < srcPointCloudList[cloud_i]->size(); ++i){ 44 | std::uint32_t num_res = 1; 45 | std::vector query_index(num_res); 46 | std::vector sq_dist(num_res, 1000); 47 | num_res = kdtree->knnSearch(srcPointCloudList[cloud_i]->at(i).data(), num_res, query_index.data(), sq_dist.data()); 48 | if(num_res > 0 && sq_dist[0] <= maxDistance) 49 | corrSet->push_back(std::make_pair(i, query_index[0])); 50 | } 51 | corrSetList.push_back(corrSet); 52 | } 53 | } 54 | 55 | 56 | 57 | cv::Mat ComputeF12(ORB_SLAM2::KeyFrame *&pKF1, ORB_SLAM2::KeyFrame *&pKF2) 58 | { 59 | cv::Mat R1w = pKF1->GetRotation(); 60 | cv::Mat t1w = pKF1->GetTranslation(); 61 | cv::Mat R2w = pKF2->GetRotation(); 62 | cv::Mat t2w = pKF2->GetTranslation(); 63 | 64 | cv::Mat R12 = R1w*R2w.t(); 65 | cv::Mat t12 = -R1w*R2w.t()*t2w+t1w; 66 | 67 | cv::Mat t12x = SkewSymmetricMatrix(t12); 68 | 69 | const cv::Mat &K1 = pKF1->mK; 70 | const cv::Mat &K2 = pKF2->mK; 71 | 72 | 73 | return K1.t().inv()*t12x*R12*K2.inv(); 74 | } 75 | 76 | void computeInitialGeoError(const std::vector* > pointCloudList, const std::vector KFList, 77 | const std::vector &poseList, const double maxDistance = 0.05){ 78 | std::vector* > pointCloudTransformedList; 79 | std::cout << "Transforming PointCloud to World Coordinate System" << std::endl; 80 | for(std::uint32_t i = 0; i < pointCloudList.size(); ++i){ 81 | std::vector* pointCloudTransformed(new std::vector); 82 | TransformationPointCloud(*pointCloudList[i], *pointCloudTransformed, std::ref(poseList[i])); 83 | pointCloudTransformedList.push_back(pointCloudTransformed); 84 | } 85 | std::cout << "Build Correspondences between Transformed PointClouds based on ORB_SLAM Edge Connection" << std::endl; 86 | // Map mnId to Index in KFList 87 | std::unordered_map KFidMap; 88 | for(std::uint32_t KFi = 0; KFi < KFList.size(); ++ KFi) 89 | KFidMap.insert(std::make_pair(KFList[KFi]->mnId, KFi)); 90 | // Construct Lists of Correspondence Set for Each PointCloud 91 | for(std::uint32_t KFi = 0; KFi < KFList.size(); ++ KFi){ 92 | const std::vector vpNeighKFs = KFList[KFi]->GetBestCovisibilityKeyFrames(20); 93 | std::vector* > subPointCloudTransformedList; 94 | // Add Corresponding PointClouds to a List and Compute Correspondences One to One; 95 | for(ORB_SLAM2::KeyFrame* KF: vpNeighKFs){ 96 | auto it = KFidMap.find(KF->mnId); 97 | assert(it!=KFidMap.end()); // Should Not be Triggered 98 | subPointCloudTransformedList.push_back(pointCloudTransformedList[it->second]); 99 | } 100 | std::vector>* > ptCorrSetList; 101 | computeCorrespondenceList(subPointCloudTransformedList, pointCloudTransformedList[KFi], std::ref(ptCorrSetList), maxDistance); 102 | } 103 | 104 | 105 | } -------------------------------------------------------------------------------- /include/HECalib.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | /** 6 | * @brief Hand-eye calibration (from B to A) 7 | * 8 | * @param TaList Motions of Sensor A 9 | * @param TbList Motions of Sensor B 10 | * @return Eigen::Matrix4d TAB (transformation from B to A) 11 | */ 12 | std::tuple HECalib(const std::vector &TaList, const std::vector &TbList){ 13 | assert(TaList.size()==TbList.size()); 14 | std::vector alphaList, betaList; 15 | Eigen::MatrixX4d A(3*TaList.size(), 4); // 3N, 4 16 | Eigen::VectorXd b(3*TaList.size()); // 3N 17 | Eigen::Vector3d alpha_mean, beta_mean; 18 | alpha_mean.setZero(); 19 | beta_mean.setZero(); 20 | for(std::size_t i = 0; i < TaList.size(); ++i){ 21 | // Ta 22 | Eigen::AngleAxisd ax; 23 | ax.fromRotationMatrix(TaList[i].rotation()); 24 | Eigen::Vector3d alpha = ax.angle() * ax.axis(); 25 | alphaList.push_back(alpha); 26 | // Tb 27 | alpha_mean += alpha; 28 | ax.fromRotationMatrix(TbList[i].rotation()); 29 | Eigen::Vector3d beta = ax.angle() * ax.axis(); 30 | betaList.push_back(beta); 31 | beta_mean += beta; 32 | } 33 | alpha_mean /= TaList.size(); 34 | beta_mean /= TbList.size(); 35 | // Decentralization and Compute Covariance Matrix 36 | Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); 37 | for(std::vector::const_iterator ita=alphaList.begin(), itb=betaList.begin(); ita!=alphaList.end() ;++ita, ++itb){ 38 | H += (*itb - beta_mean) * (*ita - alpha_mean).transpose(); // (3,1) x (1,3) -> (3,3) 39 | } 40 | // Computate Rotation Part RAB 41 | Eigen::JacobiSVD svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); 42 | Eigen::Matrix3d Ut = svd.matrixU().transpose(), Vt = svd.matrixV().transpose(); 43 | Eigen::Matrix3d RAB = Vt.transpose() * Ut; 44 | if(RAB.determinant() < 0){ 45 | Vt.row(2) *= -1; 46 | RAB = Vt.transpose() * Ut; 47 | } 48 | // Compute Translation Part tAB 49 | for(std::size_t i = 0; i < TaList.size(); ++i){ 50 | A.block<3, 3>(3*i, 0) = TaList[i].rotation() - Eigen::Matrix3d::Identity(); 51 | A.block<3, 1>(3*i, 3) = TaList[i].translation(); 52 | b.block<3, 1>(3*i, 0) = RAB * TbList[i].translation(); 53 | } 54 | Eigen::Vector4d res = (A.transpose() * A).ldlt().solve(A.transpose() * b); // use cholesky decompostion to solve this problem with large 55 | return std::make_tuple(RAB, res.block<3, 1>(0, 0), res(3)); 56 | 57 | } 58 | 59 | /** 60 | * @brief Hand-eye calibration (from B to A) used in degenerated scene 61 | * 62 | * @param TaList Motions of Sensor A 63 | * @param TbList Motions of Sensor B 64 | * @return Eigen::Matrix4d TAB (transformation from B to A) 65 | */ 66 | std::tuple DGHECalib(const std::vector &TaList, const std::vector &TbList, const double &dg_threshold=0.01){ 67 | assert(TaList.size()==TbList.size()); 68 | std::vector alphaList, betaList; 69 | Eigen::MatrixX4d A(3*TaList.size(), 4); // 3N, 4 70 | Eigen::VectorXd b(3*TaList.size()); // 3N 71 | Eigen::Vector3d alpha_mean, beta_mean; 72 | alpha_mean.setZero(); 73 | beta_mean.setZero(); 74 | std::vector tAnorm, tBnorm; 75 | for(std::size_t i = 0; i < TaList.size(); ++i){ 76 | // Ta 77 | Eigen::AngleAxisd ax; 78 | ax.fromRotationMatrix(TaList[i].rotation()); 79 | Eigen::Vector3d alpha = ax.angle() * ax.axis(); 80 | alphaList.push_back(alpha); 81 | if(alpha.norm() < dg_threshold) 82 | { 83 | tAnorm.push_back(TaList[i].translation().norm()); 84 | tBnorm.push_back(TbList[i].translation().norm()); 85 | } 86 | // Tb 87 | alpha_mean += alpha; 88 | ax.fromRotationMatrix(TbList[i].rotation()); 89 | Eigen::Vector3d beta = ax.angle() * ax.axis(); 90 | betaList.push_back(beta); 91 | beta_mean += beta; 92 | } 93 | alpha_mean /= TaList.size(); 94 | beta_mean /= TbList.size(); 95 | // Decentralization and Compute Covariance Matrix 96 | Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); 97 | for(std::vector::const_iterator ita=alphaList.begin(), itb=betaList.begin(); ita!=alphaList.end() ;++ita, ++itb){ 98 | H += (*itb - beta_mean) * (*ita - alpha_mean).transpose(); // (3,1) x (1,3) -> (3,3) 99 | } 100 | // Computate Rotation Part RAB 101 | Eigen::JacobiSVD svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); 102 | Eigen::Matrix3d Ut = svd.matrixU().transpose(), Vt = svd.matrixV().transpose(); 103 | Eigen::Matrix3d RAB = Vt.transpose() * Ut; 104 | if(RAB.determinant() < 0){ 105 | Vt.row(2) *= -1; 106 | RAB = Vt.transpose() * Ut; 107 | } 108 | // Do NOT Compute Translation Part tAB 109 | Eigen::Vector3d tAB = Eigen::Vector3d::Zero(); 110 | double scale; 111 | double num = 0, den = 0; 112 | for(std::size_t i = 0; i < tAnorm.size(); ++i) 113 | { 114 | num += tAnorm[i] * tBnorm[i]; 115 | den += tAnorm[i] * tAnorm[i]; 116 | } 117 | std::printf("Toatal Frames: %ld, Degenerated Frames: %ld",TaList.size(), tAnorm.size()); 118 | return std::make_tuple(RAB, tAB, num / den); 119 | 120 | } -------------------------------------------------------------------------------- /include/IBAPlane.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitouni/Spatial-Temporal-LiDAR-camera-Calibration/3c14ab2562199c4183cec8fc636174568635c63b/include/IBAPlane.hpp -------------------------------------------------------------------------------- /include/KDTreeVectorOfVectorsAdaptor.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | // ===== This example shows how to use nanoflann with these types of containers: 35 | // using my_vector_of_vectors_t = std::vector > ; 36 | // 37 | // The next one requires #include 38 | // using my_vector_of_vectors_t = std::vector ; 39 | // ============================================================================= 40 | 41 | 42 | namespace nanoflann{ 43 | 44 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the 45 | * storage. The i'th vector represents a point in the state space. 46 | * 47 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality 48 | * for the points in the data set, allowing more compiler optimizations. 49 | * \tparam num_t The type of the point coordinates (typ. double or float). 50 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, 51 | * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 52 | * \tparam IndexType The type for indices in the KD-tree index 53 | * (typically, size_t of int) 54 | */ 55 | template < 56 | class VectorOfVectorsType, typename num_t = double, int DIM = -1, 57 | class Distance = nanoflann::metric_L2, typename IndexType = size_t> 58 | struct KDTreeVectorOfVectorsAdaptor 59 | { 60 | using self_t = 61 | KDTreeVectorOfVectorsAdaptor; 62 | using metric_t = 63 | typename Distance::template traits::distance_t; 64 | using index_t = 65 | nanoflann::KDTreeSingleIndexAdaptor; 66 | 67 | /** The kd-tree index for the user to call its methods as usual with any 68 | * other FLANN index */ 69 | index_t* index = nullptr; 70 | 71 | /// Constructor: takes a const ref to the vector of vectors object with the 72 | /// data points 73 | /** 74 | * @brief Constructor: takes a const ref to the vector of vectors object with the 75 | * data points 76 | * @param size_t DIM 77 | * @param mat Points to be constructed as KDTree 78 | * @param leaf_max_size maximum Kdtree leaf size 79 | * @param n_thread_build use multiple threads to build kdtree 80 | */ 81 | KDTreeVectorOfVectorsAdaptor( 82 | const size_t /* dimensionality */, const VectorOfVectorsType& mat, 83 | const int leaf_max_size = 10, const unsigned int n_thread_build = 1) 84 | : m_data(mat) 85 | { 86 | assert(mat.size() != 0 && mat[0].size() != 0); 87 | const size_t dims = mat[0].size(); 88 | if (DIM > 0 && static_cast(dims) != DIM) 89 | throw std::runtime_error( 90 | "Data set dimensionality does not match the 'DIM' template " 91 | "argument"); 92 | index = new index_t( 93 | static_cast(dims), *this /* adaptor */, 94 | nanoflann::KDTreeSingleIndexAdaptorParams( 95 | leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, 96 | n_thread_build)); 97 | } 98 | 99 | ~KDTreeVectorOfVectorsAdaptor() { delete index; } 100 | 101 | const VectorOfVectorsType& m_data; 102 | 103 | /** Query for the \a num_closest closest points to a given point 104 | * (entered as query_point[0:dim-1]). 105 | * Note that this is a short-cut method for index->findNeighbors(). 106 | * The user can also call index->... methods as desired. 107 | */ 108 | inline void query( 109 | const num_t* query_point, const size_t num_closest, 110 | IndexType* out_indices, num_t* out_distances_sq) const 111 | { 112 | nanoflann::KNNResultSet resultSet(num_closest); 113 | resultSet.init(out_indices, out_distances_sq); 114 | index->findNeighbors(resultSet, query_point); 115 | } 116 | 117 | /** @name Interface expected by KDTreeSingleIndexAdaptor 118 | * @{ */ 119 | 120 | const self_t& derived() const { return *this; } 121 | self_t& derived() { return *this; } 122 | 123 | // Must return the number of data points 124 | inline size_t kdtree_get_point_count() const { return m_data.size(); } 125 | 126 | // Returns the dim'th component of the idx'th point in the class: 127 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const 128 | { 129 | return m_data[idx][dim]; 130 | } 131 | 132 | // Optional bounding-box computation: return false to default to a standard 133 | // bbox computation loop. 134 | // Return true if the BBOX was already computed by the class and returned 135 | // in "bb" so it can be avoided to redo it again. Look at bb.size() to 136 | // find out the expected dimensionality (e.g. 2 or 3 for point clouds) 137 | template 138 | bool kdtree_get_bbox(BBOX& /*bb*/) const 139 | { 140 | return false; 141 | } 142 | 143 | /** @} */ 144 | 145 | }; // end of KDTreeVectorOfVectorsAdaptor 146 | } -------------------------------------------------------------------------------- /include/color_gradient.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "pointcloud.h" 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | /** 9 | * @brief PaintPointClor through projection 10 | * 11 | * @param proj_points (u,v) of project points 12 | * @param img cv::uint_8 gray-scale image 13 | * @param weightsum_threshold if the distance to the nearest round pixel is lower than this value, do not use weighted inv-distance to compute 14 | * @param max_intensity 255 for UINT_8 Format Image 15 | * @return std::vector 16 | */ 17 | std::vector PaintPointCloud(const VecVector2d &proj_points, 18 | const cv::Mat &img, const int weightsum_threshold=0.1, const double max_intensity=255.) 19 | { 20 | std::unordered_map res; 21 | std::vector proj_intensity; 22 | const int H = img.rows, W = img.cols; 23 | for(int proj_i = 0; proj_i < static_cast(proj_points.size()); ++proj_i) 24 | { 25 | double u = proj_points[proj_i].x(); 26 | double v = proj_points[proj_i].y(); 27 | int intu = std::round(u); 28 | int intv = std::round(v); 29 | double dist = sqrt((u-intu)*(u-intu) + (v-intv)*(v-intv)); 30 | if(dist < weightsum_threshold) 31 | { 32 | proj_intensity[proj_i] = static_cast(img.at(intv, intu)) / max_intensity; 33 | }else 34 | { 35 | int minX = std::max(0, intu-1); 36 | int maxX = std::min(W, intu+1); 37 | int minY = std::max(0, intv-1); 38 | int maxY = std::min(H, intv+1); 39 | double invd_topleft = 1.0 / sqrt((u-minX)*(u-minX)+(v-minY)*(v-minY)); 40 | double invd_topright = 1.0 / sqrt((u-maxX)*(u-maxX)+(v-minY)*(v-minY)); 41 | double invd_bottomleft = 1.0 / sqrt((u-minX)*(u-minX)+(v-maxY)*(v-maxY)); 42 | double invd_bottomright = 1.0 / sqrt((u-maxX)*(u-maxX)+(v-maxY)*(v-maxY)); 43 | double topleft = static_cast(img.at(minX,minY)); 44 | double topright = static_cast(img.at(maxX,minY)); 45 | double bottomleft = static_cast(img.at(minX,maxY)); 46 | double bottomright = static_cast(img.at(maxX,maxY)); 47 | proj_intensity[proj_i] = 48 | ((invd_topleft * topleft) + (invd_topright * topright) + (invd_bottomleft * bottomleft) + (invd_bottomright * bottomright)) / 49 | (invd_topleft + invd_topright + invd_bottomleft + invd_bottomright) / max_intensity; 50 | } 51 | } 52 | return proj_intensity; 53 | } 54 | 55 | 56 | /** 57 | * @brief Build Color Gradient for project points 58 | * 59 | * @param points Raw Lidar Points 60 | * @param proj_points 61 | * @param proj_indices 62 | * @param img 63 | * @param weightsum_threshold 64 | * @param max_intensity 65 | * @return std::unordered_map 66 | */ 67 | std::unordered_map buildColorGradient(const VecVector3d &points, const KDTree3D *kdtree, 68 | const VecVector2d &proj_points, const VecIndex &proj_indices, 69 | const cv::Mat &img, const int weightsum_threshold=0.1, const double max_intensity=255., 70 | double normal_radius=0.6, int normal_max_nn=30, int normal_min_nn=5) 71 | { 72 | assert(proj_points.size() == proj_indices.size()); 73 | std::vector instensity = PaintPointCloud(proj_points, img, weightsum_threshold, max_intensity); 74 | VecIndex valid_indices; 75 | VecVector3d normals; 76 | std::vector neigh_indices; 77 | std::tie(normals, neigh_indices, valid_indices) = ComputeLocalNormalAndNeighbor(points, kdtree, proj_indices, normal_radius, normal_max_nn, normal_min_nn); 78 | } -------------------------------------------------------------------------------- /include/cv_tools.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class CameraParams{ 8 | public: 9 | cv::Mat K; // Intrinsic Parameters: 3x3 10 | cv::Mat DistCoef; // Distortion Parameters 11 | float fps; 12 | int nRGB; 13 | public: 14 | CameraParams(const std::string strSettingPath):K(cv::Mat::eye(3,3,CV_32F)), DistCoef(4,1,CV_32F){ 15 | cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); 16 | if(!fSettings.isOpened()){ 17 | throw std::runtime_error("Setting File "+strSettingPath+" Cannot open."); 18 | return; 19 | } 20 | float fx = fSettings["Camera.fx"]; 21 | float fy = fSettings["Camera.fy"]; 22 | float cx = fSettings["Camera.cx"]; 23 | float cy = fSettings["Camera.cy"]; 24 | 25 | cv::Mat K = cv::Mat::eye(3,3,CV_32F); 26 | K.at(0,0) = fx; 27 | K.at(1,1) = fy; 28 | K.at(0,2) = cx; 29 | K.at(1,2) = cy; 30 | cv::Mat DistCoef(4,1,CV_32F); 31 | DistCoef.at(0) = fSettings["Camera.k1"]; 32 | DistCoef.at(1) = fSettings["Camera.k2"]; 33 | DistCoef.at(2) = fSettings["Camera.p1"]; 34 | DistCoef.at(3) = fSettings["Camera.p2"]; 35 | const float k3 = fSettings["Camera.k3"]; 36 | if(k3!=0) 37 | { 38 | DistCoef.resize(5); 39 | DistCoef.at(4) = k3; 40 | } 41 | fps = fSettings["Camera.fps"]; 42 | if(fps==0) 43 | fps=30; 44 | nRGB = fSettings["Camera.RGB"]; 45 | } 46 | void UndistortPoints(cv::InputArray InputPoints, cv::OutputArray OutputPoints){ 47 | cv::undistortPoints(InputPoints, OutputPoints, K, DistCoef); 48 | } 49 | void ProjectPoints(cv::InputArray InputPoints, cv::OutputArray OutputPoints, bool Distortion){ 50 | cv::Mat rvec(1,3,CV_32F,0.0); 51 | cv::Mat tvec(1,3,CV_32F,0.0); 52 | if(!Distortion){ 53 | cv::projectPoints(InputPoints,rvec,tvec, K, cv::noArray(), OutputPoints); 54 | } 55 | else 56 | cv::projectPoints(InputPoints,rvec,tvec, K, DistCoef, OutputPoints); 57 | } 58 | void ProjectPoints(cv::InputArray InputPoints, cv::OutputArray OutputPoints, cv::InputArray rvec, cv::InputArray tvec, bool Distortion){ 59 | if(!Distortion){ 60 | cv::projectPoints(InputPoints, rvec, tvec, K, cv::noArray(), OutputPoints); 61 | } 62 | else 63 | cv::projectPoints(InputPoints, rvec, tvec, K, DistCoef, OutputPoints); 64 | } 65 | }; 66 | 67 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v) 68 | { 69 | return (cv::Mat_(3,3) << 70 | 0, -v.at(2), v.at(1), 71 | v.at(2), 0, -v.at(0), 72 | -v.at(1), v.at(0), 0); 73 | } -------------------------------------------------------------------------------- /include/kitti_tools.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include // F_OK 6 | #include // access 7 | #include // makdir 8 | #include 9 | #include 10 | #include 11 | 12 | void sort_string(std::vector &in_array) 13 | { 14 | sort(in_array.begin(), in_array.end()); 15 | } 16 | 17 | 18 | inline void checkpath(std::string &path){ 19 | if(path[path.size()-1] != '/') 20 | path = path + "/"; 21 | } 22 | 23 | inline bool file_exist(const std::string path){ 24 | return access(path.c_str(), F_OK) != -1; 25 | } 26 | 27 | void makedir(const char* folder){ 28 | if(access(folder,F_OK)){ 29 | if(mkdir(folder,0755)!=0) 30 | printf("\033[1;33mDirectory %s created Failed with unknown error!\033[0m\n",folder); 31 | else 32 | printf("\033[1;34mDirectory %s created successfully!\033[0m\n",folder); 33 | }else 34 | printf("\033[1;35mDirectory %s not accessible or alreadly exists!\033[0m\n",folder); 35 | } 36 | 37 | int remove_directory(const char *path) { 38 | try{ 39 | char cmd[100]; 40 | sprintf(cmd,"rm -rf %s",path); 41 | return system(cmd); 42 | }catch(const std::exception &e){ 43 | std::cout << e.what() << std::endl; 44 | return -1; 45 | } 46 | 47 | } 48 | 49 | void listdir(std::vector &filenames, const std::string &dirname){ 50 | filenames.clear(); 51 | DIR *dirp = nullptr; 52 | struct dirent *dir_entry = nullptr; 53 | if((dirp=opendir(dirname.c_str()))==nullptr){ 54 | throw std::runtime_error("Cannot open directory"); 55 | } 56 | while((dir_entry=readdir(dirp))!=nullptr){ 57 | if(dir_entry->d_type!=DT_REG) 58 | continue; 59 | filenames.push_back(dir_entry->d_name); 60 | } 61 | closedir(dirp); 62 | sort_string(filenames); 63 | } 64 | 65 | 66 | void ReadPoseList(const std::string &fileName, std::vector &vPose){ 67 | if(!file_exist(fileName)){ 68 | throw std::runtime_error("Cannot open file: "+ fileName); 69 | return; 70 | } 71 | std::ifstream ifs(fileName); 72 | while(ifs.peek()!=EOF){ 73 | double data[16]; 74 | for(int i=0; i<12; ++i){ 75 | ifs >> data[i]; 76 | } 77 | data[12] = 0.0; 78 | data[13] = 0.0; 79 | data[14] = 0.0; 80 | data[15] = 1.0; 81 | Eigen::Map mat4d(data); 82 | Eigen::Isometry3d mat; // 83 | mat.matrix() = mat4d.matrix().transpose(); 84 | vPose.push_back(mat); 85 | } 86 | ifs.close(); 87 | } 88 | 89 | /** 90 | * @brief write 13 entries: 12 for 3x4 Rigid, 1 for scale 91 | * 92 | * @param outfile File to Write (ASCII) 93 | * @param pose Rigid Transformation Matrix 94 | * @param scale Monocular Scale Factor 95 | */ 96 | void writeSim3(const std::string &outfile, const Eigen::Isometry3d &pose, const double scale){ 97 | std::ofstream of(outfile, std::ios::ate); 98 | of.precision(std::numeric_limits< double >::max_digits10); 99 | for(Eigen::Index ri=0; ri<3; ++ri) 100 | for(Eigen::Index ci=0; ci<4; ++ci){ 101 | of << pose(ri,ci) << " "; 102 | } 103 | of << scale; 104 | of.close(); 105 | } 106 | 107 | /** 108 | * @brief write 13 entries: 12 for 3x4 Rigid, 1 for scale 109 | * 110 | * @param outfile File to Write (ASCII) 111 | * @param pose Rigid Transformation Matrix 112 | * @param scale Monocular Scale Factor 113 | */ 114 | void writeSim3(const std::string &outfile, const Eigen::Matrix4d &pose, const double scale){ 115 | std::ofstream of(outfile, std::ios::ate); 116 | of.precision(std::numeric_limits< double >::max_digits10); 117 | for(Eigen::Index ri=0; ri<3; ++ri) 118 | for(Eigen::Index ci=0; ci<4; ++ci){ 119 | of << pose(ri,ci) << " "; 120 | } 121 | of << scale; 122 | of.close(); 123 | } 124 | 125 | /** 126 | * @brief write 13 entries: 12 for 3x4 Rigid, 1 for scale 127 | * 128 | * @param outfile File to Write (ASCII) 129 | * @param R Rotation 130 | * @param t Translation 131 | * @param scale scale 132 | */ 133 | void writeSim3(const std::string &outfile, const Eigen::Matrix3d &R, const Eigen::Vector3d &t, const double scale){ 134 | std::ofstream of(outfile, std::ios::ate); 135 | of.precision(std::numeric_limits< double >::max_digits10); 136 | for(Eigen::Index ri=0; ri<3; ++ri) 137 | { 138 | for(Eigen::Index ci=0; ci<3; ++ci){ 139 | of << R(ri,ci) << " "; 140 | } 141 | of << t(ri) << " "; 142 | } 143 | of << scale; 144 | of.close(); 145 | } 146 | 147 | std::tuple readSim3(const std::string &file){ 148 | std::ifstream ifs(file); 149 | double scale = 1.0; 150 | Eigen::Matrix4d mat(Eigen::Matrix4d::Identity()); 151 | for(Eigen::Index ri=0; ri<3; ++ri) 152 | for(Eigen::Index ci=0; ci<4; ++ci){ 153 | ifs >> mat(ri,ci); 154 | } 155 | ifs >> scale; 156 | ifs.close(); 157 | return {mat, scale}; 158 | } 159 | 160 | void pose2Motion(std::vector &vAbsPose, std::vector &vRelPose){ 161 | for(std::size_t i = 0; i < vAbsPose.size()-1; ++i){ 162 | Eigen::Isometry3d mTwc(vAbsPose[i+1] * vAbsPose[i].inverse()); // T(i+1) * T(i).inverse() 163 | vRelPose.push_back(mTwc); 164 | } 165 | } -------------------------------------------------------------------------------- /src/examples/ba_calib.cpp: -------------------------------------------------------------------------------- 1 | #include "HECalib.h" 2 | #include "NLHECalib.hpp" 3 | #include 4 | #include 5 | #include 6 | #include "kitti_tools.h" 7 | #include 8 | #include "orb_slam/include/System.h" 9 | #include 10 | #include 11 | #include 12 | 13 | int main(int argc, char** argv){ 14 | if(argc < 2){ 15 | std::cerr << "Got " << argc - 1 << " Parameters, but Expected parameters: yaml_file" << std::endl; 16 | exit(0); 17 | } 18 | std::string config_file(argv[1]); 19 | YAML::Node config = YAML::LoadFile(config_file); 20 | std::string base_dir = config["BaseDir"].as(); 21 | checkpath(base_dir); 22 | std::vector vTwc, vTwl, vTwlraw; 23 | std::string TwcFilename = base_dir + config["VOFile"].as(); 24 | std::string TwlFilename = base_dir + config["LOFile"].as(); 25 | std::string resFileName = base_dir + config["ResFile"].as(); 26 | std::string KeyFrameDir = base_dir + config["KeyFrameDir"].as(); 27 | std::string KyeFrameIdFile = base_dir + config["VOIdFile"].as(); 28 | std::string MapFileNmae = base_dir + config["MapFile"].as(); 29 | std::string initSim3File = base_dir + config["init_sim3"].as(); 30 | std::string VocFilename = config["ORBVocabulary"].as(); 31 | std::string ORBSetFilename = config["ORBConfig"].as(); 32 | assert(file_exist(TwcFilename)); 33 | assert(file_exist(TwlFilename)); 34 | assert(file_exist(initSim3File)); 35 | std::map method_options = config["method_options"].as >(); 36 | std::string method = config["method"].as(); 37 | assert(method_options.count(method)>0); 38 | std::cout << "Use " << method << " Bundle Adjustment for calibration." << std::endl; 39 | ReadPoseList(TwcFilename, vTwc); 40 | ReadPoseList(TwlFilename, vTwlraw); 41 | YAML::Node FrameIdCfg = YAML::LoadFile(KyeFrameIdFile); 42 | std::vector vKFFrameId = FrameIdCfg["mnFrameId"].as>(); 43 | for(auto &KFId:vKFFrameId) 44 | vTwl.push_back(vTwlraw[KFId]); 45 | vTwlraw.clear(); 46 | std::cout << "Load " << vTwc.size() << " Twc Pose files." << std::endl; 47 | std::cout << "Load " << vTwl.size() << " Twl Pose files." << std::endl; 48 | assert(vTwc.size()==vTwl.size()); 49 | std::vector vmTwc, vmTwl; 50 | vmTwc.reserve(vTwc.size()-1); 51 | vmTwl.reserve(vTwl.size()-1); 52 | pose2Motion(vTwc, vmTwc); 53 | pose2Motion(vTwl, vmTwl); 54 | Eigen::Matrix4d rigid; 55 | double s; 56 | std::tie(rigid, s) = readSim3(initSim3File); 57 | 58 | ORB_SLAM2::System VSLAM(VocFilename, ORBSetFilename, ORB_SLAM2::System::MONOCULAR, false); 59 | VSLAM.Shutdown(); // Do not need any thread 60 | VSLAM.RestoreSystemFromFile(KeyFrameDir, MapFileNmae); 61 | std::cout << "System Restored Successfully." << std::endl; 62 | std::cout << "Initial Extrinsic:\n"; 63 | std::cout << "Rotation: \n" << rigid.topLeftCorner(3,3) << std::endl; 64 | std::cout << "Translation: \n" << rigid.topRightCorner(3,1) << std::endl; 65 | std::cout << "s :" << s << std::endl; 66 | 67 | Eigen::Isometry3d TCL0; 68 | TCL0.matrix() = rigid; 69 | int ngood; 70 | if(method == "global"){ 71 | ngood = VSLAM.OptimizeExtrinsicGlobal(vTwl, TCL0, s); 72 | std::cout << "Bundle Adjustment Optimization (Global):\n"; 73 | std::cout << "Rotation: \n" << TCL0.rotation() << std::endl; 74 | std::cout << "Translation: \n" << TCL0.translation() << std::endl; 75 | std::cout<< "scale= "<< s <<" "<<"ngood= "< 5 | #include "argparse.hpp" 6 | 7 | 8 | int main(int argc, char** argv){ 9 | argparse::ArgumentParser parser("Back-end optimization of F-LOAM"); 10 | parser.add_argument("config").help("config file").required(); 11 | parser.add_argument("raw_pose").help("raw pose of the F-LOAM").required(); 12 | parser.add_argument("velo_dir").help("directory of velodyne pcd files").required(); 13 | parser.parse_args(argc, argv); 14 | std::string yaml_file(parser.get("config")); 15 | assert(file_exist(yaml_file)); 16 | std::string input_pose_file(parser.get("raw_pose")); 17 | std::string input_lidar_dir(parser.get("velo_dir")); 18 | 19 | auto option = BackEndOption(); 20 | YAML::Node config = YAML::LoadFile(yaml_file); 21 | YAML::Node gui_config = config["gui"]; 22 | YAML::Node io_config = config["io"]; 23 | YAML::Node odom_config = config["odom"]; 24 | YAML::Node loop_config = config["loopclosure"]; 25 | YAML::Node multiway_config = config["multiway"]; 26 | std::string output_pose_graph = io_config["mr_graph"].as(); 27 | std::string output_isam_poses = io_config["isam_poses"].as(); 28 | std::string output_mr_poses = io_config["mr_poses"].as(); 29 | bool save_map = io_config["save_map"].as(); 30 | std::string map_path = io_config["map_path"].as(); 31 | 32 | assert(output_pose_graph.substr(output_pose_graph.find_last_of('.') + 1) == "json"); // suffix of a Posegraph must be '.json', or your effort is in vain! 33 | 34 | option.verborse = gui_config["verborse"].as(); 35 | option.vis = gui_config["vis"].as(); 36 | 37 | option.voxel = odom_config["voxel"].as(); 38 | option.keyframeMeterGap = odom_config["keyframeMeterGap"].as(); 39 | option.keyframeRadGap = odom_config["keyframeRadGap"].as(); 40 | option.icp_corase_dist = odom_config["icp_corase_dist"].as(); 41 | option.icp_refine_dist = odom_config["icp_refine_dist"].as(); 42 | option.icp_max_iter = odom_config["icp_max_iter"].as(); 43 | 44 | option.LCkeyframeMeterGap = loop_config["LCkeyframeMeterGap"].as(); 45 | option.LCkeyframeRadGap = loop_config["LCkeyframeRadGap"].as(); 46 | option.LCSubmapSize = loop_config["LCSubmapSize"].as(); 47 | option.loopOverlapThre = loop_config["loopOverlapThre"].as(); 48 | option.loopInlierRMSEThre = loop_config["loopInlierRMSEThre"].as(); 49 | 50 | option.MRmaxIter = multiway_config["MRmaxIter"].as(); 51 | option.MRmaxCorrDist = multiway_config["MRmaxCorrDist"].as(); 52 | option.MREdgePruneThre = multiway_config["MREdgePruneThre"].as(); 53 | bool odom_refine = multiway_config["RefineOdom"].as(); 54 | bool use_multiway = multiway_config["use"].as(); 55 | std::cout << "Parameters have been loaded from " << yaml_file << "." << std::endl; 56 | BackEndOptimizer optimizer(option); 57 | std::thread p(&BackEndOptimizer::LoopClosureRegThread, &optimizer); 58 | optimizer.LoopClosureRun(input_pose_file, input_lidar_dir); 59 | optimizer.StopLoopClosure(); 60 | p.join(); 61 | optimizer.UpdateISAM(); 62 | optimizer.writePoses(output_isam_poses); 63 | std::cout << "isam optimized poses saved to " << output_isam_poses << std::endl; 64 | // *** Results of ISAM and Multway are too similar! *** 65 | if(use_multiway) 66 | { 67 | optimizer.MultiRegistration(odom_refine); 68 | optimizer.writePoseGraph(output_pose_graph); 69 | optimizer.UpdatePosesFromPG(); 70 | optimizer.writePoses(output_mr_poses); 71 | } 72 | if(save_map) 73 | optimizer.SaveMap(map_path); 74 | } -------------------------------------------------------------------------------- /src/examples/floam_kitti.cpp: -------------------------------------------------------------------------------- 1 | #include "floam/include/floamClass.h" 2 | #include "pcl/io/pcd_io.h" 3 | #include "kitti_tools.h" 4 | #include 5 | #include 6 | #include 7 | #include "argparse.hpp" 8 | 9 | template 10 | bool readPointCloud(pcl::PointCloud &point_cloud, const std::string filename); 11 | 12 | void writeKittiData(std::ofstream &fs, Eigen::Isometry3d &mat, bool end=false); 13 | 14 | int main(int argc, char **argv) 15 | { 16 | argparse::ArgumentParser parser("floam_kitti"); 17 | parser.add_argument("velo_dir").help("directory of velodyne pcd files").required(); 18 | parser.add_argument("output").help("file to write output poses").required(); 19 | parser.parse_args(argc, argv); 20 | std::string velo_dir(parser.get("velo_dir")), res_file(parser.get("output")); 21 | checkpath(velo_dir); 22 | if(!file_exist(velo_dir)){ 23 | throw std::runtime_error("Velodyne Directory: "+velo_dir+" does not exsit."); 24 | }; // velodyne/ must exist 25 | std::vector velo_files; 26 | listdir(velo_files,velo_dir); 27 | std::cout << "Found " << velo_files.size() << " velodyne files." << std::endl; 28 | std::ofstream res_stream(res_file.c_str(), std::ios::ate); // write mode | ASCII mode 29 | res_stream << std::setprecision(6); // fixed precision; 30 | FLOAM::System SLAM(FLOAM::HDL_64); 31 | std::cout << "Create FLOAM SLAM successfully." << std::endl; 32 | for(std::size_t i=0; i::Ptr laserCloudPt (new pcl::PointCloud); 35 | bool isok = readPointCloud(*laserCloudPt, velo_dir + velo_files[i]); 36 | if(!isok){ 37 | throw std::runtime_error("Fatal error in reading " + velo_files[i] + " , exit."); 38 | return -1; 39 | } 40 | char msg[60]; 41 | sprintf(msg, "Frame %06ld | %06ld: %ld points", i+1, velo_files.size(), laserCloudPt->size()); 42 | std::cout << msg << std::endl; 43 | Eigen::Isometry3d pose = SLAM.Track(laserCloudPt); 44 | writeKittiData(res_stream, pose, i==velo_files.size()-1); 45 | 46 | }catch(const std::exception &e){ 47 | std::cout << e.what() << std::endl; 48 | res_stream.close(); 49 | return -1; 50 | } 51 | 52 | } 53 | res_stream.close(); 54 | return 0; 55 | } 56 | 57 | template 58 | bool readPointCloud(pcl::PointCloud &point_cloud, const std::string filename) 59 | { 60 | std::string suffix = filename.substr(filename.find_last_of('.') + 1); 61 | if(suffix=="pcd"){ 62 | pcl::io::loadPCDFile(filename, point_cloud); 63 | return true; 64 | }else if(suffix=="bin"){ 65 | std::ifstream binfile(filename.c_str(),std::ios::binary); 66 | if(!binfile) 67 | { 68 | throw std::runtime_error("file " + filename + " cannot open"); 69 | return false; 70 | } 71 | else 72 | { 73 | while(1) 74 | { 75 | float s; 76 | PointType point; 77 | binfile.read((char*)&s,sizeof(float)); 78 | if(binfile.eof()) break; 79 | point.x = s; 80 | binfile.read((char*)&s,sizeof(float)); 81 | point.y = s; 82 | binfile.read((char*)&s,sizeof(float)); 83 | point.z = s; 84 | binfile.read((char*)&s,sizeof(float)); 85 | point.intensity = s; 86 | point_cloud.push_back(point); 87 | } 88 | } 89 | binfile.close(); 90 | return true; 91 | }else{ 92 | throw std::invalid_argument("Error read Type, got " + suffix +" ,must be .bin or .pcd"); 93 | return false; 94 | } 95 | } 96 | 97 | /** 98 | * @brief KITTI pose files must have 12 entries per row and no trailing delimiter at the end of the rows (space) 99 | * 100 | * @param fs // output file stream 101 | * @param mat // pose 3x4 or 4x4 102 | */ 103 | void writeKittiData(std::ofstream &fs, Eigen::Isometry3d &mat, bool end){ 104 | short cnt = 0; 105 | for(short i=0; i<3; ++i) 106 | for(short j=0; j<4; ++j){ 107 | fs << mat(i,j); 108 | ++cnt; 109 | if(cnt!=12) fs << " "; 110 | else if(!end) fs << "\n"; 111 | } 112 | } -------------------------------------------------------------------------------- /src/examples/he_calib.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "HECalib.h" 8 | #include "NLHECalib.hpp" 9 | #include "kitti_tools.h" 10 | #include "argparse.hpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | argparse::ArgumentParser parser("Hand-eye Calib"); 15 | parser.add_argument("config").help("config file for hand-eye calibration.").required(); 16 | parser.parse_args(argc, argv); 17 | std::string config_file(parser.get("config")); 18 | YAML::Node config = YAML::LoadFile(config_file); 19 | const YAML::Node &io_config = config["io"]; 20 | const YAML::Node &runtime_config = config["runtime"]; 21 | std::string base_dir = io_config["BaseDir"].as(); 22 | checkpath(base_dir); 23 | std::vector vTwc, vTwl, vTwlraw; 24 | const std::string TwcFilename = base_dir + io_config["VOFile"].as(); 25 | const std::string TwlFilename = base_dir + io_config["LOFile"].as(); 26 | const std::string resFileName = base_dir + io_config["ResFile"].as(); 27 | const std::string resRBFileName = base_dir + io_config["ResRBFile"].as(); 28 | const std::string resLPFileName = base_dir + io_config["ResLPFile"].as(); 29 | const std::string KyeFrameIdFile = base_dir + io_config["VOIdFile"].as(); 30 | 31 | const bool zero_translation = runtime_config["zero_translation"].as(); 32 | const double robust_kernel_size = runtime_config["robust_kernel_size"].as(); 33 | const bool regulation = runtime_config["regulation"].as(); 34 | const double regulation_weight = runtime_config["regulation_weight"].as(); 35 | const int inner_iter = runtime_config["inner_iter"].as(); 36 | const int ex_iter = runtime_config["ex_iter"].as(); 37 | const double init_mu = runtime_config["init_mu"].as(); 38 | const double min_mu = runtime_config["min_mu"].as(); 39 | const double divide_factor = runtime_config["divide_factor"].as(); 40 | const bool verborse = runtime_config["verborse"].as(); 41 | 42 | ReadPoseList(TwcFilename, vTwc); 43 | ReadPoseList(TwlFilename, vTwlraw); 44 | YAML::Node FrameIdCfg = YAML::LoadFile(KyeFrameIdFile); 45 | std::vector vKFFrameId = FrameIdCfg["mnFrameId"].as>(); 46 | vTwl.reserve(vKFFrameId.size()); 47 | if(vKFFrameId[0]==0) 48 | for(auto &KFId:vKFFrameId) 49 | vTwl.push_back(vTwlraw[KFId]); 50 | else 51 | { 52 | Eigen::Isometry3d refPose = vTwlraw[vKFFrameId[0]].inverse(); 53 | for(auto &KFId:vKFFrameId) 54 | vTwl.push_back(refPose * vTwlraw[KFId]); 55 | } 56 | vTwlraw.clear(); 57 | std::cout << "Load " << vTwc.size() << " Twc Pose files." << std::endl; 58 | std::cout << "Load " << vTwl.size() << " Twl Pose files." << std::endl; 59 | assert(vTwc.size()==vTwl.size()); 60 | 61 | std::vector vmTwc, vmTwl; 62 | vmTwc.reserve(vTwc.size()-1); 63 | vmTwl.reserve(vTwl.size()-1); 64 | pose2Motion(vTwc, vmTwc); 65 | pose2Motion(vTwl, vmTwl); 66 | 67 | Eigen::Matrix3d RCL; 68 | Eigen::Vector3d tCL; 69 | double s; 70 | 71 | std::tie(RCL,tCL,s) = HECalib(vmTwc, vmTwl); 72 | std::cout << "Ordinary Hand-eye Calibration:\n"; 73 | std::cout << "Rotation: \n" << RCL << std::endl; 74 | std::cout << "Translation: \n" << tCL << std::endl; 75 | std::cout << "s :" << s << std::endl; 76 | writeSim3(resFileName, RCL, tCL, s); 77 | if(zero_translation) 78 | tCL.setZero(); // Translation is too bad 79 | std::cout << "Result of Hand-eye Calibration saved to " << resFileName << std::endl; 80 | std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s, robust_kernel_size, regulation, regulation_weight, verborse); 81 | std::cout << "Robust Kernel Hand-eye Calibration with Regulation:\n"; 82 | std::cout << "Rotation: \n" << RCL << std::endl; 83 | std::cout << "Translation: \n" << tCL << std::endl; 84 | std::cout << "scale :" << s << std::endl; 85 | writeSim3(resRBFileName, RCL, tCL, s); 86 | 87 | std::cout << "Result of Hand-eye Calibration saved to " << resRBFileName << std::endl; 88 | std::tie(RCL,tCL,s) = HECalibLineProcessg2o(vmTwc, vmTwl, RCL, tCL, s, inner_iter, init_mu, divide_factor, min_mu, ex_iter, regulation, regulation_weight, verborse); 89 | std::cout << "Line Process Hand-eye Calibration:\n"; 90 | std::cout << "Rotation: \n" << RCL << std::endl; 91 | std::cout << "Translation: \n" << tCL << std::endl; 92 | std::cout << "s :" << s << std::endl; 93 | writeSim3(resLPFileName, RCL, tCL, s); 94 | } -------------------------------------------------------------------------------- /src/examples/icp_calib.cpp: -------------------------------------------------------------------------------- 1 | #include "open3d/geometry/PointCloud.h" 2 | #include "open3d/io/PointCloudIO.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | int main(int argc, char** argv) 11 | { 12 | if(argc < 2){ 13 | std::cerr << "Need config_file arg" << std::endl; 14 | exit(0); 15 | } 16 | YAML::Node config = YAML::LoadFile(argv[1]); 17 | YAML::Node io_config = config["io"]; 18 | YAML::Node runtime_config = config["runtime"]; 19 | YAML::Node vis_config = config["vis"]; 20 | const std::string lidar_pcd_filename = io_config["lidar_pcd"].as(); 21 | const std::string cam_pcd_filename = io_config["cam_pcd"].as(); 22 | const std::string init_sim3_filename = io_config["init_sim3"].as(); 23 | const std::string FrameIdFile = io_config["FrameId"].as(); 24 | const std::string savePath = io_config["savePath"].as(); 25 | 26 | const double icp_corr_dist = runtime_config["max_corr_dist"].as(); 27 | const int icp_max_iter = runtime_config["max_iter"].as(); 28 | 29 | const bool use_vis = vis_config["use_vis"].as(); 30 | const std::string vis_name = vis_config["name"].as(); 31 | const int height = vis_config["height"].as(); 32 | const int width = vis_config["width"].as(); 33 | const std::vector cam_color = vis_config["cam_pcd_color"].as >(); 34 | const std::vector lidar_color = vis_config["lidar_pcd_color"].as >(); 35 | 36 | YAML::Node FrameIdConfig = YAML::LoadFile(FrameIdFile); 37 | std::vector FrameId = FrameIdConfig["mnFrameId"].as >(); 38 | std::shared_ptr LidarPtPtr(new open3d::geometry::PointCloud()); 39 | std::shared_ptr CamPtPtr(new open3d::geometry::PointCloud()); 40 | open3d::io::ReadPointCloudOption option("auto", true, true, true); 41 | open3d::io::ReadPointCloud(lidar_pcd_filename, *LidarPtPtr, option); 42 | open3d::io::ReadPointCloud(cam_pcd_filename, *CamPtPtr, option); 43 | if(FrameId[0] != 0) 44 | { 45 | std::cout << "Camera Reference Frame: " << FrameId[0] << "(!=0), need transform to the reference" << std::endl; 46 | const std::string lidar_posefile = io_config["lidar_pose"].as(); 47 | std::vector lidar_poses; 48 | ReadPoseList(lidar_posefile, lidar_poses); 49 | Eigen::Isometry3d refpose = lidar_poses[FrameId[0]]; 50 | LidarPtPtr->Transform(refpose.inverse().matrix()); 51 | } 52 | Eigen::Matrix4d init_sim3; 53 | double init_scale; 54 | std::tie(init_sim3, init_scale) = readSim3(init_sim3_filename); 55 | init_sim3.topLeftCorner(3,3) = init_sim3.topLeftCorner(3,3).transpose().eval(); // R^T 56 | init_sim3.topRightCorner(3,1) = -init_sim3.topLeftCorner(3,3) * init_sim3.topRightCorner(3,1); // -R^T * t 57 | std::cout << "Raw RCL:\n" << init_sim3.topLeftCorner(3,3) << std::endl; 58 | std::cout << "Raw tCL:\n" << init_sim3.topRightCorner(3,1).transpose().eval() << std::endl; 59 | std::cout << "Raw scale:" << init_scale << std::endl; 60 | init_sim3.topLeftCorner(3,3) *= init_scale; 61 | open3d::pipelines::registration::ICPConvergenceCriteria criteria; 62 | criteria.max_iteration_ = icp_max_iter; 63 | auto reg = open3d::pipelines::registration::RegistrationICP( 64 | *CamPtPtr, *LidarPtPtr, icp_corr_dist, init_sim3, 65 | open3d::pipelines::registration::TransformationEstimationPointToPoint(true),criteria 66 | ); 67 | Eigen::Matrix3d diag = reg.transformation_.topLeftCorner(3,3) * reg.transformation_.topLeftCorner(3,3).transpose(); 68 | double scale = sqrt(diag(0,0)); 69 | Eigen::Matrix4d se3(reg.transformation_); 70 | se3.topLeftCorner(3,3) /= scale; 71 | se3 = se3.inverse().eval(); 72 | std::cout << "Tuned RCL:\n" << se3.topLeftCorner(3,3) << std::endl; 73 | std::cout << "Tuned tCL:\n" << se3.topRightCorner(3,1).transpose() << std::endl; 74 | std::cout << "Tuned scale:\n" << scale << std::endl; 75 | CamPtPtr->Transform(reg.transformation_); 76 | CamPtPtr->PaintUniformColor((Eigen::Vector3d() << cam_color[0], cam_color[1], cam_color[2]).finished()); 77 | LidarPtPtr->PaintUniformColor((Eigen::Vector3d() << lidar_color[0], lidar_color[1], lidar_color[2]).finished()); 78 | if(use_vis){ 79 | open3d::visualization::Visualizer visualizer; 80 | visualizer.CreateVisualizerWindow(vis_name, width, height); 81 | visualizer.AddGeometry(CamPtPtr); 82 | visualizer.AddGeometry(LidarPtPtr); 83 | visualizer.Run(); 84 | } 85 | writeSim3(savePath, se3, scale); 86 | } -------------------------------------------------------------------------------- /src/examples/orb_kitti.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | 32 | #include "orb_slam/include/System.h" 33 | 34 | using namespace std; 35 | 36 | 37 | void LoadImages(const string &strSequence, vector &vstrImageFilenames, 38 | vector &vTimestamps); 39 | 40 | int main(int argc, char **argv) 41 | { 42 | if(argc != 4) 43 | { 44 | std::cout << "\033[31;1m Got " << argc-1 << " Parameters, expect 3.\033[0m" << std::endl; 45 | throw std::invalid_argument("Args: path_to_vocabulary path_to_settings path_to_sequence"); 46 | exit(0); 47 | } 48 | 49 | // Retrieve paths to images 50 | vector vstrImageFilenames; 51 | vector vTimestamps; 52 | string sequenceDir = argv[3]; 53 | checkpath(sequenceDir); 54 | LoadImages(sequenceDir, vstrImageFilenames, vTimestamps); 55 | std::cout << "Find " << vstrImageFilenames.size() << " images." << endl; 56 | std::size_t nImages = vstrImageFilenames.size(); 57 | 58 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 59 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,false); 60 | 61 | // Vector for tracking time statistics 62 | vector vTimesTrack; 63 | vTimesTrack.resize(nImages); 64 | 65 | std::cout << endl << "-------" << endl; 66 | std::cout << "Start processing sequence ..." << endl; 67 | std::cout << "Images in the sequence: " << nImages << endl << endl; 68 | 69 | // Main loop 70 | cv::Mat im; 71 | // bool bLocalMapBusy = false, bLoopClosingBusy = false; 72 | // >>>>>>>>>>>>>>>> Replace 10 with nImages <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 73 | for(std::size_t ni=0; ni < nImages; ni++) 74 | { 75 | // Read image from file 76 | im = cv::imread(vstrImageFilenames[ni],cv::IMREAD_UNCHANGED); 77 | double tframe = vTimestamps[ni]; 78 | 79 | if(im.empty()) 80 | { 81 | cerr << endl << "Failed to load image at: " << vstrImageFilenames[ni] << endl; 82 | return 1; 83 | } 84 | 85 | 86 | std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); 87 | 88 | // Wait until the underlying threads finish tasks. 89 | // std::tie(bLocalMapBusy, bLoopClosingBusy) = SLAM.isRequiredWait(); 90 | // if(bLocalMapBusy || bLoopClosingBusy){ 91 | // if(bLocalMapBusy){ 92 | // if(!bLoopClosingBusy) 93 | // std::cout << "Wait for Local Map..." << std::endl; 94 | // else 95 | // std::cout << "Wait Local Mapping and Loop Closing..." << std::endl; 96 | // }else{ 97 | // std::cout << "Wait for Loop Closing..." << std::endl; 98 | // } 99 | // while(1){ 100 | // std::tie(bLocalMapBusy, bLoopClosingBusy) = SLAM.isRequiredWait(); 101 | // if(!(bLocalMapBusy || bLoopClosingBusy)){ 102 | // break; 103 | // } 104 | // usleep(5000); 105 | // } 106 | // } 107 | // Pass the image to the SLAM system 108 | SLAM.TrackMonocular(im,tframe); 109 | char msg[100]; 110 | std::sprintf(msg, "Image id : %ld, Frame id: %ld, STATE: %d", ni, SLAM.GetCurrentFid(),SLAM.GetTrackingState()); 111 | std::cout << msg << std::endl; 112 | std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); 113 | 114 | 115 | double ttrack= std::chrono::duration_cast >(t2 - t1).count(); 116 | 117 | vTimesTrack[ni]=ttrack; 118 | 119 | // Wait to load the next frame 120 | double T=0; 121 | if(ni0) 124 | T = tframe-vTimestamps[ni-1]; 125 | 126 | 127 | if(ttrack &vstrImageFilenames, vector &vTimestamps) 154 | { 155 | ifstream fTimes; 156 | string strPathTimeFile = strPathToSequence + "times.txt"; 157 | if(!file_exist(strPathTimeFile)){ 158 | throw std::runtime_error("Timestamp file: "+strPathTimeFile+" does not exsit."); 159 | return; 160 | }; // time.txt must exist 161 | fTimes.open(strPathTimeFile.c_str()); 162 | while(!fTimes.eof()) 163 | { 164 | string s; 165 | getline(fTimes,s); 166 | if(!s.empty()) 167 | { 168 | stringstream ss; 169 | ss << s; 170 | double t; 171 | ss >> t; 172 | vTimestamps.push_back(t); 173 | } 174 | } 175 | 176 | string strPrefixLeft = strPathToSequence + "image_0/"; 177 | 178 | if(!file_exist(strPrefixLeft)){ 179 | throw std::runtime_error("Image Directory: "+strPrefixLeft+" does not exsit."); 180 | return; 181 | }; // time.txt must exist 182 | listdir(vstrImageFilenames, strPrefixLeft); 183 | for(auto &imageFilename: vstrImageFilenames) 184 | imageFilename = strPrefixLeft + imageFilename; 185 | if(vTimestamps.size()!=vstrImageFilenames.size()){ 186 | char msg[100]; 187 | std::sprintf(msg, "size of vTimestamps (%ld) != size of vstrImageFilenames (%ld)", vTimestamps.size(), vstrImageFilenames.size()); 188 | throw std::runtime_error(msg); 189 | return; 190 | }; 191 | } 192 | -------------------------------------------------------------------------------- /src/examples/orb_kitti_store.cpp: -------------------------------------------------------------------------------- 1 | #include "io_tools.h" 2 | #include "orb_slam/include/System.h" 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "argparse.hpp" 11 | 12 | void check_exist(std::string &file){ 13 | if(!file_exist(file)){ 14 | throw std::invalid_argument(file + " does not exist."); 15 | return; 16 | } 17 | } 18 | 19 | void writeData(std::string &outfile, std::vector Tlist){ 20 | std::ofstream of(outfile, std::ios::ate); 21 | of.precision(std::numeric_limits< double >::max_digits10); 22 | for(std::size_t i=0; i &Twc_list, 35 | std::vector &vKFFrameId, std::vector &vstrImageFilename, std::vector &vtimestamp, 36 | const std::string &KeyFrameDir, const std::string &MapFile) 37 | { 38 | for(std::size_t i=0; iTrackMonocular(img, vtimestamp[i]); 45 | std::printf("\033[033;1m[VO]\033[0m Frame %ld | %ld STATE: %d\n", i+1, vstrImageFilename.size(), SLAM->GetTrackingState()); 46 | std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); 47 | double ttrack= std::chrono::duration_cast >(t2 - t1).count(); 48 | double T=0; 49 | if(i0) 52 | T = tframe-vtimestamp[i-1]; 53 | 54 | if(ttrackShutdown(); 63 | std::vector vKFPose; 64 | SLAM->GetKeyFramePoses(vKFPose, vKFFrameId); 65 | for(std::size_t i=0; iSaveKeyFrames(KeyFrameDir); 77 | SLAM->SaveMap(MapFile); 78 | 79 | } 80 | 81 | int main(int argc, char** argv){ 82 | argparse::ArgumentParser parser("ORB-SLAM with Keyframes storation process"); 83 | parser.add_argument("seq_dir").help("parent directory of the directory of image files.").required(); 84 | parser.add_argument("orb_yaml").help("directory of ORB yaml files").required(); 85 | parser.add_argument("orb_voc").help("Vocabulary file of DBoW2/DBow3").required(); 86 | parser.add_argument("output_pose").help("--directory to save poses of Keyframes.").required(); 87 | parser.add_argument("keyframe_dir").help("--directory to save information of KeyFrames").required(); 88 | parser.add_argument("map_file").help("--directory to save visual map").required(); 89 | parser.add_argument("--slow_rate").help("slow rate of runtime").scan<'g',double>().default_value(1.5); 90 | parser.add_argument("--img_dir").help("relative directory of images").default_value("image_0/"); 91 | parser.add_argument("--timefile").help("timestamp file").default_value("times.txt"); 92 | parser.parse_args(argc, argv); 93 | 94 | std::string seq_dir(parser.get("seq_dir")); 95 | std::string orb_yml(parser.get("orb_yaml")); 96 | std::string orb_voc(parser.get("orb_voc")); 97 | std::string TwcFile(parser.get("output_pose")); // output 98 | std::string keyframe_dir(parser.get("keyframe_dir")); // output 99 | std::string mapfile(parser.get("map_file")); 100 | double slow_rate = parser.get("--slow_rate"); 101 | checkpath(seq_dir); 102 | checkpath(keyframe_dir); 103 | check_exist(seq_dir); 104 | check_exist(orb_yml); 105 | check_exist(orb_voc); 106 | std::string img_dir = seq_dir + parser.get("--img_dir"); 107 | checkpath(img_dir); 108 | check_exist(img_dir); 109 | std::string timefile = seq_dir + parser.get("--timefile"); 110 | std::vector vImageFiles; 111 | std::vector vTimeStamps; 112 | LoadTimestamp(timefile, vTimeStamps); 113 | LoadFiles(img_dir, vImageFiles); 114 | if(!(vTimeStamps.size()==vImageFiles.size())){ 115 | char msg[100]; 116 | sprintf(msg, "Filesize error Timestamps (%ld) != ImageFiles (%ld)", vTimeStamps.size(), vImageFiles.size()); 117 | throw std::runtime_error(msg); 118 | return -1; 119 | } 120 | std::cout << "Found " << vTimeStamps.size() << " Frames of Timestamps, Laser Scans and Images." << std::endl; 121 | ORB_SLAM2::System* orbSLAM(new ORB_SLAM2::System(orb_voc, orb_yml, ORB_SLAM2::System::MONOCULAR, false)); // No Interface for VO 122 | std::vector vTwc; 123 | std::vector vKFFrameId; 124 | vTwc.reserve(vImageFiles.size()); 125 | visual_odometry(orbSLAM, slow_rate, std::ref(vTwc), std::ref(vKFFrameId), std::ref(vImageFiles), std::ref(vTimeStamps), std::ref(keyframe_dir), mapfile); 126 | std::cout << "Total visual Frames: " << vTwc.size() << ", Key Frames: " << vKFFrameId.size() << std::endl; 127 | writeData(TwcFile, vTwc); 128 | 129 | 130 | } -------------------------------------------------------------------------------- /src/examples/orb_restore.cpp: -------------------------------------------------------------------------------- 1 | #include "orb_slam/include/System.h" 2 | #include 3 | int main(int argc, char **argv){ 4 | if(argc != 6) 5 | { 6 | std::cout << "\033[31;1m Got " << argc-1 << " Parameters, expect 5.\033[0m" << std::endl; 7 | throw std::invalid_argument("Args: path_to_vocabulary path_to_settings path_to_keyframe_dir path_to_map_filename debug_log"); 8 | exit(0); 9 | } 10 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,false); 11 | std::string KeyFrameDir(argv[3]); 12 | const std::string mapFile(argv[4]), debugFile(argv[5]); 13 | SLAM.Shutdown(); 14 | SLAM.RestoreSystemFromFile(KeyFrameDir, mapFile); 15 | std::vector KeyFrames = SLAM.GetAllKeyFrames(true); 16 | std::sort(KeyFrames.begin(), KeyFrames.end(), ORB_SLAM2::KeyFrame::lId); 17 | auto KeyptMatched = KeyFrames[0]->GetMatchedKptIds(KeyFrames[1]); 18 | 19 | std::cout << "Matched pairs betwenn " << KeyFrames[0]->mnId << " & " << KeyFrames[1]->mnId << " : " << KeyptMatched.size() << std::endl; 20 | std::ofstream ofs(debugFile); 21 | for(auto it=KeyptMatched.begin(); it!=KeyptMatched.end(); ++it) 22 | { 23 | ofs << it->first << " " << it->second << std::endl; 24 | } 25 | ofs.close(); 26 | 27 | } -------------------------------------------------------------------------------- /src/examples/orb_save_map.cpp: -------------------------------------------------------------------------------- 1 | #include "orb_slam/include/System.h" 2 | #include "orb_slam/include/Converter.h" 3 | #include "open3d/geometry/PointCloud.h" 4 | #include "open3d/io/PointCloudIO.h" 5 | #include 6 | 7 | int main(int argc, char **argv){ 8 | if(argc < 2) 9 | { 10 | std::cout << "\033[31;1m Got " << argc-1 << " Parameters, expect config_file.\033[0m" << std::endl; 11 | exit(0); 12 | } 13 | YAML::Node config = YAML::LoadFile(argv[1]); 14 | YAML::Node orb_config = config["orb_setting"]; 15 | YAML::Node io_config = config["io"]; 16 | std::string vocabulary_path = orb_config["vocabulary"].as(); 17 | std::string setting_path = orb_config["setting"].as(); 18 | ORB_SLAM2::System SLAM(vocabulary_path, setting_path,ORB_SLAM2::System::MONOCULAR,false); 19 | std::string KeyFrameDir = io_config["KeyFrameDir"].as(); 20 | std::string MapFile = io_config["MapFile"].as(); 21 | std::string SavePath = io_config["SavePath"].as(); 22 | SLAM.Shutdown(); 23 | SLAM.RestoreSystemFromFile(KeyFrameDir, MapFile); 24 | std::vector MapPoints = SLAM.GetAllMapPoints(true); 25 | std::vector MapPointsData; 26 | for(auto MapPoint:MapPoints){ 27 | cv::Mat mat = MapPoint->GetWorldPos(); 28 | Eigen::Vector3d pointvec = ORB_SLAM2::Converter::toVector3d(mat); 29 | MapPointsData.push_back(pointvec); 30 | } 31 | open3d::geometry::PointCloud Map(MapPointsData); 32 | open3d::io::WritePointCloudOption option(false, false, true); 33 | open3d::io::WritePointCloud(SavePath, Map, option); 34 | 35 | } -------------------------------------------------------------------------------- /src/floam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(floam) 3 | 4 | if(NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif() 7 | MESSAGE(${PROJECT_NAME} " Build type: " ${CMAKE_BUILD_TYPE}) 8 | set(CMAKE_CXX_FLAGS "-std=c++17") 9 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 10 | 11 | find_package(Eigen3) 12 | if(NOT EIGEN3_FOUND) 13 | # Fallback to cmake_modules 14 | find_package(cmake_modules REQUIRED) 15 | find_package(Eigen REQUIRED) 16 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 17 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only 18 | # Possibly map additional variables to the EIGEN3_ prefix. 19 | else() 20 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 21 | endif() 22 | find_package(PCL REQUIRED) 23 | find_package(Ceres REQUIRED) 24 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 25 | 26 | aux_source_directory(${PROJECT_SOURCE_DIR}/src DIR_LIB_SRCS) 27 | add_library(floam SHARED ${DIR_LIB_SRCS}) 28 | target_link_libraries(floam ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 29 | 30 | target_include_directories(floam PRIVATE 31 | include 32 | ${PCL_INCLUDE_DIRS} 33 | ${CERES_INCLUDE_DIRS} 34 | ${EIGEN3_INCLUDE_DIRS} 35 | ) 36 | -------------------------------------------------------------------------------- /src/floam/include/floamClass.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "lidar.h" 4 | #include "laserMappingClass.h" 5 | #include "laserProcessingClass.h" 6 | #include "lidarOptimization.h" 7 | #include "odomEstimationClass.h" 8 | 9 | 10 | namespace FLOAM{ 11 | typedef pcl::PointXYZI PointType; 12 | enum eLiDARType{ 13 | VLP_16=0, 14 | HDL_32=1, 15 | HDL_64=2 16 | }; 17 | 18 | 19 | class System{ 20 | public: 21 | System(int num_lines=64, double scan_period=0.1,double max_dis=90,double min_dis=3.0,double map_resolution=0.4); 22 | System(eLiDARType type, double scan_period=0.1, double max_dis=90); 23 | Eigen::Isometry3d Track(const pcl::PointCloud::Ptr pointcloud_in); 24 | lidar::Lidar *lidar_params; 25 | bool odom_init; 26 | private: 27 | LaserProcessingClass *laserprocess; 28 | OdomEstimationClass *odomEstimation; 29 | LaserMappingClass *laserMapping; 30 | }; 31 | 32 | } -------------------------------------------------------------------------------- /src/floam/include/laserMappingClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _LASER_MAPPING_H_ 6 | #define _LASER_MAPPING_H_ 7 | 8 | //PCL lib 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | //eigen lib 17 | #include 18 | #include 19 | 20 | //c++ lib 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | #define LASER_CELL_WIDTH 50.0 27 | #define LASER_CELL_HEIGHT 50.0 28 | #define LASER_CELL_DEPTH 50.0 29 | 30 | //separate map as many sub point clouds 31 | 32 | #define LASER_CELL_RANGE_HORIZONTAL 2 33 | #define LASER_CELL_RANGE_VERTICAL 2 34 | 35 | 36 | class LaserMappingClass 37 | { 38 | 39 | public: 40 | LaserMappingClass(); 41 | void init(double map_resolution); 42 | void updateCurrentPointsToMap(const pcl::PointCloud::Ptr& pc_in, const Eigen::Isometry3d& pose_current); 43 | pcl::PointCloud::Ptr getMap(void); 44 | 45 | private: 46 | int origin_in_map_x; 47 | int origin_in_map_y; 48 | int origin_in_map_z; 49 | int map_width; 50 | int map_height; 51 | int map_depth; 52 | std::vector::Ptr>>> map; 53 | pcl::VoxelGrid downSizeFilter; 54 | 55 | void addWidthCellNegative(void); 56 | void addWidthCellPositive(void); 57 | void addHeightCellNegative(void); 58 | void addHeightCellPositive(void); 59 | void addDepthCellNegative(void); 60 | void addDepthCellPositive(void); 61 | void checkPoints(int& x, int& y, int& z); 62 | 63 | }; 64 | 65 | 66 | #endif // _LASER_MAPPING_H_ 67 | 68 | -------------------------------------------------------------------------------- /src/floam/include/laserProcessingClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LASER_PROCESSING_CLASS_H_ 5 | #define _LASER_PROCESSING_CLASS_H_ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "lidar.h" 17 | 18 | //points covariance class 19 | class Double2d{ 20 | public: 21 | int id; 22 | double value; 23 | Double2d(int id_in, double value_in); 24 | }; 25 | //points info class 26 | class PointsInfo{ 27 | public: 28 | int layer; 29 | double time; 30 | PointsInfo(int layer_in, double time_in); 31 | }; 32 | 33 | 34 | class LaserProcessingClass 35 | { 36 | public: 37 | LaserProcessingClass(); 38 | void init(lidar::Lidar lidar_param_in); 39 | void featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf); 40 | void featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf); 41 | private: 42 | lidar::Lidar lidar_param; 43 | }; 44 | 45 | 46 | 47 | #endif // _LASER_PROCESSING_CLASS_H_ 48 | 49 | -------------------------------------------------------------------------------- /src/floam/include/lidar.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_H_ 5 | #define _LIDAR_H_ 6 | 7 | //define lidar parameter 8 | 9 | namespace lidar{ 10 | 11 | class Lidar 12 | { 13 | public: 14 | Lidar(); 15 | 16 | void setScanPeriod(double scan_period_in); 17 | void setLines(short num_lines_in); 18 | //by default is 100. pls do not change 19 | void setMaxDistance(double max_distance_in); 20 | void setMinDistance(double min_distance_in); 21 | 22 | double max_distance; 23 | double min_distance; 24 | int num_lines; 25 | double scan_period; 26 | }; 27 | 28 | 29 | } 30 | 31 | 32 | #endif // _LIDAR_H_ 33 | 34 | -------------------------------------------------------------------------------- /src/floam/include/lidarOptimization.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_ 5 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t); 13 | 14 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in); 15 | 16 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 17 | public: 18 | 19 | EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_); 20 | virtual ~EdgeAnalyticCostFunction() {} 21 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 22 | 23 | Eigen::Vector3d curr_point; 24 | Eigen::Vector3d last_point_a; 25 | Eigen::Vector3d last_point_b; 26 | }; 27 | 28 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 29 | public: 30 | SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_); 31 | virtual ~SurfNormAnalyticCostFunction() {} 32 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 33 | 34 | Eigen::Vector3d curr_point; 35 | Eigen::Vector3d plane_unit_norm; 36 | double negative_OA_dot_norm; 37 | }; 38 | 39 | class PoseSE3Parameterization : public ceres::LocalParameterization { 40 | public: 41 | 42 | PoseSE3Parameterization() {} 43 | virtual ~PoseSE3Parameterization() {} 44 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 45 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 46 | virtual int GlobalSize() const { return 7; } 47 | virtual int LocalSize() const { return 6; } 48 | }; 49 | 50 | 51 | 52 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_ 53 | 54 | -------------------------------------------------------------------------------- /src/floam/include/odomEstimationClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _ODOM_ESTIMATION_CLASS_H_ 5 | #define _ODOM_ESTIMATION_CLASS_H_ 6 | 7 | //std lib 8 | #include 9 | #include 10 | #include 11 | 12 | //PCL 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | //ceres 24 | #include 25 | #include 26 | 27 | //eigen 28 | #include 29 | #include 30 | 31 | //LOCAL LIB 32 | #include "lidar.h" 33 | #include "lidarOptimization.h" 34 | // #include 35 | 36 | class OdomEstimationClass 37 | { 38 | 39 | public: 40 | OdomEstimationClass(); 41 | 42 | void init(lidar::Lidar lidar_param, double map_resolution); 43 | void initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 44 | void updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 45 | void getMap(pcl::PointCloud::Ptr& laserCloudMap); 46 | 47 | Eigen::Isometry3d odom; 48 | pcl::PointCloud::Ptr laserCloudCornerMap; 49 | pcl::PointCloud::Ptr laserCloudSurfMap; 50 | private: 51 | //optimization variable 52 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; 53 | Eigen::Map q_w_curr = Eigen::Map(parameters); 54 | Eigen::Map t_w_curr = Eigen::Map(parameters + 4); 55 | 56 | Eigen::Isometry3d last_odom; 57 | 58 | //kd-tree 59 | pcl::KdTreeFLANN::Ptr kdtreeEdgeMap; 60 | pcl::KdTreeFLANN::Ptr kdtreeSurfMap; 61 | 62 | //points downsampling before add to map 63 | pcl::VoxelGrid downSizeFilterEdge; 64 | pcl::VoxelGrid downSizeFilterSurf; 65 | 66 | //local map 67 | pcl::CropBox cropBoxFilter; 68 | 69 | //optimization count 70 | int optimization_count; 71 | 72 | //function 73 | void addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 74 | void addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 75 | void addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud); 76 | void pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po); 77 | void downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out); 78 | }; 79 | 80 | #endif // _ODOM_ESTIMATION_CLASS_H_ 81 | 82 | -------------------------------------------------------------------------------- /src/floam/src/floamClass.cpp: -------------------------------------------------------------------------------- 1 | #include "floamClass.h" 2 | 3 | namespace FLOAM{ 4 | System::System(int num_lines, double scan_period, double max_dis, double min_dis, double map_resolution): 5 | odom_init(false), lidar_params(new lidar::Lidar), 6 | laserprocess(new LaserProcessingClass), odomEstimation(new OdomEstimationClass),laserMapping(new LaserMappingClass){ 7 | lidar_params->setLines(num_lines); 8 | lidar_params->setScanPeriod(scan_period); 9 | lidar_params->setMaxDistance(max_dis); 10 | lidar_params->setMinDistance(min_dis); 11 | laserprocess->init(*lidar_params); 12 | odomEstimation->init(*lidar_params, map_resolution); 13 | } 14 | System::System(eLiDARType type, double scan_period, double max_dis): 15 | odom_init(false), lidar_params(new lidar::Lidar), 16 | laserprocess(new LaserProcessingClass), odomEstimation(new OdomEstimationClass),laserMapping(new LaserMappingClass){ 17 | short num_lines; 18 | double min_dis; 19 | double map_resolution; 20 | if (type==VLP_16){ 21 | num_lines = 16; 22 | min_dis = 0.3; 23 | map_resolution = 0.2; 24 | } 25 | else if (type==HDL_32) 26 | { 27 | num_lines = 32; 28 | min_dis = 0.5; 29 | map_resolution = 0.2; 30 | } 31 | else if (type==HDL_64) 32 | { 33 | num_lines = 64; 34 | min_dis = 3.0; 35 | map_resolution = 0.4; 36 | }else{ 37 | throw std::invalid_argument("Invalid LiDARType"); 38 | return; 39 | } 40 | lidar_params->setLines(num_lines); 41 | lidar_params->setScanPeriod(scan_period); 42 | lidar_params->setMaxDistance(max_dis); 43 | lidar_params->setMinDistance(min_dis); 44 | laserprocess->init(*lidar_params); 45 | odomEstimation->init(*lidar_params, map_resolution); 46 | } 47 | Eigen::Isometry3d System::Track(const pcl::PointCloud::Ptr pointcloud_in){ 48 | pcl::PointCloud::Ptr pointcloud_edge(new pcl::PointCloud); 49 | pcl::PointCloud::Ptr pointcloud_surf(new pcl::PointCloud); 50 | laserprocess->featureExtraction(pointcloud_in, pointcloud_edge, pointcloud_surf); 51 | if(!odom_init){ 52 | odomEstimation->initMapWithPoints(pointcloud_edge, pointcloud_surf); 53 | odom_init = true; 54 | return Eigen::Isometry3d::Identity(); 55 | } 56 | else{ 57 | odomEstimation->updatePointsToMap(pointcloud_edge, pointcloud_surf); 58 | return odomEstimation->odom; 59 | } 60 | } 61 | 62 | } -------------------------------------------------------------------------------- /src/floam/src/lidar.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "lidar.h" 6 | 7 | 8 | lidar::Lidar::Lidar(){ 9 | 10 | } 11 | 12 | 13 | void lidar::Lidar::setLines(short num_lines_in){ 14 | num_lines=num_lines_in; 15 | } 16 | 17 | void lidar::Lidar::setScanPeriod(double scan_period_in){ 18 | scan_period = scan_period_in; 19 | } 20 | 21 | 22 | void lidar::Lidar::setMaxDistance(double max_distance_in){ 23 | max_distance = max_distance_in; 24 | } 25 | 26 | void lidar::Lidar::setMinDistance(double min_distance_in){ 27 | min_distance = min_distance_in; 28 | } -------------------------------------------------------------------------------- /src/floam/src/lidarOptimization.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "lidarOptimization.h" 6 | 7 | EdgeAnalyticCostFunction::EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_) 8 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_){ 9 | 10 | } 11 | 12 | bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 13 | { 14 | 15 | Eigen::Map q_last_curr(parameters[0]); 16 | Eigen::Map t_last_curr(parameters[0] + 4); 17 | Eigen::Vector3d lp; 18 | lp = q_last_curr * curr_point + t_last_curr; 19 | 20 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b); 21 | Eigen::Vector3d de = last_point_a - last_point_b; 22 | double de_norm = de.norm(); 23 | residuals[0] = nu.norm()/de_norm; 24 | 25 | if(jacobians != NULL) 26 | { 27 | if(jacobians[0] != NULL) 28 | { 29 | Eigen::Matrix3d skew_lp = skew(lp); 30 | Eigen::Matrix dp_by_se3; 31 | dp_by_se3.block<3,3>(0,0) = -skew_lp; 32 | (dp_by_se3.block<3,3>(0, 3)).setIdentity(); 33 | Eigen::Map > J_se3(jacobians[0]); 34 | J_se3.setZero(); 35 | Eigen::Matrix3d skew_de = skew(de); 36 | J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm; 37 | 38 | } 39 | } 40 | 41 | return true; 42 | 43 | } 44 | 45 | 46 | SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) 47 | : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_){ 48 | 49 | } 50 | 51 | bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 52 | { 53 | Eigen::Map q_w_curr(parameters[0]); 54 | Eigen::Map t_w_curr(parameters[0] + 4); 55 | Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr; 56 | residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm; 57 | 58 | if(jacobians != NULL) 59 | { 60 | if(jacobians[0] != NULL) 61 | { 62 | Eigen::Matrix3d skew_point_w = skew(point_w); 63 | Eigen::Matrix dp_by_se3; 64 | dp_by_se3.block<3,3>(0,0) = -skew_point_w; 65 | (dp_by_se3.block<3,3>(0, 3)).setIdentity(); 66 | Eigen::Map > J_se3(jacobians[0]); // jacobian size: [1,7] 67 | J_se3.setZero(); 68 | J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_se3; 69 | 70 | } 71 | } 72 | return true; 73 | 74 | } 75 | 76 | 77 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 78 | { 79 | Eigen::Map trans(x + 4); 80 | 81 | Eigen::Quaterniond delta_q; 82 | Eigen::Vector3d delta_t; 83 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 84 | Eigen::Map quater(x); 85 | Eigen::Map quater_plus(x_plus_delta); 86 | Eigen::Map trans_plus(x_plus_delta + 4); 87 | 88 | quater_plus = delta_q * quater; 89 | trans_plus = delta_q * trans + delta_t; 90 | 91 | return true; 92 | } 93 | 94 | bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const 95 | { 96 | Eigen::Map> j(jacobian); 97 | (j.topRows(6)).setIdentity(); 98 | (j.bottomRows(1)).setZero(); 99 | 100 | return true; 101 | } 102 | 103 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){ 104 | Eigen::Vector3d omega(se3.data()); 105 | Eigen::Vector3d upsilon(se3.data()+3); 106 | Eigen::Matrix3d Omega = skew(omega); 107 | 108 | double theta = omega.norm(); 109 | double half_theta = 0.5*theta; 110 | 111 | double imag_factor; 112 | double real_factor = cos(half_theta); 113 | if(theta<1e-10) 114 | { 115 | double theta_sq = theta*theta; 116 | double theta_po4 = theta_sq*theta_sq; 117 | imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4; // Tayor Approximation 118 | } 119 | else 120 | { 121 | double sin_half_theta = sin(half_theta); 122 | imag_factor = sin_half_theta/theta; 123 | } 124 | 125 | q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z()); 126 | 127 | 128 | Eigen::Matrix3d J; 129 | if (theta<1e-10) 130 | { 131 | J = q.matrix(); 132 | } 133 | else 134 | { 135 | Eigen::Matrix3d Omega2 = Omega*Omega; 136 | J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2); 137 | } 138 | 139 | t = J*upsilon; 140 | } 141 | 142 | Eigen::Matrix skew(Eigen::Matrix& mat_in){ 143 | Eigen::Matrix skew_mat; 144 | skew_mat.setZero(); 145 | skew_mat(0,1) = -mat_in(2); 146 | skew_mat(0,2) = mat_in(1); 147 | skew_mat(1,2) = -mat_in(0); 148 | skew_mat(1,0) = mat_in(2); 149 | skew_mat(2,0) = -mat_in(1); 150 | skew_mat(2,1) = mat_in(0); 151 | return skew_mat; 152 | } -------------------------------------------------------------------------------- /src/orb_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | project(ORB_SLAM2) 3 | set(OpenGL_GL_PREFERENCE "GLVND") # Set for Pangolin 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE(${PROJECT_NAME} " Build type: " ${CMAKE_BUILD_TYPE}) 9 | set(CMAKE_CXX_FLAGS "-std=c++17 -O3 -fopenmp ") # add -fopenmp for OpenMP 10 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 11 | find_package(OpenCV 4 REQUIRED) 12 | find_package(Eigen3 REQUIRED) 13 | find_package(Pangolin REQUIRED) 14 | set(g2o_DIR "/usr/local/lib/g2o") 15 | 16 | include_directories(${G2O_INCLUDE_DIRS} "/usr/include/eigen3") 17 | include_directories( 18 | ${PROJECT_SOURCE_DIR} 19 | ${PROJECT_SOURCE_DIR}/include 20 | ${EIGEN3_INCLUDE_DIR} 21 | ${Pangolin_INCLUDE_DIRS} 22 | ) 23 | 24 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/../../lib) 25 | 26 | SET (G2O_LIBS cxsparse g2o_csparse_extension g2o_cli g2o_core g2o_interface g2o_parser g2o_solver_cholmod 27 | g2o_solver_csparse g2o_solver_dense g2o_solver_pcg g2o_stuff g2o_types_icp 28 | g2o_types_sba g2o_types_sim3 g2o_types_slam2d g2o_types_slam3d) 29 | add_library(${PROJECT_NAME} SHARED 30 | src/System.cc 31 | src/Tracking.cc 32 | src/LocalMapping.cc 33 | src/LoopClosing.cc 34 | src/ORBextractor.cc 35 | src/ORBmatcher.cc 36 | src/FrameDrawer.cc 37 | src/Converter.cc 38 | src/MapPoint.cc 39 | src/KeyFrame.cc 40 | src/Map.cc 41 | src/MapDrawer.cc 42 | src/Optimizer.cc 43 | src/Frame.cc 44 | src/KeyFrameDatabase.cc 45 | src/Sim3Solver.cc 46 | src/Initializer.cc 47 | src/Viewer.cc 48 | )# src/PnPsolver.cc 49 | 50 | include_directories( 51 | /usr/include/boost 52 | ) 53 | 54 | target_link_libraries(${PROJECT_NAME} 55 | ${OpenCV_LIBS} 56 | ${EIGEN3_LIBS} 57 | ${Pangolin_LIBRARIES} 58 | ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so 59 | ${G2O_LIBS} 60 | boost_serialization 61 | ) 62 | # set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/examples) 63 | # add_executable(kitti_orb ../examples/orb_kitti.cpp) 64 | # target_link_libraries(kitti_orb 65 | # ${PROJECT_NAME} 66 | # /usr/local/lib/libglog.so # ?? 67 | # ) -------------------------------------------------------------------------------- /src/orb_slam/include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | #include "g2o/types/sba/types_six_dof_expmap.h" 28 | #include "g2o/types/sim3/types_seven_dof_expmap.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class Converter 34 | { 35 | public: 36 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 37 | 38 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 39 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 40 | 41 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 42 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 43 | static cv::Mat toCvMat(const Eigen::Matrix &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 47 | 48 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 49 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 50 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 51 | 52 | static std::vector toQuaternion(const cv::Mat &M); 53 | }; 54 | 55 | }// namespace ORB_SLAM 56 | 57 | #endif // CONVERTER_H 58 | -------------------------------------------------------------------------------- /src/orb_slam/include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEDRAWER_H 22 | #define FRAMEDRAWER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | class Viewer; 39 | 40 | class FrameDrawer 41 | { 42 | public: 43 | FrameDrawer(Map* pMap); 44 | 45 | // Update info from the last processed frame. 46 | void Update(Tracking *pTracker); 47 | 48 | // Draw last processed frame. 49 | cv::Mat DrawFrame(); 50 | 51 | protected: 52 | 53 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 54 | 55 | // Info of the frame to be drawn 56 | cv::Mat mIm; 57 | int N; 58 | std::vector mvCurrentKeys; 59 | std::vector mvbMap, mvbVO; 60 | bool mbOnlyTracking; 61 | int mnTracked, mnTrackedVO; 62 | std::vector mvIniKeys; 63 | std::vector mvIniMatches; 64 | int mState; 65 | 66 | Map* mpMap; 67 | 68 | std::mutex mMutex; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif // FRAMEDRAWER_H 74 | -------------------------------------------------------------------------------- /src/orb_slam/include/Initializer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | #ifndef INITIALIZER_H 21 | #define INITIALIZER_H 22 | 23 | #include 24 | #include "Frame.h" 25 | 26 | 27 | namespace ORB_SLAM2 28 | { 29 | 30 | // THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE. 31 | class Initializer 32 | { 33 | typedef std::pair Match; 34 | 35 | public: 36 | 37 | // Fix the reference frame 38 | Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200); 39 | 40 | // Computes in parallel a fundamental matrix and a homography 41 | // Selects a model and tries to recover the motion and the structure from motion 42 | bool Initialize(const Frame &CurrentFrame, const std::vector &vMatches12, 43 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated); 44 | 45 | 46 | private: 47 | 48 | void FindHomography(std::vector &vbMatchesInliers, float &score, cv::Mat &H21); 49 | void FindFundamental(std::vector &vbInliers, float &score, cv::Mat &F21); 50 | 51 | cv::Mat ComputeH21(const std::vector &vP1, const std::vector &vP2); 52 | cv::Mat ComputeF21(const std::vector &vP1, const std::vector &vP2); 53 | 54 | float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, std::vector &vbMatchesInliers, float sigma); 55 | 56 | float CheckFundamental(const cv::Mat &F21, std::vector &vbMatchesInliers, float sigma); 57 | 58 | bool ReconstructF(std::vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, 59 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated, float minParallax, int minTriangulated); 60 | 61 | bool ReconstructH(std::vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, 62 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated, float minParallax, int minTriangulated); 63 | 64 | void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); 65 | 66 | void Normalize(const std::vector &vKeys, std::vector &vNormalizedPoints, cv::Mat &T); 67 | 68 | int CheckRT(const cv::Mat &R, const cv::Mat &t, const std::vector &vKeys1, const std::vector &vKeys2, 69 | const std::vector &vMatches12, std::vector &vbInliers, 70 | const cv::Mat &K, std::vector &vP3D, float th2, std::vector &vbGood, float ¶llax); 71 | 72 | void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t); 73 | 74 | 75 | // Keypoints from Reference Frame (Frame 1) 76 | std::vector mvKeys1; 77 | 78 | // Keypoints from Current Frame (Frame 2) 79 | std::vector mvKeys2; 80 | 81 | // Current Matches from Reference to Current 82 | std::vector mvMatches12; 83 | std::vector mvbMatched1; 84 | 85 | // Calibration 86 | cv::Mat mK; 87 | 88 | // Standard Deviation and Variance 89 | float mSigma, mSigma2; 90 | 91 | // Ransac max iterations 92 | int mMaxIterations; 93 | 94 | // Ransac sets 95 | std::vector > mvSets; 96 | 97 | }; 98 | 99 | } //namespace ORB_SLAM 100 | 101 | #endif // INITIALIZER_H 102 | -------------------------------------------------------------------------------- /src/orb_slam/include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | 42 | class KeyFrameDatabase 43 | { 44 | public: 45 | 46 | KeyFrameDatabase(const ORBVocabulary &voc); 47 | 48 | void add(KeyFrame* pKF); 49 | 50 | void erase(KeyFrame* pKF); 51 | 52 | void clear(); 53 | 54 | // Loop Detection 55 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 56 | 57 | // Relocalization 58 | std::vector DetectRelocalizationCandidates(Frame* F); 59 | 60 | protected: 61 | 62 | // Associated vocabulary 63 | const ORBVocabulary* mpVoc; 64 | 65 | // Inverted file 66 | std::vector > mvInvertedFile; 67 | 68 | // Mutex 69 | std::mutex mMutex; 70 | }; 71 | 72 | } //namespace ORB_SLAM 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/orb_slam/include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOCALMAPPING_H 22 | #define LOCALMAPPING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "Map.h" 26 | #include "LoopClosing.h" 27 | #include "Tracking.h" 28 | #include "KeyFrameDatabase.h" 29 | 30 | #include 31 | 32 | 33 | namespace ORB_SLAM2 34 | { 35 | 36 | class Tracking; 37 | class LoopClosing; 38 | class Map; 39 | 40 | class LocalMapping 41 | { 42 | public: 43 | LocalMapping(Map* pMap, const float bMonocular); 44 | 45 | void SetLoopCloser(LoopClosing* pLoopCloser); 46 | 47 | void SetTracker(Tracking* pTracker); 48 | 49 | // Main function 50 | void Run(); 51 | 52 | void InsertKeyFrame(KeyFrame* pKF); 53 | 54 | // Thread Synch 55 | void RequestStop(); 56 | void RequestReset(); 57 | bool Stop(); 58 | void Release(); 59 | bool isStopped(); 60 | bool stopRequested(); 61 | bool AcceptKeyFrames(); 62 | void SetAcceptKeyFrames(bool flag); 63 | bool SetNotStop(bool flag); 64 | 65 | void InterruptBA(); 66 | 67 | void RequestFinish(); 68 | bool isFinished(); 69 | 70 | int KeyframesInQueue(){ 71 | std::unique_lock lock(mMutexNewKFs); 72 | return mlNewKeyFrames.size(); 73 | } 74 | bool isRequiredWait(){ 75 | return !AcceptKeyFrames(); 76 | } 77 | 78 | void CalibEpipolarOptimization(const Eigen::Matrix3d &InitialRotation, const Eigen::Vector3d &InitialTranslation, 79 | std::vector); 80 | 81 | 82 | protected: 83 | 84 | bool CheckNewKeyFrames(); 85 | void ProcessNewKeyFrame(); 86 | void CreateNewMapPoints(); 87 | 88 | void MapPointCulling(); 89 | void SearchInNeighbors(); 90 | 91 | void KeyFrameCulling(); 92 | 93 | cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); 94 | 95 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 96 | 97 | bool mbMonocular; 98 | 99 | void ResetIfRequested(); 100 | bool mbResetRequested; 101 | std::mutex mMutexReset; 102 | 103 | bool CheckFinish(); 104 | void SetFinish(); 105 | bool mbFinishRequested; 106 | bool mbFinished; 107 | std::mutex mMutexFinish; 108 | 109 | Map* mpMap; 110 | 111 | LoopClosing* mpLoopCloser; 112 | Tracking* mpTracker; 113 | 114 | std::list mlNewKeyFrames; 115 | 116 | KeyFrame* mpCurrentKeyFrame; 117 | 118 | std::list mlpRecentAddedMapPoints; 119 | 120 | std::mutex mMutexNewKFs; 121 | 122 | bool mbAbortBA; 123 | 124 | bool mbStopped; 125 | bool mbStopRequested; 126 | bool mbNotStop; 127 | std::mutex mMutexStop; 128 | 129 | bool mbAcceptKeyFrames; 130 | std::mutex mMutexAccept; 131 | }; 132 | 133 | } //namespace ORB_SLAM 134 | 135 | #endif // LOCALMAPPING_H 136 | -------------------------------------------------------------------------------- /src/orb_slam/include/LoopClosing.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOOPCLOSING_H 22 | #define LOOPCLOSING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "LocalMapping.h" 26 | #include "Map.h" 27 | #include "ORBVocabulary.h" 28 | #include "Tracking.h" 29 | 30 | #include "KeyFrameDatabase.h" 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace ORB_SLAM2 37 | { 38 | 39 | class Tracking; 40 | class LocalMapping; 41 | class KeyFrameDatabase; 42 | 43 | 44 | class LoopClosing 45 | { 46 | public: 47 | 48 | typedef std::pair,int> ConsistentGroup; 49 | typedef std::map, 50 | Eigen::aligned_allocator > > KeyFrameAndPose; 51 | 52 | public: 53 | 54 | LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale); 55 | 56 | void SetTracker(Tracking* pTracker); 57 | 58 | void SetLocalMapper(LocalMapping* pLocalMapper); 59 | 60 | // Main function 61 | void Run(); 62 | 63 | void InsertKeyFrame(KeyFrame *pKF); 64 | 65 | void RequestReset(); 66 | 67 | // This function will run in a separate thread 68 | void RunGlobalBundleAdjustment(unsigned long nLoopKF); 69 | 70 | bool isRunningGBA(){ 71 | std::unique_lock lock(mMutexGBA); 72 | return mbRunningGBA; 73 | } 74 | 75 | bool isFinishedGBA(){ 76 | std::unique_lock lock(mMutexGBA); 77 | return mbFinishedGBA; 78 | } 79 | 80 | void RequestFinish(); 81 | 82 | bool isFinished(); 83 | size_t GetLastLoopKFid(); 84 | 85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 86 | 87 | protected: 88 | 89 | bool CheckNewKeyFrames(); 90 | 91 | bool DetectLoop(); 92 | 93 | bool ComputeSim3(); 94 | 95 | void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap); 96 | 97 | void CorrectLoop(); 98 | 99 | void ResetIfRequested(); 100 | bool mbResetRequested; 101 | std::mutex mMutexReset; 102 | 103 | bool CheckFinish(); 104 | void SetFinish(); 105 | bool mbFinishRequested; 106 | bool mbFinished; 107 | std::mutex mMutexFinish; 108 | 109 | Map* mpMap; 110 | Tracking* mpTracker; 111 | 112 | KeyFrameDatabase* mpKeyFrameDB; 113 | ORBVocabulary* mpORBVocabulary; 114 | 115 | LocalMapping *mpLocalMapper; 116 | 117 | std::list mlpLoopKeyFrameQueue; 118 | 119 | std::mutex mMutexLoopQueue; 120 | 121 | // Loop detector parameters 122 | float mnCovisibilityConsistencyTh; 123 | 124 | // Loop detector variables 125 | KeyFrame* mpCurrentKF; 126 | KeyFrame* mpMatchedKF; 127 | std::vector mvConsistentGroups; 128 | std::vector mvpEnoughConsistentCandidates; 129 | std::vector mvpCurrentConnectedKFs; 130 | std::vector mvpCurrentMatchedPoints; 131 | std::vector mvpLoopMapPoints; 132 | cv::Mat mScw; 133 | g2o::Sim3 mg2oScw; 134 | 135 | long unsigned int mLastLoopKFid; 136 | 137 | // Variables related to Global Bundle Adjustment 138 | bool mbRunningGBA; 139 | bool mbFinishedGBA; 140 | bool mbStopGBA; 141 | std::mutex mMutexGBA; 142 | std::thread* mpThreadGBA; 143 | 144 | // Fix scale in the stereo/RGB-D case 145 | bool mbFixScale; 146 | 147 | 148 | int mnFullBAIdx; 149 | }; 150 | 151 | } //namespace ORB_SLAM 152 | 153 | #endif // LOOPCLOSING_H 154 | -------------------------------------------------------------------------------- /src/orb_slam/include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAP_H 22 | #define MAP_H 23 | 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include 27 | 28 | #include 29 | 30 | 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class MapPoint; 36 | class KeyFrame; 37 | 38 | class Map 39 | { 40 | public: 41 | Map(); 42 | friend class boost::serialization::access; // for serialization 43 | template 44 | void serialize(Archive & ar, const unsigned int version) 45 | { 46 | std::vector mvMapPoints; 47 | for(auto it=mspMapPoints.begin(); it!=mspMapPoints.end();++it){ 48 | mvMapPoints.push_back(**it); 49 | } 50 | ar& mvMapPoints; 51 | 52 | } 53 | void AddKeyFrame(KeyFrame* pKF); 54 | void AddMapPoint(MapPoint* pMP); 55 | void EraseMapPoint(MapPoint* pMP); 56 | void EraseKeyFrame(KeyFrame* pKF); 57 | void SetReferenceMapPoints(const std::vector &vpMPs); 58 | void InformNewBigChange(); 59 | int GetLastBigChangeIdx(); 60 | 61 | std::vector GetAllKeyFrames(bool onlyGood=false); 62 | std::vector GetAllMapPoints(bool onlyGood=false); 63 | std::vector GetReferenceMapPoints(); 64 | static bool lId(MapPoint* pMpt1, MapPoint* pMpt2); 65 | long unsigned int MapPointsInMap(); 66 | long unsigned KeyFramesInMap(); 67 | 68 | long unsigned int GetMaxKFid(); 69 | 70 | void clear(); 71 | 72 | std::vector mvpKeyFrameOrigins; 73 | 74 | std::mutex mMutexMapUpdate; 75 | 76 | // This avoid that two points are created simultaneously in separate threads (id conflict) 77 | std::mutex mMutexPointCreation; 78 | void RestoreMap(cv::FileStorage &fs, std::vector &vkFId); 79 | void RestoreMapPointsConnection(cv::FileStorage &fs, std::unordered_map &mapKFId, unordered_map &mapMptId); 80 | 81 | protected: 82 | std::set mspMapPoints; 83 | std::set mspKeyFrames; 84 | 85 | std::vector mvpReferenceMapPoints; 86 | 87 | long unsigned int mnMaxKFid; 88 | 89 | // Index related to a big change in the map (loop closure, global BA) 90 | int mnBigChangeIdx; 91 | 92 | std::mutex mMutexMap; 93 | }; 94 | 95 | } //namespace ORB_SLAM 96 | cv::FileStorage& operator<<(cv::FileStorage &fs, ORB_SLAM2::Map &map); 97 | #endif // MAP_H 98 | -------------------------------------------------------------------------------- /src/orb_slam/include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPDRAWER_H 22 | #define MAPDRAWER_H 23 | 24 | #include"Map.h" 25 | #include"MapPoint.h" 26 | #include"KeyFrame.h" 27 | #include 28 | 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class MapDrawer 35 | { 36 | public: 37 | MapDrawer(Map* pMap, const std::string &strSettingPath); 38 | 39 | Map* mpMap; 40 | 41 | void DrawMapPoints(); 42 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 43 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 44 | void SetCurrentCameraPose(const cv::Mat &Tcw); 45 | void SetReferenceKeyFrame(KeyFrame *pKF); 46 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 47 | 48 | private: 49 | 50 | float mKeyFrameSize; 51 | float mKeyFrameLineWidth; 52 | float mGraphLineWidth; 53 | float mPointSize; 54 | float mCameraSize; 55 | float mCameraLineWidth; 56 | 57 | cv::Mat mCameraPose; 58 | 59 | std::mutex mMutexCamera; 60 | }; 61 | 62 | } //namespace ORB_SLAM 63 | 64 | #endif // MAPDRAWER_H 65 | -------------------------------------------------------------------------------- /src/orb_slam/include/MapPoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPPOINT_H 22 | #define MAPPOINT_H 23 | 24 | #include"KeyFrame.h" 25 | #include"Frame.h" 26 | #include"Map.h" 27 | 28 | #include 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class KeyFrame; 35 | class Map; 36 | class Frame; 37 | 38 | 39 | class MapPoint 40 | { 41 | public: 42 | MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap); 43 | MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF); 44 | MapPoint(const cv::FileNode &fn, Map* pMap); 45 | void SetWorldPos(const cv::Mat &Pos); 46 | cv::Mat GetWorldPos(); 47 | 48 | cv::Mat GetNormal(); 49 | KeyFrame* GetReferenceKeyFrame(); 50 | 51 | std::map GetObservations(); 52 | int Observations(); 53 | 54 | void AddObservation(KeyFrame* pKF,size_t idx); 55 | void AddObservationStatic(KeyFrame* pKF, size_t idx); 56 | void EraseObservation(KeyFrame* pKF); 57 | void AssignReference(KeyFrame* pKF); 58 | 59 | int GetIndexInKeyFrame(KeyFrame* pKF); 60 | bool IsInKeyFrame(KeyFrame* pKF); 61 | 62 | void SetBadFlag(); 63 | bool isBad(); 64 | 65 | void Replace(MapPoint* pMP); 66 | MapPoint* GetReplaced(); 67 | 68 | void IncreaseVisible(int n=1); 69 | void IncreaseFound(int n=1); 70 | float GetFoundRatio(); 71 | inline int GetFound(){ 72 | return mnFound; 73 | } 74 | 75 | void ComputeDistinctiveDescriptors(); 76 | 77 | cv::Mat GetDescriptor(); 78 | 79 | void UpdateNormalAndDepth(); 80 | 81 | float GetMinDistanceInvariance(); 82 | float GetMaxDistanceInvariance(); 83 | int GetVisible(); 84 | 85 | int PredictScale(const float ¤tDist, KeyFrame*pKF); 86 | int PredictScale(const float ¤tDist, Frame* pF); 87 | 88 | public: 89 | long unsigned int mnId; 90 | static long unsigned int nNextId; 91 | long int mnFirstKFid; 92 | long int mnFirstFrame; 93 | int nObs; 94 | 95 | // Variables used by the tracking 96 | float mTrackProjX; 97 | float mTrackProjY; 98 | float mTrackProjXR; 99 | bool mbTrackInView; 100 | int mnTrackScaleLevel; 101 | float mTrackViewCos; 102 | long unsigned int mnTrackReferenceForFrame; 103 | long unsigned int mnLastFrameSeen; 104 | 105 | // Variables used by local mapping 106 | long unsigned int mnBALocalForKF; 107 | long unsigned int mnFuseCandidateForKF; 108 | 109 | // Variables used by loop closing 110 | long unsigned int mnLoopPointForKF; 111 | long unsigned int mnCorrectedByKF; 112 | long unsigned int mnCorrectedReference; 113 | cv::Mat mPosGBA; 114 | long unsigned int mnBAGlobalForKF; 115 | 116 | 117 | static std::mutex mGlobalMutex; 118 | 119 | int mpRefKFId; // For restoration 120 | protected: 121 | 122 | // Position in absolute coordinates 123 | cv::Mat mWorldPos; 124 | 125 | // Keyframes observing the point and associated index in keyframe 126 | std::map mObservations; 127 | 128 | 129 | // Mean viewing direction 130 | cv::Mat mNormalVector; 131 | 132 | // Best descriptor to fast matching 133 | cv::Mat mDescriptor; 134 | 135 | // Reference KeyFrame 136 | KeyFrame* mpRefKF; 137 | 138 | 139 | 140 | // Tracking counters 141 | int mnVisible; 142 | int mnFound; 143 | 144 | // Bad flag (we do not currently erase MapPoint from memory) 145 | bool mbBad; 146 | MapPoint* mpReplaced; 147 | 148 | // Scale invariance distances 149 | float mfMinDistance; 150 | float mfMaxDistance; 151 | 152 | Map* mpMap; 153 | 154 | std::mutex mMutexPos; 155 | std::mutex mMutexFeatures; 156 | }; 157 | 158 | } //namespace ORB_SLAM 159 | cv::FileStorage& operator<<(cv::FileStorage &fs, ORB_SLAM2::MapPoint &mapPoint); 160 | #endif // MAPPOINT_H 161 | -------------------------------------------------------------------------------- /src/orb_slam/include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | #include "../Thirdparty/DBoW2/DBoW2/FORB.h" 25 | #include "../Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 26 | 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /src/orb_slam/include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBEXTRACTOR_H 22 | #define ORBEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class ExtractorNode 34 | { 35 | public: 36 | ExtractorNode():bNoMore(false){} 37 | 38 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 39 | 40 | std::vector vKeys; 41 | cv::Point2i UL, UR, BL, BR; 42 | std::list::iterator lit; 43 | bool bNoMore; 44 | }; 45 | 46 | class ORBextractor 47 | { 48 | public: 49 | 50 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 51 | 52 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 53 | int iniThFAST, int minThFAST); 54 | 55 | ~ORBextractor(){} 56 | 57 | // Compute the ORB features and descriptors on an image. 58 | // ORB are dispersed on the image using an octree. 59 | // Mask is ignored in the current implementation. 60 | void operator()( cv::InputArray image, cv::InputArray mask, 61 | std::vector& keypoints, 62 | cv::OutputArray descriptors); 63 | 64 | int inline GetLevels(){ 65 | return nlevels;} 66 | 67 | float inline GetScaleFactor(){ 68 | return scaleFactor;} 69 | 70 | std::vector inline GetScaleFactors(){ 71 | return mvScaleFactor; 72 | } 73 | 74 | std::vector inline GetInverseScaleFactors(){ 75 | return mvInvScaleFactor; 76 | } 77 | 78 | std::vector inline GetScaleSigmaSquares(){ 79 | return mvLevelSigma2; 80 | } 81 | 82 | std::vector inline GetInverseScaleSigmaSquares(){ 83 | return mvInvLevelSigma2; 84 | } 85 | 86 | std::vector mvImagePyramid; 87 | 88 | protected: 89 | 90 | void ComputePyramid(cv::Mat image); 91 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 92 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 93 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 94 | 95 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 96 | std::vector pattern; 97 | 98 | int nfeatures; 99 | double scaleFactor; 100 | int nlevels; 101 | int iniThFAST; 102 | int minThFAST; 103 | 104 | std::vector mnFeaturesPerLevel; 105 | 106 | std::vector umax; 107 | 108 | std::vector mvScaleFactor; 109 | std::vector mvInvScaleFactor; 110 | std::vector mvLevelSigma2; 111 | std::vector mvInvLevelSigma2; 112 | }; 113 | 114 | } //namespace ORB_SLAM 115 | 116 | #endif 117 | 118 | -------------------------------------------------------------------------------- /src/orb_slam/include/ORBmatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBMATCHER_H 23 | #define ORBMATCHER_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include"MapPoint.h" 30 | #include"KeyFrame.h" 31 | #include"Frame.h" 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class ORBmatcher 38 | { 39 | public: 40 | 41 | ORBmatcher(float nnratio=0.6, bool checkOri=true); 42 | 43 | // Computes the Hamming distance between two ORB descriptors 44 | static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 45 | 46 | // Search matches between Frame keypoints and projected MapPoints. Returns number of matches 47 | // Used to track the local map (Tracking) 48 | int SearchByProjection(Frame &F, const std::vector &vpMapPoints, const float th=3); 49 | 50 | // Project MapPoints tracked in last frame into the current frame and search matches. 51 | // Used to track from previous frame (Tracking) 52 | int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono); 53 | 54 | // Project MapPoints seen in KeyFrame into the Frame and search matches. 55 | // Used in relocalisation (Tracking) 56 | int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set &sAlreadyFound, const float th, const int ORBdist); 57 | 58 | // Project MapPoints using a Similarity Transformation and search matches. 59 | // Used in loop detection (Loop Closing) 60 | int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, std::vector &vpMatched, int th); 61 | 62 | // Search matches between MapPoints in a KeyFrame and ORB in a Frame. 63 | // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) 64 | // Used in Relocalisation and Loop Detection 65 | int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); 66 | int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); 67 | 68 | // Matching for the Map Initialization (only used in the monocular case) 69 | int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); 70 | 71 | // Matching to triangulate new MapPoints. Check Epipolar Constraint. 72 | int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12, 73 | std::vector > &vMatchedPairs, const bool bOnlyStereo); 74 | 75 | // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12] 76 | // In the stereo and RGB-D case, s12=1 77 | int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th); 78 | 79 | // Project MapPoints into KeyFrame and search for duplicated MapPoints. 80 | int Fuse(KeyFrame* pKF, const std::vector &vpMapPoints, const float th=3.0); 81 | 82 | // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints. 83 | int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, float th, std::vector &vpReplacePoint); 84 | 85 | public: 86 | 87 | static const int TH_LOW; 88 | static const int TH_HIGH; 89 | static const int HISTO_LENGTH; 90 | 91 | 92 | protected: 93 | 94 | bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF); 95 | 96 | float RadiusByViewingCos(const float &viewCos); 97 | 98 | void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3); 99 | 100 | float mfNNratio; 101 | bool mbCheckOrientation; 102 | }; 103 | 104 | }// namespace ORB_SLAM 105 | 106 | #endif // ORBMATCHER_H 107 | -------------------------------------------------------------------------------- /src/orb_slam/include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | #include "g2o/core/block_solver.h" 30 | #include "g2o/core/optimization_algorithm_levenberg.h" 31 | #include "g2o/solvers/eigen/linear_solver_eigen.h" 32 | #include "g2o/types/sba/types_six_dof_expmap.h" 33 | #include "g2o/core/robust_kernel_impl.h" 34 | #include "g2o/solvers/dense/linear_solver_dense.h" 35 | #include "g2o/types/sim3/types_seven_dof_expmap.h" 36 | #include "g2o/core/auto_differentiation.h" 37 | #include 38 | #include 39 | #include 40 | namespace ORB_SLAM2 41 | { 42 | 43 | class LoopClosing; 44 | class scaleVertex; 45 | class calibEdge; 46 | class Optimizer 47 | { 48 | public: 49 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 50 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 51 | const bool bRobust = true); 52 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 53 | const unsigned long nLoopKF=0, const bool bRobust = true); 54 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 55 | static int PoseOptimization(Frame* pFrame); 56 | 57 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 58 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 59 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 60 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 61 | const std::map > &LoopConnections, 62 | const bool &bFixScale); 63 | 64 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 65 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 66 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 67 | 68 | //optimize extrinsic 69 | static int OptimizeExtrinsicGlobal(vector &allKF, const vector &vTwl, Eigen::Isometry3d &Tcl, double &scale, bool verbose=true); 70 | static int OptimizeExtrinsicLocal(vector &allKF, const vector &vTwl, Eigen::Isometry3d &Tcl, double &scale, bool verbose=true); 71 | }; 72 | 73 | } //namespace ORB_SLAM 74 | 75 | #endif // OPTIMIZER_H 76 | -------------------------------------------------------------------------------- /src/orb_slam/include/Sim3Solver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef SIM3SOLVER_H 23 | #define SIM3SOLVER_H 24 | 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | 30 | 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Sim3Solver 36 | { 37 | public: 38 | 39 | Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12, const bool bFixScale = true); 40 | 41 | void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); 42 | 43 | cv::Mat find(std::vector &vbInliers12, int &nInliers); 44 | 45 | cv::Mat iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); 46 | 47 | cv::Mat GetEstimatedRotation(); 48 | cv::Mat GetEstimatedTranslation(); 49 | float GetEstimatedScale(); 50 | 51 | 52 | protected: 53 | 54 | void ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C); 55 | 56 | void ComputeSim3(cv::Mat &P1, cv::Mat &P2); 57 | 58 | void CheckInliers(); 59 | 60 | void Project(const std::vector &vP3Dw, std::vector &vP2D, cv::Mat Tcw, cv::Mat K); 61 | void FromCameraToImage(const std::vector &vP3Dc, std::vector &vP2D, cv::Mat K); 62 | 63 | 64 | protected: 65 | 66 | // KeyFrames and matches 67 | KeyFrame* mpKF1; 68 | KeyFrame* mpKF2; 69 | 70 | std::vector mvX3Dc1; 71 | std::vector mvX3Dc2; 72 | std::vector mvpMapPoints1; 73 | std::vector mvpMapPoints2; 74 | std::vector mvpMatches12; 75 | std::vector mvnIndices1; 76 | std::vector mvSigmaSquare1; 77 | std::vector mvSigmaSquare2; 78 | std::vector mvnMaxError1; 79 | std::vector mvnMaxError2; 80 | 81 | int N; 82 | int mN1; 83 | 84 | // Current Estimation 85 | cv::Mat mR12i; 86 | cv::Mat mt12i; 87 | float ms12i; 88 | cv::Mat mT12i; 89 | cv::Mat mT21i; 90 | std::vector mvbInliersi; 91 | int mnInliersi; 92 | 93 | // Current Ransac State 94 | int mnIterations; 95 | std::vector mvbBestInliers; 96 | int mnBestInliers; 97 | cv::Mat mBestT12; 98 | cv::Mat mBestRotation; 99 | cv::Mat mBestTranslation; 100 | float mBestScale; 101 | 102 | // Scale is fixed to 1 in the stereo/RGBD case 103 | bool mbFixScale; 104 | 105 | // Indices for random selection 106 | std::vector mvAllIndices; 107 | 108 | // Projections 109 | std::vector mvP1im1; 110 | std::vector mvP2im2; 111 | 112 | // RANSAC probability 113 | double mRansacProb; 114 | 115 | // RANSAC min inliers 116 | int mRansacMinInliers; 117 | 118 | // RANSAC max iterations 119 | int mRansacMaxIts; 120 | 121 | // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2 122 | float mTh; 123 | float mSigma2; 124 | 125 | // Calibration 126 | cv::Mat mK1; 127 | cv::Mat mK2; 128 | 129 | }; 130 | 131 | } //namespace ORB_SLAM 132 | 133 | #endif // SIM3SOLVER_H 134 | -------------------------------------------------------------------------------- /src/orb_slam/include/Tracking.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef TRACKING_H 23 | #define TRACKING_H 24 | 25 | #include 26 | #include 27 | 28 | #include"Viewer.h" 29 | #include"FrameDrawer.h" 30 | #include"Map.h" 31 | #include"LocalMapping.h" 32 | #include"LoopClosing.h" 33 | #include"Frame.h" 34 | #include "ORBVocabulary.h" 35 | #include"KeyFrameDatabase.h" 36 | #include"ORBextractor.h" 37 | #include "Initializer.h" 38 | #include "MapDrawer.h" 39 | #include "System.h" 40 | 41 | #include 42 | 43 | namespace ORB_SLAM2 44 | { 45 | 46 | class Viewer; 47 | class FrameDrawer; 48 | class Map; 49 | class LocalMapping; 50 | class LoopClosing; 51 | class System; 52 | 53 | class Tracking 54 | { 55 | 56 | public: 57 | Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, 58 | KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor); 59 | 60 | // Preprocess the input and call Track(). Extract features and performs stereo matching. 61 | cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); 62 | cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp); 63 | cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp); 64 | 65 | void SetLocalMapper(LocalMapping* pLocalMapper); 66 | void SetLoopClosing(LoopClosing* pLoopClosing); 67 | void SetViewer(Viewer* pViewer); 68 | // Load new settings 69 | // The focal lenght should be similar or scale prediction will fail when projecting points 70 | // TODO: Modify MapPoint::PredictScale to take into account focal lenght 71 | void ChangeCalibration(const std::string &strSettingPath); 72 | 73 | // Use this function if you have deactivated local mapping and you only want to localize the camera. 74 | void InformOnlyTracking(const bool &flag); 75 | public: 76 | 77 | // Tracking states 78 | enum eTrackingState{ 79 | SYSTEM_NOT_READY=-1, 80 | NO_IMAGES_YET=0, 81 | NOT_INITIALIZED=1, 82 | OK=2, 83 | LOST=3 84 | }; 85 | 86 | eTrackingState mState; 87 | eTrackingState mLastProcessedState; 88 | 89 | // Input sensor 90 | int mSensor; 91 | 92 | // Current Frame 93 | Frame mCurrentFrame; 94 | cv::Mat mImGray; 95 | 96 | // Initialization Variables (Monocular) 97 | std::vector mvIniLastMatches; 98 | std::vector mvIniMatches; 99 | std::vector mvbPrevMatched; 100 | std::vector mvIniP3D; 101 | Frame mInitialFrame; 102 | 103 | // std::lists used to recover the full camera trajectory at the end of the execution. 104 | // Basically we store the reference keyframe for each frame and its relative transformation 105 | std::list mlRelativeFramePoses; 106 | std::list mlpReferences; 107 | std::list mlFrameTimes; 108 | std::list mlbLost; 109 | 110 | // True if local mapping is deactivated and we are performing only localization 111 | bool mbOnlyTracking; 112 | long unsigned int GetCurrentFid(); 113 | void Reset(); 114 | 115 | protected: 116 | 117 | // Main tracking function. It is independent of the input sensor. 118 | void Track(); 119 | 120 | // Map initialization for stereo and RGB-D 121 | void StereoInitialization(); 122 | 123 | // Map initialization for monocular 124 | void MonocularInitialization(); 125 | void CreateInitialMapMonocular(); 126 | 127 | void CheckReplacedInLastFrame(); 128 | bool TrackReferenceKeyFrame(); 129 | void UpdateLastFrame(); 130 | bool TrackWithMotionModel(); 131 | 132 | bool Relocalization(); 133 | 134 | void UpdateLocalMap(); 135 | void UpdateLocalPoints(); 136 | void UpdateLocalKeyFrames(); 137 | 138 | bool TrackLocalMap(); 139 | void SearchLocalPoints(); 140 | 141 | bool NeedNewKeyFrame(); 142 | void CreateNewKeyFrame(); 143 | 144 | // In case of performing only localization, this flag is true when there are no matches to 145 | // points in the map. Still tracking will continue if there are enough matches with temporal points. 146 | // In that case we are doing visual odometry. The system will try to do relocalization to recover 147 | // "zero-drift" localization to the map. 148 | bool mbVO; 149 | 150 | //Other Thread Pointers 151 | LocalMapping* mpLocalMapper; 152 | LoopClosing* mpLoopClosing; 153 | 154 | //ORB 155 | ORBextractor* mpORBextractorLeft, *mpORBextractorRight; 156 | ORBextractor* mpIniORBextractor; 157 | 158 | //BoW 159 | ORBVocabulary* mpORBVocabulary; 160 | KeyFrameDatabase* mpKeyFrameDB; 161 | 162 | // Initalization (only for monocular) 163 | Initializer* mpInitializer; 164 | 165 | //Local Map 166 | KeyFrame* mpReferenceKF; 167 | std::vector mvpLocalKeyFrames; 168 | std::vector mvpLocalMapPoints; 169 | 170 | // System 171 | System* mpSystem; 172 | 173 | //Drawers 174 | Viewer* mpViewer; 175 | FrameDrawer* mpFrameDrawer; 176 | MapDrawer* mpMapDrawer; 177 | 178 | //Map 179 | Map* mpMap; 180 | 181 | //Calibration matrix 182 | cv::Mat mK; 183 | cv::Mat mDistCoef; 184 | float mbf; 185 | 186 | //New KeyFrame rules (according to fps) 187 | int mMinFrames; 188 | int mMaxFrames; 189 | 190 | // Threshold close/far points 191 | // Points seen as close by the stereo/RGBD sensor are considered reliable 192 | // and inserted from just one frame. Far points requiere a match in two keyframes. 193 | float mThDepth; 194 | 195 | // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled. 196 | float mDepthMapFactor; 197 | 198 | //Current matches in frame 199 | int mnMatchesInliers; 200 | 201 | //Last Frame, KeyFrame and Relocalisation Info 202 | KeyFrame* mpLastKeyFrame; 203 | Frame mLastFrame; 204 | unsigned int mnLastKeyFrameId; 205 | unsigned int mnLastRelocFrameId; 206 | 207 | //Motion Model 208 | cv::Mat mVelocity; 209 | 210 | //Color order (true RGB, false BGR, ignored if grayscale) 211 | bool mbRGB; 212 | 213 | std::list mlpTemporalPoints; 214 | }; 215 | 216 | } //namespace ORB_SLAM 217 | 218 | #endif // TRACKING_H 219 | -------------------------------------------------------------------------------- /src/orb_slam/include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Tracking; 36 | class FrameDrawer; 37 | class MapDrawer; 38 | class System; 39 | 40 | class Viewer 41 | { 42 | public: 43 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const std::string &strSettingPath); 44 | 45 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 46 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 47 | void Run(); 48 | 49 | void RequestFinish(); 50 | 51 | void RequestStop(); 52 | 53 | bool isFinished(); 54 | 55 | bool isStopped(); 56 | 57 | void Release(); 58 | 59 | private: 60 | 61 | bool Stop(); 62 | 63 | System* mpSystem; 64 | FrameDrawer* mpFrameDrawer; 65 | MapDrawer* mpMapDrawer; 66 | Tracking* mpTracker; 67 | 68 | // 1/fps in ms 69 | double mT; 70 | float mImageWidth, mImageHeight; 71 | 72 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 73 | 74 | bool CheckFinish(); 75 | void SetFinish(); 76 | bool mbFinishRequested; 77 | bool mbFinished; 78 | std::mutex mMutexFinish; 79 | 80 | bool mbStopped; 81 | bool mbStopRequested; 82 | std::mutex mMutexStop; 83 | 84 | }; 85 | 86 | } 87 | 88 | 89 | #endif // VIEWER_H 90 | 91 | 92 | -------------------------------------------------------------------------------- /src/orb_slam/src/Converter.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include "Converter.h" 23 | 24 | namespace ORB_SLAM2 25 | { 26 | 27 | std::vector Converter::toDescriptorVector(const cv::Mat &Descriptors) 28 | { 29 | std::vector vDesc; 30 | vDesc.reserve(Descriptors.rows); 31 | for (int j=0;j R; 40 | R << cvT.at(0,0), cvT.at(0,1), cvT.at(0,2), 41 | cvT.at(1,0), cvT.at(1,1), cvT.at(1,2), 42 | cvT.at(2,0), cvT.at(2,1), cvT.at(2,2); 43 | 44 | Eigen::Matrix t(cvT.at(0,3), cvT.at(1,3), cvT.at(2,3)); 45 | 46 | return g2o::SE3Quat(R,t); 47 | } 48 | 49 | cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3) 50 | { 51 | Eigen::Matrix eigMat = SE3.to_homogeneous_matrix(); 52 | return toCvMat(eigMat); 53 | } 54 | 55 | cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3) 56 | { 57 | Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix(); 58 | Eigen::Vector3d eigt = Sim3.translation(); 59 | double s = Sim3.scale(); 60 | return toCvSE3(s*eigR,eigt); 61 | } 62 | 63 | cv::Mat Converter::toCvMat(const Eigen::Matrix &m) 64 | { 65 | cv::Mat cvMat(4,4,CV_32F); 66 | for(int i=0;i<4;i++) 67 | for(int j=0; j<4; j++) 68 | cvMat.at(i,j)=m(i,j); 69 | 70 | return cvMat.clone(); 71 | } 72 | 73 | cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m) 74 | { 75 | cv::Mat cvMat(3,3,CV_32F); 76 | for(int i=0;i<3;i++) 77 | for(int j=0; j<3; j++) 78 | cvMat.at(i,j)=m(i,j); 79 | 80 | return cvMat.clone(); 81 | } 82 | 83 | cv::Mat Converter::toCvMat(const Eigen::Matrix &m) 84 | { 85 | cv::Mat cvMat(3,1,CV_32F); 86 | for(int i=0;i<3;i++) 87 | cvMat.at(i)=m(i); 88 | 89 | return cvMat.clone(); 90 | } 91 | 92 | cv::Mat Converter::toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t) 93 | { 94 | cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F); 95 | for(int i=0;i<3;i++) 96 | { 97 | for(int j=0;j<3;j++) 98 | { 99 | cvMat.at(i,j)=R(i,j); 100 | } 101 | } 102 | for(int i=0;i<3;i++) 103 | { 104 | cvMat.at(i,3)=t(i); 105 | } 106 | 107 | return cvMat.clone(); 108 | } 109 | 110 | Eigen::Matrix Converter::toVector3d(const cv::Mat &cvVector) 111 | { 112 | Eigen::Matrix v; 113 | v << cvVector.at(0), cvVector.at(1), cvVector.at(2); 114 | 115 | return v; 116 | } 117 | 118 | Eigen::Matrix Converter::toVector3d(const cv::Point3f &cvPoint) 119 | { 120 | Eigen::Matrix v; 121 | v << cvPoint.x, cvPoint.y, cvPoint.z; 122 | 123 | return v; 124 | } 125 | 126 | Eigen::Matrix Converter::toMatrix3d(const cv::Mat &cvMat3) 127 | { 128 | Eigen::Matrix M; 129 | 130 | M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2), 131 | cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2), 132 | cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2); 133 | 134 | return M; 135 | } 136 | 137 | std::vector Converter::toQuaternion(const cv::Mat &M) 138 | { 139 | Eigen::Matrix eigMat = toMatrix3d(M); 140 | Eigen::Quaterniond q(eigMat); 141 | 142 | std::vector v(4); 143 | v[0] = q.x(); 144 | v[1] = q.y(); 145 | v[2] = q.z(); 146 | v[3] = q.w(); 147 | 148 | return v; 149 | } 150 | 151 | } //namespace ORB_SLAM 152 | -------------------------------------------------------------------------------- /src/orb_slam/src/FrameDrawer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #include "FrameDrawer.h" 22 | #include "Tracking.h" 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | using namespace std; 29 | namespace ORB_SLAM2 30 | { 31 | 32 | FrameDrawer::FrameDrawer(Map* pMap):mpMap(pMap) 33 | { 34 | mState=Tracking::SYSTEM_NOT_READY; 35 | mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0)); 36 | } 37 | 38 | cv::Mat FrameDrawer::DrawFrame() 39 | { 40 | cv::Mat im; 41 | vector vIniKeys; // Initialization: KeyPoints in reference frame 42 | vector vMatches; // Initialization: correspondeces with reference keypoints 43 | vector vCurrentKeys; // KeyPoints in current frame 44 | vector vbVO, vbMap; // Tracked MapPoints in current frame 45 | int state; // Tracking state 46 | 47 | //Copy variables within scoped mutex 48 | { 49 | unique_lock lock(mMutex); 50 | state=mState; 51 | if(mState==Tracking::SYSTEM_NOT_READY) 52 | mState=Tracking::NO_IMAGES_YET; 53 | 54 | mIm.copyTo(im); 55 | 56 | if(mState==Tracking::NOT_INITIALIZED) 57 | { 58 | vCurrentKeys = mvCurrentKeys; 59 | vIniKeys = mvIniKeys; 60 | vMatches = mvIniMatches; 61 | } 62 | else if(mState==Tracking::OK) 63 | { 64 | vCurrentKeys = mvCurrentKeys; 65 | vbVO = mvbVO; 66 | vbMap = mvbMap; 67 | } 68 | else if(mState==Tracking::LOST) 69 | { 70 | vCurrentKeys = mvCurrentKeys; 71 | } 72 | } // destroy scoped mutex -> release mutex 73 | 74 | if(im.channels()<3) //this should be always true 75 | cvtColor(im,im,cv::COLOR_GRAY2BGR); //cv::COLOR_GRAY2BGR 76 | 77 | //Draw 78 | if(state==Tracking::NOT_INITIALIZED) //INITIALIZING 79 | { 80 | for(unsigned int i=0; i=0) 83 | { 84 | cv::line(im,vIniKeys[i].pt,vCurrentKeys[vMatches[i]].pt, 85 | cv::Scalar(0,255,0)); 86 | } 87 | } 88 | } 89 | else if(state==Tracking::OK) //TRACKING 90 | { 91 | mnTracked=0; 92 | mnTrackedVO=0; 93 | const float r = 5; 94 | const int n = vCurrentKeys.size(); 95 | for(int i=0;iKeyFramesInMap(); 143 | int nMPs = mpMap->MapPointsInMap(); 144 | s << "KFs: " << nKFs << ", MPs: " << nMPs << ", Matches: " << mnTracked; 145 | if(mnTrackedVO>0) 146 | s << ", + VO matches: " << mnTrackedVO; 147 | } 148 | else if(nState==Tracking::LOST) 149 | { 150 | s << " TRACK LOST. TRYING TO RELOCALIZE "; 151 | } 152 | else if(nState==Tracking::SYSTEM_NOT_READY) 153 | { 154 | s << " LOADING ORB VOCABULARY. PLEASE WAIT..."; 155 | } 156 | 157 | int baseline=0; 158 | cv::Size textSize = cv::getTextSize(s.str(),cv::FONT_HERSHEY_PLAIN,1,1,&baseline); 159 | 160 | imText = cv::Mat(im.rows+textSize.height+10,im.cols,im.type()); 161 | im.copyTo(imText.rowRange(0,im.rows).colRange(0,im.cols)); 162 | imText.rowRange(im.rows,imText.rows) = cv::Mat::zeros(textSize.height+10,im.cols,im.type()); 163 | cv::putText(imText,s.str(),cv::Point(5,imText.rows-5),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,255),1,8); 164 | 165 | } 166 | 167 | void FrameDrawer::Update(Tracking *pTracker) 168 | { 169 | unique_lock lock(mMutex); 170 | pTracker->mImGray.copyTo(mIm); 171 | mvCurrentKeys=pTracker->mCurrentFrame.mvKeys; 172 | N = mvCurrentKeys.size(); 173 | mvbVO = vector(N,false); 174 | mvbMap = vector(N,false); 175 | mbOnlyTracking = pTracker->mbOnlyTracking; 176 | 177 | 178 | if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED) 179 | { 180 | mvIniKeys=pTracker->mInitialFrame.mvKeys; 181 | mvIniMatches=pTracker->mvIniMatches; 182 | } 183 | else if(pTracker->mLastProcessedState==Tracking::OK) 184 | { 185 | for(int i=0;imCurrentFrame.mvpMapPoints[i]; 188 | if(pMP) 189 | { 190 | if(!pTracker->mCurrentFrame.mvbOutlier[i]) 191 | { 192 | if(pMP->Observations()>0) 193 | mvbMap[i]=true; 194 | else 195 | mvbVO[i]=true; 196 | } 197 | } 198 | } 199 | } 200 | mState=static_cast(pTracker->mLastProcessedState); 201 | } 202 | 203 | } //namespace ORB_SLAM 204 | -------------------------------------------------------------------------------- /src/scancontext/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(scancontext) 3 | if(NOT CMAKE_BUILD_TYPE) 4 | set(CMAKE_BUILD_TYPE Release) 5 | endif() 6 | MESSAGE(${PROJECT_NAME} " Build type: " ${CMAKE_BUILD_TYPE}) 7 | set(CMAKE_CXX_FLAGS "-std=c++17") 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g") 9 | 10 | find_package(Eigen3) 11 | if(NOT EIGEN3_FOUND) 12 | # Fallback to cmake_modules 13 | find_package(cmake_modules REQUIRED) 14 | find_package(Eigen REQUIRED) 15 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 16 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only 17 | # Possibly map additional variables to the EIGEN3_ prefix. 18 | else() 19 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 20 | endif() 21 | find_package(PCL REQUIRED) 22 | 23 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/../../lib) 24 | 25 | aux_source_directory(${PROJECT_SOURCE_DIR} DIR_LIB_SRCS) 26 | add_library(scancontext SHARED ${DIR_LIB_SRCS}) 27 | target_link_libraries(scancontext ${EIGEN3_LIBRARIES} ${PCL_LIBRARIES}) 28 | 29 | target_include_directories(scancontext PRIVATE 30 | include 31 | ${PROJECT_SOURCE_DIR}/../ 32 | ${PCL_INCLUDE_DIRS} 33 | ${EIGEN3_INCLUDE_DIRS} 34 | ) -------------------------------------------------------------------------------- /src/scancontext/KDTreeVectorOfVectorsAdaptor.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include "scancontext/nanoflann.hpp" 32 | 33 | #include 34 | 35 | // ===== This example shows how to use nanoflann with these types of containers: ======= 36 | //typedef std::vector > my_vector_of_vectors_t; 37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include 38 | // ===================================================================================== 39 | 40 | 41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. 42 | * The i'th vector represents a point in the state space. 43 | * 44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. 45 | * \tparam num_t The type of the point coordinates (typically, double or float). 46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) 48 | */ 49 | template 50 | struct KDTreeVectorOfVectorsAdaptor 51 | { 52 | typedef KDTreeVectorOfVectorsAdaptor self_t; 53 | typedef typename Distance::template traits::distance_t metric_t; 54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; 55 | 56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. 57 | 58 | /// Constructor: takes a const ref to the vector of vectors object with the data points 59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) 60 | { 61 | assert(mat.size() != 0 && mat[0].size() != 0); 62 | const size_t dims = mat[0].size(); 63 | if (DIM>0 && static_cast(dims) != DIM) 64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); 65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); 66 | index->buildIndex(); 67 | } 68 | 69 | ~KDTreeVectorOfVectorsAdaptor() { 70 | delete index; 71 | } 72 | 73 | const VectorOfVectorsType &m_data; 74 | 75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). 76 | * Note that this is a short-cut method for index->findNeighbors(). 77 | * The user can also call index->... methods as desired. 78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 79 | */ 80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const 81 | { 82 | nanoflann::KNNResultSet resultSet(num_closest); 83 | resultSet.init(out_indices, out_distances_sq); 84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 85 | } 86 | 87 | /** @name Interface expected by KDTreeSingleIndexAdaptor 88 | * @{ */ 89 | 90 | const self_t & derived() const { 91 | return *this; 92 | } 93 | self_t & derived() { 94 | return *this; 95 | } 96 | 97 | // Must return the number of data points 98 | inline size_t kdtree_get_point_count() const { 99 | return m_data.size(); 100 | } 101 | 102 | // Returns the dim'th component of the idx'th point in the class: 103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { 104 | return m_data[idx][dim]; 105 | } 106 | 107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 110 | template 111 | bool kdtree_get_bbox(BBOX & /*bb*/) const { 112 | return false; 113 | } 114 | 115 | /** @} */ 116 | 117 | }; // end of KDTreeVectorOfVectorsAdaptor 118 | -------------------------------------------------------------------------------- /src/scancontext/Scancontext.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | // #include 16 | // #include 17 | // #include 18 | // #include 19 | 20 | #include 21 | #include 22 | #include 23 | // #include 24 | 25 | #include "scancontext/nanoflann.hpp" 26 | #include "scancontext/KDTreeVectorOfVectorsAdaptor.h" 27 | 28 | 29 | using namespace Eigen; 30 | using namespace nanoflann; 31 | 32 | using std::cout; 33 | using std::endl; 34 | using std::make_pair; 35 | 36 | using std::atan2; 37 | using std::cos; 38 | using std::sin; 39 | 40 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) 41 | using KeyMat = std::vector >; 42 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; 43 | 44 | 45 | namespace SC2 46 | { 47 | 48 | void coreImportTest ( void ); 49 | 50 | 51 | // sc param-independent helper functions 52 | float xy2theta( const float & _x, const float & _y ); 53 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ); 54 | std::vector eig2stdvec( MatrixXd _eigmat ); 55 | 56 | 57 | class SCManager 58 | { 59 | public: 60 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. 61 | 62 | Eigen::MatrixXd makeScancontext(const pcl::PointCloud & _scan_down ); 63 | Eigen::MatrixXd makeScancontext(const std::vector & _scan_down ); 64 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); 65 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); 66 | 67 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 68 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) 69 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) 70 | 71 | // User-side API 72 | void makeAndSaveScancontextAndKeys(const pcl::PointCloud & _scan_down ); 73 | void makeAndSaveScancontextAndKeys(const std::vector & _scan_down ); 74 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw 75 | 76 | // for ltslam 77 | // User-side API for multi-session 78 | void saveScancontextAndKeys( Eigen::MatrixXd _scd ); 79 | std::pair detectLoopClosureIDBetweenSession ( std::vector& curr_key, Eigen::MatrixXd& curr_desc); 80 | 81 | const Eigen::MatrixXd& getConstRefRecentSCD(void); 82 | 83 | public: 84 | // hyper parameters () 85 | const double LIDAR_HEIGHT = 0.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. 86 | 87 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) 88 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) 89 | double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) 90 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); 91 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); 92 | 93 | // tree 94 | const int NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. 95 | const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper) 96 | 97 | // loop thres 98 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. 99 | // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) 100 | 101 | double SC_DIST_THRES = 0.2; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 102 | // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 103 | 104 | // config 105 | const int TREE_MAKING_PERIOD_ = 30; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N. 106 | int tree_making_period_conter = 0; 107 | 108 | // setter 109 | void setSCdistThres(double _new_thres); 110 | void setMaximumRadius(double _max_r); 111 | 112 | // data 113 | std::vector polarcontexts_timestamp_; // optional. 114 | std::vector polarcontexts_; 115 | std::vector polarcontext_invkeys_; 116 | std::vector polarcontext_vkeys_; 117 | 118 | KeyMat polarcontext_invkeys_mat_; 119 | KeyMat polarcontext_invkeys_to_search_; 120 | std::unique_ptr polarcontext_tree_; 121 | 122 | bool is_tree_batch_made = false; 123 | std::unique_ptr polarcontext_tree_batch_; 124 | bool verborse = false; 125 | }; // SCManager 126 | 127 | } // namespace SC2 128 | -------------------------------------------------------------------------------- /src/scancontext/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | class TicToc 12 | { 13 | public: 14 | TicToc() 15 | { 16 | tic(); 17 | } 18 | 19 | void tic() 20 | { 21 | start = std::chrono::system_clock::now(); 22 | } 23 | 24 | double toc() 25 | { 26 | end = std::chrono::system_clock::now(); 27 | std::chrono::duration elapsed_seconds = end - start; 28 | return elapsed_seconds.count() * 1000; 29 | } 30 | 31 | private: 32 | std::chrono::time_point start, end; 33 | }; 34 | 35 | class TicTocV2 36 | { 37 | public: 38 | TicTocV2() 39 | { 40 | tic(); 41 | } 42 | 43 | TicTocV2( bool _disp ) 44 | { 45 | disp_ = _disp; 46 | tic(); 47 | } 48 | 49 | void tic() 50 | { 51 | start = std::chrono::system_clock::now(); 52 | } 53 | 54 | void toc( std::string _about_task ) 55 | { 56 | end = std::chrono::system_clock::now(); 57 | std::chrono::duration elapsed_seconds = end - start; 58 | double elapsed_ms = elapsed_seconds.count() * 1000; 59 | 60 | if( disp_ ) 61 | { 62 | std::cout.precision(3); // 10 for sec, 3 for ms 63 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 64 | } 65 | } 66 | 67 | private: 68 | std::chrono::time_point start, end; 69 | bool disp_ = false; 70 | }; -------------------------------------------------------------------------------- /src/test/test_gpr.cpp: -------------------------------------------------------------------------------- 1 | #include "GPR.hpp" 2 | #include "yaml-cpp/yaml.h" 3 | 4 | 5 | /** 6 | * @brief load train_x, train_y, test_x, test_y 7 | * 8 | * @param filename 9 | * @return std::tuple 10 | */ 11 | std::tuple, Eigen::VectorXd, Eigen::Vector2d, double, double, double> load_data(const std::string &filename) 12 | { 13 | const YAML::Node node = YAML::LoadFile(filename); 14 | std::vector> train_x; 15 | auto train_x_node = node["train_x"]; 16 | for(const auto &train_x_itemnode: train_x_node) 17 | train_x.push_back(train_x_itemnode.as>()); 18 | std::vector train_y = node["train_y"].as>(); 19 | std::vector test_x = node["test_x"].as>(); 20 | double test_y = node["test_y"].as(); 21 | double sigma = node["sigma"].as(); 22 | double l = node["l"].as(); 23 | std::vector trX; 24 | Eigen::VectorXd trY; 25 | Eigen::Vector2d teX; 26 | trY.resize(train_y.size()); 27 | for(const auto &trx : train_x) 28 | { 29 | Eigen::Vector2d trX_eig(trx.data()); 30 | trX.push_back(std::move(trX_eig)); 31 | } 32 | for(Eigen::Index ri = 0; ri < trY.rows(); ++ri) 33 | trY(ri) = train_y[ri]; 34 | teX[0] = test_x[0]; 35 | teX[1] = test_x[1]; 36 | return {trX, trY, teX, test_y, sigma, l}; 37 | } 38 | 39 | 40 | int main(int argc, char** argv) 41 | { 42 | std::vector trX; 43 | Eigen::VectorXd trY; 44 | Eigen::Vector2d teX; 45 | double teY; 46 | double sigma, l; 47 | std::tie(trX, trY, teX, teY, sigma, l) = load_data("../debug/test_gpr.yml"); 48 | std::cout << "trX:\n"; 49 | for(auto const &trx:trX) 50 | std::cout << trx.transpose() << std::endl; 51 | std::cout << "trY:" << trY.transpose() << std::endl; 52 | std::cout << "teX:" << teX.transpose() << std::endl; 53 | std::cout << "teY:" << teY << std::endl; 54 | auto gpr_params = GPRParams(); 55 | gpr_params.sigma = sigma; 56 | gpr_params.l = l; 57 | gpr_params.optimize = true; 58 | gpr_params.verborse = true; 59 | auto gpr = GPR(gpr_params); 60 | std::tie(sigma, l) = gpr.fit(trX, trY); 61 | std::printf("sigma: %lf, l: %lf\n", sigma, l); 62 | double preY = gpr.predict(teX); 63 | std::printf("predict y: %lf, real y: %lf ,knn y: %lf\n", preY, teY, trY[0]); 64 | 65 | } -------------------------------------------------------------------------------- /src/test/test_igl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "io_tools.h" 13 | #include "open3d/geometry/PointCloud.h" 14 | #include "open3d/geometry/KDTreeSearchParam.h" 15 | #include "open3d/geometry/TriangleMesh.h" 16 | 17 | int main(int argc, char *argv[]) 18 | { 19 | using namespace Eigen; 20 | std::string filename = "../data/00/velodyne/000000.bin"; 21 | if(argc>1) 22 | { 23 | filename = argv[1]; 24 | } 25 | else 26 | { 27 | std::printf("Expected test pointcloud file, for example: %s\n",filename.c_str()); 28 | exit(0); 29 | } 30 | std::vector pointcloud_arr; 31 | readPointCloud(filename, pointcloud_arr); 32 | auto pointcloud = open3d::geometry::PointCloud(std::move(pointcloud_arr)); 33 | pointcloud.EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.6, 30)); 34 | std::shared_ptr mesh; 35 | std::tie(mesh, std::ignore) = open3d::geometry::TriangleMesh::CreateFromPointCloudPoisson(pointcloud); 36 | Eigen::MatrixXd Vtr = Eigen::Map(reinterpret_cast(mesh->vertices_.data()), 3, mesh->vertices_.size()); // Vertices 37 | Eigen::MatrixXi Ftr = Eigen::Map(reinterpret_cast(mesh->triangles_.data()), 3, mesh->triangles_.size()); // Faces (Connectity of Vertices as Triangles) 38 | auto const &V = Vtr.transpose(); // N, 3 39 | auto const &F = Ftr.transpose(); // N, 3 40 | std::printf("V:(%ld,%ld), F:(%ld,%ld)\n", V.rows(), V.cols(), F.rows(), F.cols()); 41 | // Alternative discrete mean curvature 42 | MatrixXd HN; 43 | SparseMatrix L,M,Minv; 44 | igl::cotmatrix(V,F,L); 45 | igl::massmatrix(V,F,igl::MASSMATRIX_TYPE_VORONOI,M); 46 | igl::invert_diag(M,Minv); 47 | // Laplace-Beltrami of position 48 | HN = -Minv*(L*V); 49 | // Extract magnitude as mean curvature 50 | VectorXd H = HN.rowwise().norm(); 51 | 52 | // Compute curvature directions via quadric fitting 53 | MatrixXd PD1,PD2; 54 | VectorXd PV1,PV2; 55 | igl::principal_curvature(V,F,PD1,PD2,PV1,PV2); 56 | // mean curvature 57 | H = 0.5*(PV1+PV2); 58 | 59 | igl::opengl::glfw::Viewer viewer; 60 | viewer.data().set_mesh(V, F); 61 | 62 | viewer.data().set_data(H); 63 | 64 | // Average edge length for sizing 65 | const double avg = igl::avg_edge_length(V,F); 66 | 67 | // Draw a red segment parallel to the maximal curvature direction 68 | const RowVector3d red(0.8,0.2,0.2),blue(0.2,0.2,0.8); 69 | viewer.data().add_edges(V + PD1*avg, V - PD1*avg, red); 70 | 71 | // Draw a blue segment parallel to the minimal curvature direction 72 | viewer.data().add_edges(V + PD2*avg, V - PD2*avg, blue); 73 | 74 | // Hide wireframe 75 | viewer.data().show_lines = false; 76 | 77 | viewer.launch(); 78 | } --------------------------------------------------------------------------------