├── .gitignore ├── Apps ├── DefSLAM ├── DefSLAMGT ├── DefSLAMGTCT ├── DefSLAMHamyln ├── DefSLAMHamylnINV ├── rmoutput.sh ├── runHamlyn.sh ├── runHamlynstd.sh ├── runHamlynstd2.sh ├── rungt.sh ├── runvalgrind.sh ├── scriptCT ├── simple_CT.cc ├── simple_camera.cc ├── simple_endoscope.cc ├── stereo_Hamlyn.cc ├── stereo_Hamlyn_inv.cc └── stereo_groundtruth.cc ├── CMakeLists.txt ├── Dataloaders ├── CMakeLists.txt ├── Hamlyn │ ├── dataloader.cc │ └── dataloader.h ├── StereoRectifier.cc └── StereoRectifier.h ├── Dependencies.md ├── LICENSE ├── Modules ├── Common │ ├── DefKeyFrame.cc │ ├── DefKeyFrame.h │ ├── DefMap.cc │ ├── DefMap.h │ ├── DefMapPoint.cc │ ├── DefMapPoint.h │ ├── SettingsLoader.cc │ ├── SettingsLoader.h │ ├── System.cc │ └── System.h ├── GroundTruth │ ├── DisparityEstimatorLibelas.cc │ ├── DisparityEstimatorLibelas.h │ ├── GroundTruthCalculator.cc │ ├── GroundTruthCalculator.h │ ├── GroundTruthFrame.cc │ ├── GroundTruthFrame.h │ ├── GroundTruthKeyFrame.cc │ ├── GroundTruthKeyFrame.h │ ├── MinMedianFilter.cc │ └── MinMedianFilter.h ├── Mapping │ ├── DefKLTLocalMapping.cc │ ├── DefKLTLocalMapping.h │ ├── DefLocalMapping.cc │ ├── DefLocalMapping.h │ ├── NormalEstimator.cc │ ├── NormalEstimator.h │ ├── PolySolver.cc │ ├── PolySolver.h │ ├── Schwarp.cc │ ├── Schwarp.h │ ├── SchwarpDatabase.cc │ ├── SchwarpDatabase.h │ ├── ShapeFromNormals.cc │ ├── ShapeFromNormals.h │ ├── Surface.cc │ ├── Surface.h │ ├── SurfacePoint.cc │ ├── SurfacePoint.h │ ├── SurfaceRegistration.cc │ ├── SurfaceRegistration.h │ ├── WarpDatabase.h │ └── diffProp.h ├── Masker │ ├── BorderMask.cc │ ├── BorderMask.h │ ├── BrightMask.cc │ ├── BrightMask.h │ ├── CnnSegmentation.cc │ ├── CnnSegmentation.h │ ├── Filter.h │ ├── Masker.cc │ └── Masker.h ├── Matching │ ├── DefORBmatcher.cc │ ├── DefORBmatcher.h │ ├── LucasKanadeTracker.cc │ └── LucasKanadeTracker.h ├── Settings │ ├── CC_MAC.h │ └── set_MAC.h ├── Template │ ├── Edge.cc │ ├── Edge.h │ ├── Facet.cc │ ├── Facet.h │ ├── LaplacianMesh.cc │ ├── LaplacianMesh.h │ ├── Node.cc │ ├── Node.h │ ├── Template.cc │ ├── Template.h │ ├── TemplateGenerator.cc │ ├── TemplateGenerator.h │ ├── TriangularMesh.cc │ └── TriangularMesh.h ├── ToolsPCL │ ├── CMakeLists.txt │ ├── PCLNormalEstimator.cc │ ├── PCLNormalEstimator.h │ ├── SmootherMLS.cc │ └── SmootherMLS.h ├── Tracking │ ├── DefKLTTracking.cc │ ├── DefKLTTracking.h │ ├── DefOptimizer.cc │ ├── DefOptimizer.h │ ├── DefTracking.cc │ └── DefTracking.h └── Viewer │ ├── DefFrameDrawer.cc │ ├── DefFrameDrawer.h │ ├── DefMapDrawer.cc │ ├── DefMapDrawer.h │ ├── DefViewer.cc │ ├── DefViewer.h │ ├── MeshDrawer.cc │ └── MeshDrawer.h ├── README.md ├── Thirdparty ├── BBS │ ├── CMakeLists.txt │ ├── bbs.cc │ ├── bbs.h │ ├── bbs_MAC.h │ ├── bbs_coloc.cc │ ├── bbs_coloc.h │ └── licence.txt ├── DBoW2 │ ├── CMakeLists.txt │ ├── DBoW2 │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── FClass.h │ │ ├── FORB.cpp │ │ ├── FORB.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ └── TemplatedVocabulary.h │ ├── DUtils │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ ├── LICENSE.txt │ └── README.txt ├── ORBSLAM_2 │ ├── CMakeLists.txt │ ├── LICENSE.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 │ │ ├── PnPsolver.h │ │ ├── Sim3Solver.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 │ │ ├── PnPsolver.cc │ │ ├── Sim3Solver.cc │ │ ├── Tracking.cc │ │ └── Viewer.cc ├── g2o │ ├── CMakeLists.txt │ ├── README.txt │ ├── cmake_modules │ │ ├── FindBLAS.cmake │ │ ├── FindEigen3.cmake │ │ └── FindLAPACK.cmake │ ├── config.h │ ├── config.h.in │ ├── g2o │ │ ├── core │ │ │ ├── base_binary_edge.h │ │ │ ├── base_binary_edge.hpp │ │ │ ├── base_edge.h │ │ │ ├── base_multi_edge.h │ │ │ ├── base_multi_edge.hpp │ │ │ ├── base_unary_edge.h │ │ │ ├── base_unary_edge.hpp │ │ │ ├── base_vertex.h │ │ │ ├── base_vertex.hpp │ │ │ ├── batch_stats.cpp │ │ │ ├── batch_stats.h │ │ │ ├── block_solver.h │ │ │ ├── block_solver.hpp │ │ │ ├── cache.cpp │ │ │ ├── cache.h │ │ │ ├── creators.h │ │ │ ├── eigen_types.h │ │ │ ├── estimate_propagator.cpp │ │ │ ├── estimate_propagator.h │ │ │ ├── factory.cpp │ │ │ ├── factory.h │ │ │ ├── hyper_dijkstra.cpp │ │ │ ├── hyper_dijkstra.h │ │ │ ├── hyper_graph.cpp │ │ │ ├── hyper_graph.h │ │ │ ├── hyper_graph_action.cpp │ │ │ ├── hyper_graph_action.h │ │ │ ├── jacobian_workspace.cpp │ │ │ ├── jacobian_workspace.h │ │ │ ├── linear_solver.h │ │ │ ├── marginal_covariance_cholesky.cpp │ │ │ ├── marginal_covariance_cholesky.h │ │ │ ├── matrix_operations.h │ │ │ ├── matrix_structure.cpp │ │ │ ├── matrix_structure.h │ │ │ ├── openmp_mutex.h │ │ │ ├── optimizable_graph.cpp │ │ │ ├── optimizable_graph.h │ │ │ ├── optimization_algorithm.cpp │ │ │ ├── optimization_algorithm.h │ │ │ ├── optimization_algorithm_dogleg.cpp │ │ │ ├── optimization_algorithm_dogleg.h │ │ │ ├── optimization_algorithm_factory.cpp │ │ │ ├── optimization_algorithm_factory.h │ │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ │ ├── optimization_algorithm_gauss_newton.h │ │ │ ├── optimization_algorithm_levenberg.cpp │ │ │ ├── optimization_algorithm_levenberg.h │ │ │ ├── optimization_algorithm_property.h │ │ │ ├── optimization_algorithm_with_hessian.cpp │ │ │ ├── optimization_algorithm_with_hessian.h │ │ │ ├── parameter.cpp │ │ │ ├── parameter.h │ │ │ ├── parameter_container.cpp │ │ │ ├── parameter_container.h │ │ │ ├── robust_kernel.cpp │ │ │ ├── robust_kernel.h │ │ │ ├── robust_kernel_factory.cpp │ │ │ ├── robust_kernel_factory.h │ │ │ ├── robust_kernel_impl.cpp │ │ │ ├── robust_kernel_impl.h │ │ │ ├── solver.cpp │ │ │ ├── solver.h │ │ │ ├── sparse_block_matrix.h │ │ │ ├── sparse_block_matrix.hpp │ │ │ ├── sparse_block_matrix_ccs.h │ │ │ ├── sparse_block_matrix_diagonal.h │ │ │ ├── sparse_block_matrix_test.cpp │ │ │ ├── sparse_optimizer.cpp │ │ │ └── sparse_optimizer.h │ │ ├── solvers │ │ │ ├── linear_solver_dense.h │ │ │ └── linear_solver_eigen.h │ │ ├── stuff │ │ │ ├── color_macros.h │ │ │ ├── macros.h │ │ │ ├── misc.h │ │ │ ├── os_specific.c │ │ │ ├── os_specific.h │ │ │ ├── property.cpp │ │ │ ├── property.h │ │ │ ├── string_tools.cpp │ │ │ ├── string_tools.h │ │ │ ├── timeutil.cpp │ │ │ └── timeutil.h │ │ └── types │ │ │ ├── se3_ops.h │ │ │ ├── se3_ops.hpp │ │ │ ├── se3quat.h │ │ │ ├── sft_types.h │ │ │ ├── sim3.h │ │ │ ├── types_sba.cpp │ │ │ ├── types_sba.h │ │ │ ├── types_seven_dof_expmap.cpp │ │ │ ├── types_seven_dof_expmap.h │ │ │ ├── types_six_dof_expmap.cpp │ │ │ └── types_six_dof_expmap.h │ ├── lib │ │ └── libg2o.so │ └── license-bsd.txt └── libelas-for-hamlyn │ ├── .gitignore │ ├── 2_disp.pgm │ ├── CMakeLists.txt │ ├── Matlab image rectification │ ├── README │ ├── rectifyAllStereoVideos.m │ ├── rectifyStereo.m │ ├── rectifyStereoFunction.m │ ├── stereoVideo2frames.m │ ├── stereoVideo2monoVideos.m │ └── video2frames.m │ ├── README.TXT │ ├── _disp.pgm │ ├── dataset │ ├── dataloader.h │ └── stereorectifier.h │ ├── elas │ ├── generateRectImages.cpp │ ├── img │ ├── aloe_left.pgm │ ├── aloe_left_disp.pgm │ ├── aloe_right.pgm │ ├── aloe_right_disp.pgm │ ├── cones_left.pgm │ ├── cones_right.pgm │ ├── raindeer_left.pgm │ ├── raindeer_right.pgm │ ├── urban1_left.pgm │ ├── urban1_right.pgm │ ├── urban2_left.pgm │ ├── urban2_right.pgm │ ├── urban3_left.pgm │ ├── urban3_right.pgm │ ├── urban4_left.pgm │ └── urban4_right.pgm │ ├── main.cpp │ ├── mainHamlyn.cpp │ ├── matlab │ ├── demo.m │ ├── elasMex.cpp │ ├── make.m │ └── process.m │ ├── runHamlyn.sh │ ├── runHamlynRectifier.sh │ ├── runHamlynRectifierstd.sh │ ├── runHamlynRectifierstd2.sh │ ├── runHamlynstd.sh │ ├── runHamlynstd2.sh │ └── src │ ├── descriptor.cpp │ ├── descriptor.h │ ├── elas.cpp │ ├── elas.h │ ├── filter.cpp │ ├── filter.h │ ├── image.h │ ├── matrix.cpp │ ├── matrix.h │ ├── timer.h │ ├── triangle.cpp │ └── triangle.h ├── Vocabulary └── ORBvoc.txt.tar.gz ├── build.sh ├── calibration_files ├── Calib_12+39.yaml ├── default.yaml ├── stereo0.yaml └── stereo0_.yaml ├── cmake_modules └── FindEigen3.cmake ├── filters.txt ├── filtersCNN.txt ├── lib ├── libBBS.so ├── libDBoW2.so ├── libDeformableSLAM.so ├── libORBSLAM.so └── libToolsPCL.so ├── run.sh └── runHamlyn.sh /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | Apps/simple 3 | Apps/simpleCT 4 | Apps/simplestereo 5 | Thirdparty/BBS/build/ 6 | Thirdparty/BBS/lib/ 7 | Thirdparty/DBoW2/build/ 8 | Thirdparty/DBoW2/lib/ 9 | Thirdparty/TPS/build/ 10 | Thirdparty/TPS/lib/ 11 | Thirdparty/g2o/build/ 12 | Thirdparty/g2o/config.h 13 | Thirdparty/g2o/lib/ 14 | Thirdparty/Schwarps/build 15 | Thirdparty/Schwarps/lib/ 16 | Thirdparty/Schwarps/BBS/build 17 | Thirdparty/Schwarps/BBS/lib/ 18 | Vocabulary/ORBvoc.txt 19 | build/ 20 | *~ 21 | lib/ 22 | videos/ 23 | test/ 24 | .vscode 25 | Apps/DefSLAM 26 | Apps/DefSLAMGT 27 | Apps/DefSLAMGTCT 28 | .so 29 | Apps/DefSLAM 30 | Apps/DefSLAMGT 31 | Apps/DefSLAMGTCT 32 | lib/libBBS.so 33 | lib/libDBoW2.so 34 | lib/libDeformableSLAM.so 35 | lib/libORBSLAM.so 36 | lib/libToolsPCL.so 37 | Thirdparty/g2o/lib/libg2o.so 38 | -------------------------------------------------------------------------------- /Apps/DefSLAM: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Apps/DefSLAM -------------------------------------------------------------------------------- /Apps/DefSLAMGT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Apps/DefSLAMGT -------------------------------------------------------------------------------- /Apps/DefSLAMGTCT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Apps/DefSLAMGTCT -------------------------------------------------------------------------------- /Apps/DefSLAMHamyln: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Apps/DefSLAMHamyln -------------------------------------------------------------------------------- /Apps/DefSLAMHamylnINV: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Apps/DefSLAMHamylnINV -------------------------------------------------------------------------------- /Apps/rmoutput.sh: -------------------------------------------------------------------------------- 1 | rm 2D* 3D* Error* MapPointUsage* Matches* ScaleVariation.txt 2 | -------------------------------------------------------------------------------- /Apps/runHamlyn.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | local folder_images_left="$local_dir/camera0" 22 | local folder_images_right="$local_dir/camera1" 23 | local pattern_image_right="stereo_im_l_" 24 | local pattern_image_left="stereo_im_r_" 25 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 26 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 27 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 28 | 29 | echo "Searching left images in : " $folder_images_left " with pattern " $pattern_image_right 30 | echo "Searching right images in : " $folder_images_right " with pattern " $pattern_image_left 31 | echo $folder_images_left $folder_images_right $pattern_image_right $pattern_image_left $leftCalibration $rightCalibration $extrinsicCalibration 32 | ./build/elasHamlyn $folder_images_left $folder_images_right $pattern_image_right $pattern_image_left $leftCalibration $rightCalibration $extrinsicCalibration 33 | return 0 34 | ); } 35 | 36 | main $@ 37 | 38 | -------------------------------------------------------------------------------- /Apps/runHamlynstd.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | local vocabulary_file="/home/jose/DefKLTSLAM/Vocabulary/ORBvoc.txt" 22 | 23 | local video="$local_dir/stereo.avi" 24 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 25 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 26 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 27 | 28 | echo "Processing Video : " $video 29 | echo $video $leftCalibration $rightCalibration $extrinsicCalibration 30 | ./DefSLAMHamyln $vocabulary_file $video $leftCalibration $rightCalibration $extrinsicCalibration 31 | return 0 32 | ); } 33 | 34 | main $@ 35 | 36 | -------------------------------------------------------------------------------- /Apps/runHamlynstd2.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | 22 | local calibration_file="/home/jose/DefKLTSLAM/Vocabulary/ORBvoc.txt" 23 | #local videoleft="$local_dir/f7_dynamic_deint_L.avi" 24 | #local videoright="$local_dir/f7_dynamic_deint_R.avi" 25 | local videoleft="$local_dir/left.avi" 26 | local videoright="$local_dir/right.avi" 27 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 28 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 29 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 30 | 31 | echo "Processing Videos : " $videoleft 32 | local args2="$calibration_file $videoleft $videoright $leftCalibration $rightCalibration $extrinsicCalibration" 33 | echo $args2 34 | ./DefSLAMHamyln $args2 35 | return 0 36 | ); } 37 | 38 | main $@ 39 | 40 | -------------------------------------------------------------------------------- /Apps/rungt.sh: -------------------------------------------------------------------------------- 1 | # Run camera 2 | #./DefSLAM /home/jose/DeformableSLAM/Vocabulary/ORBvoc.txt /home/jose/DeformableSLAM/calibration_files/logitechc922.yaml 3 | # Run one video without ground truth 4 | #./DefSLAM /home/jose/DeformableSLAM/Vocabulary/ORBvoc.txt /media/jose/NuevoVol/videosDataset/sequence_heart/hamlyn.yaml /media/jose/NuevoVol/videosDataset/f5phantom/f5_dynamic_deint_L.avi 5 | 6 | # Groundtruth depth image 7 | ./DefSLAMGTCT /home/jose/DefKLTSLAM/Vocabulary/ORBvoc.txt /media/jose/NuevoVol/videosDataset/f5phantom/hamlyn.yaml /media/jose/NuevoVol/videosDataset/f5phantom/f5_dynamic_deint_L.avi /media/jose/NuevoVol/videosDataset/f5phantom/f5/heartDepthMap_ 8 | 9 | # Groundtruth stereo 10 | #./DefSLAMGT /home/jose/DefKLTSLAM/Vocabulary/ORBvoc.txt /media/jose/NuevoVol/videosDataset/Jose/stereo3.yaml /media/jose/NuevoVol/videosDataset/Jose/Mandala3/images /media/jose/NuevoVol/videosDataset/Jose/Mandala3/images /media/jose/NuevoVol/videosDataset/Jose/Mandala3/timestamps/timestamps.txt 11 | 12 | #./DefSLAMHamyln /home/jose/DeformableSLAM/Vocabulary/ORBvoc.txt /media/jose/NuevoVol/videosDataset/Jose/stereo3.yaml /media/jose/NuevoVol/videosDataset/Jose/Mandala3/images /media/jose/NuevoVol/videosDataset/Jose/Mandala3/images /media/jose/NuevoVol/videosDataset/Jose/Mandala3/timestamps/timestamps.txt 13 | -------------------------------------------------------------------------------- /Apps/runvalgrind.sh: -------------------------------------------------------------------------------- 1 | valgrind --leak-check=yes --log-file="valgrindoutput.txt" ./simplestereo /home/jose/DeformableSLAM/Vocabulary/ORBvoc.txt /media/jose/NuevoVol/videosDataset/Jose/stereo3.yaml /media/jose/NuevoVol/videosDataset/Jose/Mandala3/images /media/jose/NuevoVol/videosDataset/Jose/Mandala3/images /media/jose/NuevoVol/videosDataset/Jose/Mandala3/timestamps/timestamps.txt 2 | -------------------------------------------------------------------------------- /Apps/scriptCT: -------------------------------------------------------------------------------- 1 | ../../Vocabulary/ORBvoc.txt /home/jose/DeformableSLAM/videos/f5phantom/hamlyn.yaml /home/jose/DeformableSLAM/videos/f5phantom/f5_dynamic_deint_L.avi /home/jose/DeformableSLAM/videos/f5phantom/f5/heartDepthMap_ 2 | ../../Vocabulary/ORBvoc.txt /home/jose/DeformableSLAM/videos/f7phantom/hamlyn.yaml /home/jose/DeformableSLAM/videos/f7phantom/f7_dynamic_deint_L.avi /home/jose/DeformableSLAM/videos/f7phantom/f7/heartDepthMap_ 3 | -------------------------------------------------------------------------------- /Apps/simple_camera.cc: -------------------------------------------------------------------------------- 1 | // This software is for internal use in the EndoMapper project. 2 | // Not to be re-distributed. 3 | 4 | #include 5 | #include 6 | 7 | int main(int argc, char **argv) 8 | { 9 | std::cout << argc << std::endl; 10 | if ((argc != 4) and (argc != 3)) 11 | { 12 | cerr << endl 13 | << "Usage: ./DefSLAM ORBvocabulary calibrationFile video" << endl 14 | << " or ./DefSLAM ORBvocabulary calibrationFile " << endl 15 | << endl; 16 | return 1; 17 | } 18 | 19 | cv::VideoCapture cap; 20 | if (argc == 3) 21 | cap.open(0); 22 | else 23 | cap.open(argv[3]); 24 | 25 | if (!cap.isOpened()) // check if we succeeded 26 | return -1; 27 | 28 | string arg = argv[2]; 29 | string arg2 = argv[1]; 30 | 31 | // Create SLAM system. It initializes all system threads and gets ready to 32 | // process frames. 33 | defSLAM::System SLAM(argv[1], argv[2], true); 34 | 35 | uint i(-1); 36 | //cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); 37 | //cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); 38 | 39 | uint initial_point(0); 40 | while (cap.isOpened()) 41 | { 42 | cv::Mat imLeft; 43 | std::cout << "Loading : " << (double(i) / initial_point) * 100 << "% " << '\r' << std::flush; 44 | cap >> imLeft; 45 | i++; 46 | if (i < initial_point) 47 | continue; 48 | if (imLeft.empty()) 49 | { 50 | cerr << endl 51 | << "Failed to load image at: " << to_string(i) << endl; 52 | return 1; 53 | } 54 | 55 | SLAM.TrackMonocular(imLeft, i); 56 | } 57 | 58 | SLAM.Shutdown(); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /Dataloaders/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(Dataloader) 3 | 4 | find_package(Eigen3 REQUIRED) 5 | 6 | find_package(OpenCV 4.2.0 REQUIRED) 7 | find_package(Boost 1.60 REQUIRED COMPONENTS filesystem) 8 | 9 | include_directories( 10 | ${PROJECT_SOURCE_DIR} 11 | ${OpenCV_INCLUDE_DIRS} 12 | ${Boost_INCLUDE_DIR} 13 | ) 14 | 15 | add_library(${PROJECT_NAME} SHARED 16 | StereoRectifier.cc 17 | Hamlyn/dataloader.cc 18 | ) 19 | 20 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} Boost::filesystem) 21 | -------------------------------------------------------------------------------- /Dataloaders/Hamlyn/dataloader.cc: -------------------------------------------------------------------------------- 1 | // This software is for internal use in the EndoMapper project. 2 | // Not to be re-distributed. 3 | 4 | #include "dataloader.h" -------------------------------------------------------------------------------- /Dataloaders/StereoRectifier.cc: -------------------------------------------------------------------------------- 1 | // This software is for internal use in the EndoMapper project. 2 | // Not to be re-distributed. 3 | 4 | #include "StereoRectifier.h" -------------------------------------------------------------------------------- /Dependencies.md: -------------------------------------------------------------------------------- 1 | ##List of Known Dependencies 2 | ### DefSLAM version 1.0 3 | 4 | In this document we list all the pieces of code included by defSLAM and linked libraries which are not property of the authors of defSLAM. 5 | 6 | #####Code in Thirdparty folder 7 | * All code in **ORBSLAM_2** folder. 8 | This is a modified version of [ORBSLAM_2](https://github.com/raulmur/ORB_SLAM2) library. All files included are GPL licensed. 9 | 10 | * All code in **DBoW2** folder. 11 | This is a modified version of [DBoW2](https://github.com/dorian3d/DBoW2) and [DLib](https://github.com/dorian3d/DLib) library. All files included are BSD licensed. 12 | 13 | * All code in **g2o** folder. 14 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 15 | 16 | 17 | #####Library dependencies 18 | 19 | * **Pangolin (visualization and user interface)**. 20 | [MIT license](https://en.wikipedia.org/wiki/MIT_License). 21 | 22 | * **OpenCV**. 23 | BSD license. 24 | 25 | * **Eigen3**. 26 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Modules/Common/DefMap.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | #ifndef DEFMAP_H 22 | #define DEFMAP_H 23 | 24 | #include "Map.h" 25 | 26 | #include "DefMapPoint.h" 27 | #include "Template.h" 28 | #include 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | class Map; 35 | class Template; 36 | class KeyFrame; 37 | class MapPoint; 38 | } // namespace ORB_SLAM2 39 | 40 | namespace defSLAM 41 | { 42 | using ORB_SLAM2::KeyFrame; 43 | using ORB_SLAM2::Map; 44 | 45 | class DefMap : public Map 46 | { 47 | public: 48 | DefMap(); 49 | 50 | /*********** 51 | * Constructor: 52 | * Function that generates a template with the current points 53 | * and the estimated surface for the keyframe kf. (The surface 54 | * must have been estimated.) 55 | *********/ 56 | DefMap(std::vector> &, 57 | std::vector> &); 58 | 59 | /*********** 60 | * Function that generates a template with the estimated surface for the keyframe kf. 61 | * (Kf->surface must have been initializated.) 62 | *********/ 63 | virtual void createTemplate(KeyFrame *Kf); 64 | 65 | void createInitialTemplate(KeyFrame *pKF); 66 | // return the template 67 | virtual void clear(); 68 | 69 | // deletes the current template 70 | Template *GetTemplate(); 71 | 72 | // clear the entire map 73 | void clearTemplate(); 74 | 75 | // Return the reference keyframe or the keyframe used to 76 | // create the template 77 | KeyFrame *getLastKFTemplate(); 78 | 79 | // Set the reference keyframe or the keyframe used to 80 | // create the template 81 | void setLastKFTemplate(KeyFrame *); 82 | 83 | public: 84 | bool ThereIsATemplate; 85 | 86 | std::mutex MutexUpdating; 87 | 88 | protected: 89 | Template *Mesh; 90 | KeyFrame *lastKfTemplate_; 91 | std::mutex MutexUsingMap; 92 | std::mutex MutexKf; 93 | }; 94 | 95 | } // namespace defSLAM 96 | 97 | #endif // DEFORMABLE MAP_H 98 | -------------------------------------------------------------------------------- /Modules/GroundTruth/DisparityEstimatorLibelas.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef __DISPARITYLIBESTIMATOR_H__ 11 | #define __DISPARITYLIBESTIMATOR_H__ 12 | 13 | #include "opencv2/core/core.hpp" 14 | #include "opencv2/highgui/highgui.hpp" 15 | #include "opencv2/imgproc/imgproc.hpp" 16 | 17 | namespace stereodisparity 18 | { 19 | class DisparityEstimatorLibelas 20 | { 21 | public: 22 | // Initialize the class 23 | DisparityEstimatorLibelas() = default; 24 | 25 | // Process the disparity of an image 26 | cv::Mat process(const cv::Mat &, const cv::Mat &); 27 | }; 28 | } // namespace stereodisparity 29 | #endif -------------------------------------------------------------------------------- /Modules/GroundTruth/GroundTruthKeyFrame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef GTKEYFRAME_H 23 | #define GTKEYFRAME_H 24 | 25 | #include "DefKeyFrame.h" 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace defSLAM 32 | { 33 | 34 | class DefKeyFrame; 35 | 36 | class GroundTruthKeyFrame : public DefKeyFrame 37 | { 38 | public: 39 | /***** 40 | * Constructor. Not by default 41 | *****/ 42 | GroundTruthKeyFrame() = delete; 43 | /***** 44 | * Constructor. It initializes the parameters for the groundtruth estimation. 45 | * TODO: Implement with depth image as in GroundTruthFrame 46 | *****/ 47 | GroundTruthKeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB); 48 | 49 | //Default Destructor 50 | ~GroundTruthKeyFrame() = default; 51 | /***** 52 | * estimateAngleErrorAndScale. This method estimates the 3D groundtruth of the 53 | * surface estimated for this keyframe. It use the keypoints of the left image 54 | * with a normal and search for estimates the 3D in the right image. It uses the 55 | * pcl library to determinate the normals of the point cloud and compares them 56 | * with the estimated by the NRSfM and the SfN. NRSfM tends to be quite noisy. 57 | */ 58 | float estimateAngleErrorAndScale(); 59 | 60 | // Drawer purposes. Not used now 61 | std::vector mvLocalMapPoints, mvStereoMapPoints; 62 | 63 | private: 64 | cv::Mat imRight; 65 | 66 | bool StereoAvaliable; 67 | 68 | private: 69 | std::vector> posMono_; 70 | std::vector> posStereo_; 71 | 72 | private: 73 | std::mutex mutexPoints; 74 | }; 75 | 76 | } // namespace defSLAM 77 | 78 | #endif // DEFORMATION MAPPOINT_H 79 | -------------------------------------------------------------------------------- /Modules/GroundTruth/MinMedianFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | #ifndef MINMEDIANFILTER_H 22 | #define MINMEDIANFILTER_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace defSLAM 29 | { 30 | namespace PointCloudFilters 31 | { 32 | /********************** 33 | * The funtion minMedianFilter return the non-outliers in a ditribution 34 | * applying a min median filter. 35 | * Arg: 36 | * PointsToFilter std::vector> points to filter 37 | * Return: 38 | * NonOutlier: Index of the points that are in the distribution 39 | * 40 | * *******************/ 41 | std::vector minMedianFilter(std::vector> PointsToFilter); 42 | } // namespace PointCloudFilters 43 | } // namespace defSLAM 44 | 45 | #endif // MINMEDIANFILTER_H 46 | -------------------------------------------------------------------------------- /Modules/Mapping/DefKLTLocalMapping.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef DEFKLTLOCALMAPPING_H 11 | #define DEFKLTLOCALMAPPING_H 12 | 13 | #include "DefLocalMapping.h" 14 | #include "WarpDatabase.h" 15 | namespace ORB_SLAM2 16 | { 17 | class KeyFrame; 18 | } 19 | namespace defSLAM 20 | { 21 | using ORB_SLAM2::KeyFrame; 22 | using ORB_SLAM2::Map; 23 | using ORB_SLAM2::MapDrawer; 24 | 25 | class DefKLTLocalMapping : public DefLocalMapping 26 | { 27 | public: 28 | DefKLTLocalMapping(Map *pMap, const string &strSettingPath); 29 | 30 | DefKLTLocalMapping(Map *pMap, const SettingsLoader &settingLoader); 31 | 32 | ~DefKLTLocalMapping() = default; 33 | 34 | /********************************* 35 | * Create the new map points. They are extracted from the surface 36 | * estimated for the keyframe with the Isometric NRSfM. KLT variation 37 | ********************************/ 38 | virtual void CreateNewMapPoints() override; 39 | }; 40 | 41 | } // namespace defSLAM 42 | 43 | #endif // LOCALMAPPING_H 44 | -------------------------------------------------------------------------------- /Modules/Mapping/NormalEstimator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | #ifndef NORMALESTIMATOR_H 22 | #define NORMALESTIMATOR_H 23 | 24 | #include "PolySolver.h" 25 | #include "Surface.h" 26 | #include "SurfacePoint.h" 27 | #include "WarpDatabase.h" 28 | #include "ceres/ceres.h" 29 | 30 | namespace defSLAM 31 | { 32 | 33 | class PolySolver; 34 | class SchwarpDatabase; 35 | class Surface; 36 | class SurfacePoint; 37 | using ORB_SLAM2::KeyFrame; 38 | 39 | class NormalEstimator 40 | { 41 | public: 42 | // Construtor by default deleted. 43 | NormalEstimator() = delete; 44 | // The program is initialized with the database of warps that contain the information needed 45 | // to estimate the normals. 46 | NormalEstimator(WarpDatabase *WarpDatabase); 47 | 48 | ~NormalEstimator(); 49 | /*********** 50 | * Core function to estimate K1 and K2 that are the two first components of the normal. 51 | * n = [k1, k2 , 1- k1*u-k2*v] 52 | * *********/ 53 | void ObtainK1K2(); 54 | 55 | public: 56 | WarpDatabase *WarpDB; 57 | }; 58 | 59 | } // namespace defSLAM 60 | 61 | #endif // DEFORMATION MAPPOINT_H 62 | -------------------------------------------------------------------------------- /Modules/Mapping/PolySolver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | #ifndef POLYSOLVER_H 22 | #define POLYSOLVER_H 23 | #include 24 | #include 25 | #include 26 | 27 | namespace defSLAM 28 | { 29 | 30 | struct Eqs 31 | { 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | Eigen::Matrix eqs1; 34 | Eigen::Matrix eqs2; 35 | }; 36 | 37 | class PolySolver : public ceres::SizedCostFunction<2, 2> 38 | { 39 | /*********** 40 | * This class is a block of ceres used to estimate the NRSfM. 41 | * The equations used in this class use the warp estimated from 42 | * the image one to the image 2 \eta_{12}. There is jupyter notebook 43 | * and a script in python to develop the polynomials with a simbolic 44 | * library. 45 | * *******/ 46 | 47 | public: 48 | // Initialize 49 | PolySolver(const Eigen::Matrix &eq1, 50 | const Eigen::Matrix &eq2) 51 | { 52 | init_d(eq1, eq2); 53 | } 54 | ~PolySolver() { delete eqs; } 55 | 56 | // Evaluate function for Ceres 57 | bool Evaluate(double const *const *parameters, double *e, 58 | double **Jac) const; 59 | 60 | /**** 61 | * This function gets the differential parameters and 62 | * returns the coefficients of the polynomials. There 63 | * are two polinomials. Eq (13) and (14) of the paper. 64 | * *****/ 65 | static void getCoefficients(double a, double b, double c, double d, 66 | double t1, double t2, double e, double e1, 67 | double x1, double y1, double x2, double y2, 68 | int i, double *eq); 69 | 70 | private: 71 | // Helper to initialize Eigen parameters in a class. It gave 72 | // some problems. 73 | void init_d(const Eigen::Matrix &eq1, 74 | const Eigen::Matrix &eq2) 75 | { 76 | eqs = new Eqs; 77 | eqs->eqs1 = eq1; 78 | eqs->eqs2 = eq2; 79 | } 80 | Eqs *eqs; 81 | }; 82 | } // namespace defSLAM 83 | #endif 84 | -------------------------------------------------------------------------------- /Modules/Mapping/ShapeFromNormals.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef SHAPEFROMNORMALS_H 23 | #define SHAPEFROMNORMALS_H 24 | 25 | #include "Eigen/Dense" 26 | #include "Eigen/Sparse" 27 | #include "KeyFrame.h" 28 | #include "Surface.h" 29 | #include "WarpDatabase.h" 30 | #include 31 | 32 | namespace defSLAM 33 | { 34 | class Surface; 35 | using ORB_SLAM2::KeyFrame; 36 | using ORB_SLAM2::Map; 37 | 38 | // Structure for Eigen. It was created to avoid some issues 39 | // with the initialization 40 | struct SfNEigen 41 | { 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | Eigen::MatrixXd LinearSystem; 44 | Eigen::SparseMatrix LinearSystemSparse; 45 | Eigen::MatrixXd M; // Matrix to impose the normals 46 | Eigen::MatrixXd B; // Bending regularizer. 47 | Eigen::MatrixXd X; 48 | Eigen::MatrixXd Solaux; 49 | }; 50 | 51 | class ShapeFromNormals 52 | { 53 | public: 54 | // Constructor. It initializes the linear system. 55 | ShapeFromNormals(KeyFrame *refKf_, 56 | double bendingWeight_); 57 | // destructor 58 | ~ShapeFromNormals() { delete sfnEigen_; } 59 | /************* 60 | * estimate(). Estimate a surface from the normals registered in 61 | * refKf_ 62 | * ********/ 63 | virtual bool estimate(); 64 | 65 | private: 66 | /************* 67 | * obtainM(). Estimate M that is a matrix that makes the cross 68 | * product with the current control points and penalizes the 69 | * surface estimated if does not accomplish with the normals. 70 | * ********/ 71 | void obtainM(BBS::bbs_t &bbs, KeyFrame *Refdefkf, Eigen::MatrixXd &M); 72 | 73 | private: 74 | // Keyframe whose surface is estimated. 75 | KeyFrame *refKf_; 76 | 77 | double bendingWeight_; // Weight for the bending penalization 78 | 79 | std::vector u_vector_; 80 | std::vector v_vector_; 81 | 82 | int Mrows_; 83 | int Brows; 84 | 85 | private: 86 | void doInit() { sfnEigen_ = new SfNEigen; } 87 | SfNEigen *sfnEigen_; 88 | }; 89 | 90 | } // namespace defSLAM 91 | 92 | #endif // LOCALMAPPING_H 93 | -------------------------------------------------------------------------------- /Modules/Mapping/SurfacePoint.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #include "SurfacePoint.h" 23 | 24 | namespace defSLAM 25 | { 26 | // Constructor. 27 | SurfacePoint::SurfacePoint() 28 | : NormalOn(false) {} 29 | 30 | SurfacePoint::~SurfacePoint() {} 31 | 32 | void SurfacePoint::setNormal(cv::Vec3f &N) 33 | { 34 | Normal = N; 35 | NormalOn = true; 36 | } 37 | 38 | bool SurfacePoint::getNormal(cv::Vec3f &N) 39 | { 40 | if (NormalOn) 41 | { 42 | N = Normal; 43 | return true; 44 | } 45 | else 46 | { 47 | return false; 48 | } 49 | } 50 | void SurfacePoint::setDepth(cv::Vec3f &x3d) { x3D = x3d; } 51 | 52 | void SurfacePoint::getDepth(cv::Vec3f &x3d) { x3d = x3D; } 53 | 54 | bool SurfacePoint::thereisNormal() { return NormalOn; } 55 | } // namespace defSLAM 56 | -------------------------------------------------------------------------------- /Modules/Mapping/SurfacePoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef SURFACEPOINT_H 23 | #define SURFACEPOINT_H 24 | 25 | #include 26 | 27 | namespace defSLAM 28 | { 29 | 30 | class SurfacePoint 31 | { 32 | public: 33 | // Constructor. 34 | SurfacePoint(); 35 | 36 | // Destructor. 37 | ~SurfacePoint(); 38 | 39 | // Set normal 40 | void setNormal(cv::Vec3f &N); 41 | 42 | // Get normal 43 | bool getNormal(cv::Vec3f &N); 44 | 45 | // Check if the normal was set 46 | bool thereisNormal(); 47 | 48 | // Set depth 49 | void setDepth(cv::Vec3f &N); 50 | 51 | // Get depth 52 | void getDepth(cv::Vec3f &x3D); 53 | 54 | private: 55 | bool NormalOn; // Normal is set. 56 | cv::Vec3f Normal; // Normal 57 | cv::Vec3f x3D; // Position 3D 58 | }; 59 | 60 | } // namespace defSLAM 61 | 62 | #endif // SURFACEPOINT_H 63 | -------------------------------------------------------------------------------- /Modules/Mapping/SurfaceRegistration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef SURFACEREGISTRATION_H 23 | #define SURFACEREGISTRATION_H 24 | 25 | #include "KeyFrame.h" 26 | 27 | namespace ORB_SLAM2 28 | { 29 | class KeyFrame; 30 | } 31 | namespace defSLAM 32 | { 33 | using ORB_SLAM2::KeyFrame; 34 | 35 | class SurfaceRegistration 36 | { 37 | public: 38 | // No constructor by default. 39 | SurfaceRegistration() = delete; 40 | // Constructor. 41 | SurfaceRegistration(KeyFrame *Keyframe, double chiLimit_ = 0.07, 42 | bool check_chi = true); 43 | // Destructor. 44 | ~SurfaceRegistration(); 45 | 46 | /************** 47 | * Function to align the point clouds. It returns true if the 48 | * registration is correct. It use a Levenger-Marquard to 49 | * recover the Sim(3) that align the surface estimated with the 50 | * point clouds register in the keyframe. 51 | * ************/ 52 | bool registerSurfaces(); 53 | 54 | private: 55 | KeyFrame *refKF; // Keyframe to align. 56 | double chiLimit_; // Limit of residual to accept a residual 57 | bool check_chi; // Make the alignment and check the residual 58 | }; 59 | 60 | } // namespace defSLAM 61 | 62 | #endif // SURFACEREGISTRATION_H 63 | -------------------------------------------------------------------------------- /Modules/Mapping/WarpDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef WARPDATABASE_H 23 | #define WARPDATABASE_H 24 | 25 | #include "KeyFrame.h" 26 | #include "MapPoint.h" 27 | #include "diffProp.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace defSLAM 33 | { 34 | using ORB_SLAM2::KeyFrame; 35 | using ORB_SLAM2::MapPoint; 36 | typedef std::vector> kr2krdata; 37 | 38 | class DiffProp; 39 | class WarpDatabase 40 | { 41 | /******* 42 | * Base class for the class that estimate the warps and 43 | * each differential properties. DefSLAM implemented with 44 | * Schwarzian warps (See SchwarpDatabase.h). 45 | * ******/ 46 | public: 47 | // Constructor by default 48 | virtual ~WarpDatabase() = default; 49 | 50 | // It insert the keyframe to the database. 51 | virtual void add(KeyFrame *kf) = 0; 52 | 53 | // Delete kf from database 54 | virtual void erase(KeyFrame *kf) = 0; 55 | 56 | // clear the database 57 | virtual void clear() = 0; 58 | 59 | // Get DB of correspondencies and differential properties 60 | std::map &getDiffDatabase() { return mapPointsDB_; } 61 | 62 | // Check if the correspondencies and differential properties 63 | // of a map has changed since it was used. 64 | std::map &getToProccess() { return newInformation_; } 65 | 66 | protected: 67 | std::map newInformation_; // Register of new information 68 | // for a map point. 69 | 70 | std::map mapPointsDB_; // DB of correspondencies and differential properties 71 | // for a pair of keyframes used in normal estimator 72 | }; 73 | } // namespace defSLAM 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /Modules/Mapping/diffProp.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef DIFFPROP_H 23 | #define DIFFPROP_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "KeyFrame.h" 30 | #include 31 | 32 | namespace defSLAM 33 | { 34 | using ORB_SLAM2::KeyFrame; 35 | 36 | class DiffProp 37 | { 38 | /// Class with the differential properties of the warp 39 | /// used in normal estimation. 40 | public: 41 | DiffProp() = default; 42 | ~DiffProp() = default; 43 | 44 | public: 45 | std::pair KFToKF; // Pair of keyframes warped. 46 | // Indexes 47 | size_t idx1; 48 | size_t idx2; 49 | 50 | // Image Points correspondences 51 | float I1u; 52 | float I1v; 53 | float I2u; 54 | float I2v; 55 | 56 | /*********** 57 | * Jacobian Image Points 58 | * J12 = [[J12a,J12c],[J12b,J12d]] 59 | * J21 = [[J21a,J21c],[J21b,J21d]] 60 | ***********/ 61 | float J12a; 62 | float J12b; 63 | float J12c; 64 | float J12d; 65 | float J21a; 66 | float J21b; 67 | float J21c; 68 | float J21d; 69 | 70 | /*********** 71 | * Hessians Image Points 72 | * H12x = [[H12uux,H12uvx],[H12uvx,H12vvx]] 73 | * H12y = [[H12uuy,H12uvy],[H12uvy,H12vvy]] 74 | ***********/ 75 | float H12uux; 76 | float H12uuy; 77 | float H12uvx; 78 | float H12uvy; 79 | float H12vvx; 80 | float H12vvy; 81 | 82 | // first two normal component in both keyframes 83 | // n1 = [k1, k2, 1-k1u-k2u] 84 | float k1; 85 | float k2; 86 | float k12; 87 | float k22; 88 | 89 | // Outlier flag 90 | bool outlier; 91 | }; 92 | 93 | } // namespace defSLAM 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /Modules/Masker/BorderMask.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #include 11 | 12 | #include "BorderMask.h" 13 | #include 14 | 15 | namespace defSLAM 16 | { 17 | cv::Mat BorderMask::generateMask(const cv::Mat &im) 18 | { 19 | cv::Mat imGray = im, mask; 20 | 21 | if (imGray.channels() == 3) 22 | { 23 | cvtColor(imGray, imGray, cv::COLOR_BGR2GRAY); 24 | } 25 | else if (imGray.channels() == 4) 26 | { 27 | cvtColor(imGray, imGray, cv::COLOR_BGR2GRAY); 28 | } 29 | cv::Rect roi(cb_, rb_, imGray.cols - ce_ - cb_, imGray.rows - re_ - rb_); 30 | cv::Mat maska = cv::Mat::zeros(imGray.size(), CV_8U); 31 | maska(roi) = cv::Scalar(255); 32 | maska.setTo(0, imGray == 0); 33 | cv::erode(maska, maska, getStructuringElement(cv::MORPH_RECT, cv::Size(21, 21))); 34 | return maska; 35 | } 36 | 37 | std::string BorderMask::getDescription() 38 | { 39 | return std::string("Border mask with parameters [" + std::to_string(rb_) + "," + std::to_string(re_) + "," + 40 | std::to_string(cb_) + "," + std::to_string(ce_) + "]"); 41 | } 42 | } // namespace defSLAM -------------------------------------------------------------------------------- /Modules/Masker/BorderMask.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef DEFORMABLESLAM_BORDERMASK_H 11 | #define DEFORMABLESLAM_BORDERMASK_H 12 | 13 | #include 14 | 15 | #include "Filter.h" 16 | 17 | namespace defSLAM { 18 | class BorderMask : public Filter{ 19 | /* 20 | * This class defines a mask over the outer borders of the image. It masks out: 21 | * -rb_ and re_ rows from the beginning and the end respectively 22 | * -cb_ and ce_ rows from the beginning and the end respectively 23 | */ 24 | 25 | public: 26 | BorderMask(int rb, int re, int cb, int ce, int th) : 27 | rb_(rb),re_(re),cb_(cb),ce_(ce), th_(th) {} 28 | 29 | cv::Mat generateMask(const cv::Mat &im); 30 | 31 | std::string getDescription(); 32 | 33 | private: 34 | int th_; 35 | int rb_,re_,cb_,ce_; 36 | }; 37 | } 38 | 39 | 40 | #endif //DEFORMABLESLAM_BORDERMASK_H 41 | -------------------------------------------------------------------------------- /Modules/Masker/BrightMask.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #include 11 | 12 | #include "BrightMask.h" 13 | 14 | #include 15 | 16 | 17 | namespace defSLAM { 18 | 19 | cv::Mat BrightMask::generateMask(const cv::Mat &im) { 20 | cv::Mat imGray = im, mask; 21 | if(imGray.channels()==3){ 22 | cvtColor(imGray,imGray,cv::COLOR_BGR2GRAY); 23 | } 24 | else if(imGray.channels()==4){ 25 | cvtColor(imGray,imGray,cv::COLOR_BGR2GRAY); 26 | } 27 | 28 | cv::threshold(imGray,mask,th_,255,cv::THRESH_BINARY_INV); 29 | 30 | cv::erode(mask, mask, getStructuringElement(cv::MORPH_RECT, cv::Size(21, 21))); 31 | cv::GaussianBlur(mask, mask, cv::Size(11, 11), 5, 5, cv::BORDER_REFLECT_101); 32 | 33 | return mask; 34 | } 35 | 36 | std::string BrightMask::getDescription() { 37 | return std::string("Bright mask with th_ = " + std::to_string(th_)); 38 | } 39 | } 40 | 41 | -------------------------------------------------------------------------------- /Modules/Masker/BrightMask.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef DEFORMABLESLAM_BRIGHTMASK_H 11 | #define DEFORMABLESLAM_BRIGHTMASK_H 12 | 13 | #include 14 | 15 | #include "Filter.h" 16 | 17 | namespace defSLAM { 18 | 19 | class BrightMask : public Filter{ 20 | /* 21 | * This class defines a mask over the brightest pixel on an image. Pixels masked out must 22 | * have a value grater than th_ 23 | */ 24 | public: 25 | BrightMask(int th) : th_(th) {} 26 | 27 | cv::Mat generateMask(const cv::Mat& im); 28 | 29 | std::string getDescription(); 30 | private: 31 | int th_; 32 | }; 33 | } 34 | 35 | 36 | #endif //DEFORMABLESLAM_BRIGHTMASK_H 37 | -------------------------------------------------------------------------------- /Modules/Masker/CnnSegmentation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef DEFORMABLESLAM_CNNSEGMENTATION_H 11 | #define DEFORMABLESLAM_CNNSEGMENTATION_H 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include "Filter.h" 19 | 20 | namespace defSLAM{ 21 | 22 | class CnnSegmentation : public Filter{ 23 | /* 24 | * This class allows to load a convolutional neural network saved as a tor::jit script for binary 25 | * image segmentation. It provides an easy way to infer from OpenCv types, taking care of image pre 26 | * and post processing and data structure translation. 27 | */ 28 | public: 29 | CnnSegmentation() : mean_(0.485 * 255, 0.456 * 255, 0.406 * 255), 30 | std_(0.229 * 255, 0.224 * 255, 0.225 * 255) {}; 31 | 32 | /* 33 | * Loads a cnn model stored at path 34 | */ 35 | void loadModel(std::string path); 36 | 37 | cv::Mat generateMask(const cv::Mat& im){ 38 | return forward(im); 39 | } 40 | 41 | std::string getDescription(); 42 | 43 | private: 44 | /* 45 | * Uses de loaded model to infer a mask from an image im 46 | */ 47 | cv::Mat forward(const cv::Mat &im); 48 | 49 | /* 50 | * Checks that the size of the image im is divisible by 2⁵ and resizes it if necessary 51 | */ 52 | cv::Mat checkImageSize(const cv::Mat &im); 53 | 54 | /* 55 | * Returns a per-channel normalitazed version of the image im 56 | */ 57 | cv::Mat preprocessMat(const cv::Mat &im); 58 | 59 | /* 60 | * Converts an image im stored in OpenCV to a vector of torch::jit::IValue. This 61 | * is the data structure used by any torch cnn 62 | */ 63 | std::vector convertFromMatToTorch(const cv::Mat &im); 64 | 65 | /* 66 | * Converts a torch tensor to an OpenCV mat image 67 | */ 68 | cv::Mat convertFromTorchToMat(at::Tensor &tensor); 69 | 70 | /* 71 | * Applies a pos-processing step of the data infered by the net. Internally applies a binary threshold 72 | * and converts the image to the correct data type 73 | */ 74 | cv::Mat postprocessMat(const cv::Mat &im); 75 | 76 | //Image normalization 77 | cv::Scalar mean_; 78 | cv::Scalar std_; 79 | 80 | //Image resize 81 | const int SIZE_DIV_ = 1 << 5; 82 | 83 | torch::jit::script::Module model_; 84 | }; 85 | } 86 | 87 | #endif //DEFORMABLESLAM_CNNSEGMENTATION_H 88 | -------------------------------------------------------------------------------- /Modules/Masker/Filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef DEFORMABLESLAM_FILTER_H 11 | #define DEFORMABLESLAM_FILTER_H 12 | 13 | 14 | #include 15 | 16 | namespace defSLAM { 17 | class Filter { 18 | /* 19 | * This class defines a generic filter to generate a single mask from an input image 20 | */ 21 | public: 22 | Filter(){}; 23 | 24 | /* 25 | * Virtual method for generating the mask 26 | */ 27 | virtual cv::Mat generateMask(const cv::Mat &im) = 0; 28 | 29 | /* 30 | * Retrieves a short description of the filter 31 | */ 32 | virtual std::string getDescription() = 0; 33 | 34 | private: 35 | std::string filterDescription; 36 | }; 37 | } 38 | 39 | 40 | #endif //DEFORMABLESLAM_FILTER_H 41 | -------------------------------------------------------------------------------- /Modules/Masker/Masker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | #ifndef DEFORMABLESLAM_MASKER_H 11 | #define DEFORMABLESLAM_MASKER_H 12 | 13 | #include 14 | 15 | #include "Filter.h" 16 | 17 | namespace defSLAM { 18 | class Masker { 19 | /* 20 | * This class colects a set of filter to generate a global mask from all of them 21 | */ 22 | public: 23 | Masker(){}; 24 | 25 | /* 26 | * Load filters from a .txt file with the following format: 27 | * 28 | */ 29 | void loadFromTxt(std::string path); 30 | 31 | /* 32 | * Adds a filter to the masker 33 | */ 34 | void addFilter(std::unique_ptr& f); 35 | 36 | /* 37 | * Removes the filter at pos idx 38 | */ 39 | void deleteFilter(size_t idx); 40 | 41 | /* 42 | * Applies all filters stored and generates a global mask 43 | */ 44 | cv::Mat mask(const cv::Mat& im); 45 | 46 | std::string printFilters(); 47 | 48 | private: 49 | std::vector> filters_; 50 | }; 51 | } 52 | 53 | #endif //DEFORMABLESLAM_MASKER_H 54 | -------------------------------------------------------------------------------- /Modules/Settings/CC_MAC.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | // Parameters for the ground truth stereo with matching 22 | // by cross correlation. Set up depending of the dataset 23 | #ifndef CC_MAC 24 | #define CC_MAC 25 | const int TEMPX(15); // Parameters for the template to match 26 | const int TEMPY(15); // Parameters for the template 27 | const int MARGIN(2); // margin in the epipolar line. The images must be rectified 28 | const int SEARCHX(300); // Search window in columns 29 | const double THRESHOLD(0.99); // Threshold of acceptance. 30 | #endif 31 | -------------------------------------------------------------------------------- /Modules/Settings/set_MAC.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | /** 11 | * This file is part of DefSLAM. 12 | * 13 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 14 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 15 | * For more information see 16 | * 17 | * DefSLAM is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * DefSLAM is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with DefSLAM. If not, see . 29 | */ 30 | //Uncomment to run in parallel. TODO: Do it from Cmake 31 | //#define PARALLEL 32 | 33 | //Uncomment to run ORBSLAM adaptation. TODO: Do it from Cmake 34 | //#define ORBSLAM 35 | #define USE_KLT 36 | //#define USE_AKAZE -------------------------------------------------------------------------------- /Modules/Template/Edge.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef EDGE_H 23 | #define EDGE_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace defSLAM 32 | { 33 | 34 | class Template; 35 | class Node; 36 | class Facet; 37 | 38 | class Edge 39 | { 40 | public: 41 | // Constructor. Initialize an edge from two nodes, a facet 42 | // and the template 43 | Edge(Node *, Node *, Facet *, Template *); 44 | 45 | // Destructor 46 | ~Edge(); 47 | 48 | // Get initial length of the edge 49 | double getDist(); 50 | 51 | // Get index of the nodes in the edge 52 | std::pair getPairNodes(); 53 | 54 | // Add facet 55 | void addFacet(Facet *facet); 56 | 57 | // Erase facet 58 | void eraseFacet(Facet *facet); 59 | 60 | // get ptr to the neighbour facets splited by the edge. 61 | std::set getFacets(); 62 | 63 | // get ptr to the nodes in the edge. 64 | std::set getNodes(); 65 | 66 | // Check if the are no connected facets. 67 | bool noFacets(); 68 | 69 | // Delete this edge 70 | void setBadFlag(); 71 | 72 | // returns template that contains this edge. 73 | Template *getTemplate() { return template_; } 74 | 75 | //Comparing functions 76 | // compare this edge against another edge of the same class 77 | bool operator==(const Edge &); 78 | 79 | // See if this edge links the nodes v1 and v2. 80 | bool isEqual(const Node *v1, const Node *v2); 81 | 82 | private: 83 | Template *template_; // Template that contains this edge. 84 | std::set mspNode; // Set of ptr to the nodes linked by this edge. 85 | 86 | Node *mNodes[2]; // 87 | std::set facetsConected; // set of nodes. 88 | double InitialDist; // Distance when the edge was initialized, i.e. shape-at-rest. 89 | }; 90 | 91 | } // namespace defSLAM 92 | 93 | #endif // EDGE_H 94 | -------------------------------------------------------------------------------- /Modules/Template/LaplacianMesh.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef LAPLACIANMESH_H 23 | #define LAPLACIANMESH_H 24 | 25 | #include "KeyFrame.h" 26 | #include "MapPoint.h" 27 | #include "TriangularMesh.h" 28 | #include "set" 29 | 30 | namespace Eigen 31 | { 32 | typedef Matrix Vector1d; 33 | } 34 | 35 | namespace ORB_SLAM2 36 | { 37 | class TriangularMesh; 38 | class MapPoint; 39 | class KeyFrame; 40 | } // namespace ORB_SLAM2 41 | namespace defSLAM 42 | { 43 | using ORB_SLAM2::KeyFrame; 44 | using ORB_SLAM2::Map; 45 | using ORB_SLAM2::MapPoint; 46 | 47 | class LaplacianMesh : public TriangularMesh 48 | { 49 | public: 50 | // Constructor from mesh. 51 | LaplacianMesh(std::vector> &vertex, 52 | std::vector> &index, Map *map); 53 | 54 | // Constructor from surface estimated for kF. 55 | LaplacianMesh(std::set &mspMapPoints, Map *map, KeyFrame *kF); 56 | 57 | // Destructor 58 | virtual ~LaplacianMesh(); 59 | 60 | /******** 61 | * This function extract mean curvatures. It is thought for irregular meshes. 62 | * it use the method proposed in to extract Laplacian coordiantes that represent 63 | * the mean curvature. 64 | * ********/ 65 | void ExtractMeanCurvatures(); 66 | 67 | // Get the initial laplacian coordinate(vector of mean curvature) of the node 68 | const Eigen::Vector3d GetLaplacianCoord(Node *); 69 | 70 | // Get the initial mean curvature (norm of mean curvature)) of the node 71 | Eigen::Vector1d const GetMeanCurvatureInitial(Node *n); 72 | 73 | // Get current mean curvature. It consider that the weights remain constant. 74 | const Eigen::Vector1d GetMeanCurvature(Node *n); 75 | 76 | // Get current mean curvature as Laplacian vector 77 | Eigen::Vector3d const GetMeanCurvatureVector(Node *n); 78 | 79 | std::map, 80 | Eigen::aligned_allocator>> 81 | LaplacianCoords; // Laplacian coordinates. 82 | }; 83 | } // namespace defSLAM 84 | #endif 85 | -------------------------------------------------------------------------------- /Modules/Template/TemplateGenerator.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #include 23 | #include 24 | 25 | #include "LaplacianMesh.h" 26 | 27 | namespace defSLAM 28 | { 29 | using ORB_SLAM2::KeyFrame; 30 | using ORB_SLAM2::Map; 31 | using ORB_SLAM2::MapPoint; 32 | 33 | class LaplacianMesh; 34 | // Load template with a keyframe that contains a surface. Used for shape-from-template. 35 | Template * 36 | TemplateGenerator::ChargeLaplacianMesh(std::vector> &vertex, 37 | std::vector> &index, 38 | Map *map) 39 | { 40 | LaplacianMesh *lapmesh = new LaplacianMesh(vertex, index, map); 41 | 42 | return lapmesh; 43 | 44 | return (static_cast(nullptr)); 45 | } 46 | 47 | // Load template with a keyframe that contains a surface. Used during in DefMap. 48 | Template * 49 | TemplateGenerator::LaplacianMeshCreate(std::set &mspMapPoints, 50 | Map *map, KeyFrame *kF) 51 | { 52 | LaplacianMesh *lapmesh = new LaplacianMesh(mspMapPoints, map, kF); 53 | 54 | return lapmesh; 55 | 56 | return (static_cast(nullptr)); 57 | } 58 | 59 | } // namespace defSLAM 60 | -------------------------------------------------------------------------------- /Modules/Template/TemplateGenerator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef TEMPLATEGENERATOR_H 23 | #define TEMPLATEGENERATOR_H 24 | 25 | #include "KeyFrame.h" 26 | #include "Map.h" 27 | #include "MapPoint.h" 28 | #include "Template.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | class MapPoint; 33 | class KeyFrame; 34 | } // namespace ORB_SLAM2 35 | namespace defSLAM 36 | { 37 | using ORB_SLAM2::KeyFrame; 38 | using ORB_SLAM2::MapPoint; 39 | 40 | class TemplateGenerator 41 | { 42 | public: 43 | //// Load a template given the nodes and facets. 44 | static Template *ChargeLaplacianMesh(std::vector> &, 45 | std::vector> &, 46 | Map *map); 47 | 48 | // Load template with a keyframe that contains a surface. 49 | static Template *LaplacianMeshCreate(std::set &mspMapPoints, 50 | Map *map, KeyFrame *kF); 51 | }; 52 | 53 | } // namespace defSLAM 54 | 55 | #endif // OPTIMIZER_H 56 | -------------------------------------------------------------------------------- /Modules/ToolsPCL/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0 FATAL_ERROR) 2 | project(ToolsPCL) 3 | 4 | find_package(PCL 1.5 REQUIRED COMPONENTS io common features surface) 5 | message(STATUS "Using PCL ${PCL_VERSION}") 6 | include_directories(${PROJECT_SOURCE_DIR} 7 | ${PCL_INCLUDE_DIRS}) 8 | 9 | link_directories(${PCL_LIBRARY_DIRS}) 10 | add_definitions(${PCL_DEFINITIONS}) 11 | 12 | add_library(${PROJECT_NAME} SHARED 13 | PCLNormalEstimator.cc 14 | SmootherMLS.cc 15 | ) 16 | target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) 17 | 18 | 19 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 20 | -------------------------------------------------------------------------------- /Modules/ToolsPCL/PCLNormalEstimator.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | #include "PCLNormalEstimator.h" 22 | 23 | #include 24 | #include 25 | 26 | // Constructor from pcl to estimate the normals. It directly estimate the normals. 27 | // The radious search the neighbours in the point cloud to estimate the normals. 28 | // Points introduced as [X0,Y0,Z0,X1,Y1,Z1,...,Xi,Yi,Zi] in the same vector 29 | PCLNormalEstimator::PCLNormalEstimator(std::vector &Points, double radius) 30 | { 31 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 32 | pcl::PointCloud::Ptr cloud_normals(new pcl::PointCloud); 33 | 34 | for (size_t i = 0; i < Points.size(); i = i + 3) 35 | { 36 | cloud->push_back(pcl::PointXYZ(Points[i], Points[i + 1], Points[i + 2])); 37 | } 38 | 39 | pcl::NormalEstimation ne; 40 | ne.setInputCloud(cloud); 41 | 42 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 43 | ne.setSearchMethod(tree); 44 | ne.setRadiusSearch(radius); 45 | ne.setViewPoint(0, 0, 0); 46 | 47 | ne.compute(*cloud_normals); 48 | 49 | for (uint i(0); i < cloud_normals->size(); i++) 50 | { 51 | normals.push_back(cloud_normals->at(i).normal_x); 52 | normals.push_back(cloud_normals->at(i).normal_y); 53 | normals.push_back(cloud_normals->at(i).normal_z); 54 | } 55 | } 56 | 57 | // Get the result. 58 | // Normals given as [X0,Y0,Z0,X1,Y1,Z1,...,Xi,Yi,Zi] in the same vector 59 | std::vector &&PCLNormalEstimator::getNormals() 60 | { 61 | return std::move(normals); 62 | } 63 | -------------------------------------------------------------------------------- /Modules/ToolsPCL/PCLNormalEstimator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | 26 | class PCLNormalEstimator 27 | { 28 | public: 29 | PCLNormalEstimator() = delete; 30 | 31 | // Points introduced as [X0,Y0,Z0,X1,Y1,Z1,...,Xi,Yi,Zi] in the same vector 32 | PCLNormalEstimator(std::vector &Points, double radious = 0.3); 33 | 34 | // Normals given as [X0,Y0,Z0,X1,Y1,Z1,...,Xi,Yi,Zi] in the same vector 35 | std::vector &&getNormals(); 36 | 37 | private: 38 | std::vector normals; 39 | }; 40 | -------------------------------------------------------------------------------- /Modules/ToolsPCL/SmootherMLS.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | /** 11 | * This file is part of DefSLAM. 12 | * 13 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 14 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 15 | * For more information see 16 | * 17 | * DefSLAM is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * DefSLAM is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with DefSLAM. If not, see . 29 | */ 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | // Constructor. 40 | SmootherMLS::SmootherMLS(int polynomialOrder_, double searchRadius_) 41 | : polynomialOrder_(polynomialOrder_), searchRadius_(searchRadius_){ 42 | 43 | }; 44 | 45 | using namespace pcl; 46 | 47 | std::vector SmootherMLS::outlierRemovalRadius(const SmootherMLS::pointcloud &pcVector) 48 | { 49 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 50 | 51 | for (uint i(0); i < pcVector.size(); i++) 52 | { 53 | PointXYZ pclpoint; 54 | pclpoint.x = pcVector[i][0]; 55 | pclpoint.y = pcVector[i][1]; 56 | pclpoint.z = pcVector[i][2]; 57 | cloud->push_back(pclpoint); 58 | } 59 | 60 | // Set parameters 61 | pcl::RadiusOutlierRemoval outrem; 62 | outrem.setInputCloud(cloud); 63 | outrem.setRadiusSearch(searchRadius_); 64 | outrem.setMinNeighborsInRadius(20); 65 | std::vector indices; 66 | outrem.filter(indices); 67 | return indices; 68 | } -------------------------------------------------------------------------------- /Modules/ToolsPCL/SmootherMLS.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SD-DefSLAM 3 | * Copyright (C) 2020 Juan J. Gómez Rodríguez, Jose Lamarca Peiro, J. Morlana, 4 | * Juan D. Tardós and J.M.M. Montiel, University of Zaragoza 5 | * 6 | * This software is for internal use in the EndoMapper project. 7 | * Not to be re-distributed. 8 | */ 9 | 10 | /** 11 | * This file is part of DefSLAM. 12 | * 13 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 14 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 15 | * For more information see 16 | * 17 | * DefSLAM is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * DefSLAM is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with DefSLAM. If not, see . 29 | */ 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | class SmootherMLS 36 | { 37 | /************************************* 38 | * Class to smooth using Moving Least Squares or outlierRemovalRadius. 39 | * 40 | * args: 41 | * polynomialOrder_: polynomial fitting order 42 | * searchRadius_: radius of point to fit the polinomial. 43 | * methods: 44 | * input : std::vector> point cloud to smooth 45 | * output : std::vector> smoothed point cloud 46 | * 47 | * Author: Jose Lamarca 48 | * Inspired in https://pcl-tutorials.readthedocs.io/en/latest/resampling.html?highlight=resampling#smoothing-and-normal-estimation-based-on-polynomial-reconstruction 49 | * 50 | **********************************/ 51 | public: 52 | typedef std::vector> pointcloud; 53 | 54 | // Initialize the smoother. 55 | SmootherMLS(int polynomialOrder_ = 2, double searchRadius_ = 0.03); 56 | 57 | // Outlier Removal with radius criteria. 58 | std::vector outlierRemovalRadius(const SmootherMLS::pointcloud &); 59 | 60 | private: 61 | int polynomialOrder_; // polynomial order for the MLS 62 | double searchRadius_; // Seach radius. 63 | }; -------------------------------------------------------------------------------- /Modules/Tracking/DefOptimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef DEFOPTIMIZER_H 23 | #define DEFOPTIMIZER_H 24 | 25 | #include "Frame.h" 26 | #include "KeyFrame.h" 27 | #include "Map.h" 28 | 29 | #include "Thirdparty/g2o/g2o/core/sparse_optimizer.h" 30 | #include "Thirdparty/g2o/g2o/types/sim3.h" 31 | // Fwr declaration 32 | namespace ORB_SLAM2 33 | { 34 | class Frame; 35 | class Map; 36 | } // namespace ORB_SLAM2 37 | namespace defSLAM 38 | { 39 | using ORB_SLAM2::Frame; 40 | using ORB_SLAM2::Map; 41 | 42 | namespace Optimizer 43 | { 44 | // Copy of pose Optimization from optimizer in ORBSLAM to increase thresholds 45 | // for deformable scenarios. 46 | int poseOptimization(Frame *pFrame); 47 | 48 | // Shape-from-template with camera motion estimation. This function is used in 49 | // the deformation tracking to estimate the map deformation and camera pose each 50 | // frame. 51 | int DefPoseOptimization(Frame *pFrame, Map *mMap, double RegLap = 5000, 52 | double RegInex = 5000, double RegTemp = 0, 53 | uint NeighboursLayers = 1); 54 | 55 | // Shape-from-template w/o camera motion. This function was used 56 | // for testing shape-from-template with conventional dataset. It works by giving it the matches 57 | // so it is suitable for testing. 58 | int DefPoseOptimization(const std::vector> &matches, 59 | Frame *pframe, Map *mMap, std::vector &outlier, 60 | double RegLap = 5000, double RegInex = 5000, 61 | double RegTemp = 0); 62 | 63 | // Function to Sim(3) alignment of two point clouds. Used in surface registration. 64 | bool OptimizeHorn(std::vector> &pts1, 65 | std::vector> &pts2, g2o::Sim3 &g2oS12, 66 | double chi, double huber); 67 | } // namespace Optimizer 68 | 69 | } // namespace defSLAM 70 | 71 | #endif // OPTIMIZER_H 72 | -------------------------------------------------------------------------------- /Modules/Viewer/DefFrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef DEFFRAMEDRAWER_H 23 | #define DEFFRAMEDRAWER_H 24 | 25 | #include "FrameDrawer.h" 26 | 27 | #include 28 | 29 | namespace ORB_SLAM2 30 | { 31 | class Map; 32 | class Tracking; 33 | class FrameDrawer; 34 | } // namespace ORB_SLAM2 35 | namespace defSLAM 36 | { 37 | using ORB_SLAM2::FrameDrawer; 38 | using ORB_SLAM2::Map; 39 | using ORB_SLAM2::Tracking; 40 | 41 | class DefFrameDrawer : public FrameDrawer 42 | { 43 | public: 44 | // Constructor. 45 | DefFrameDrawer(Map *pMap); 46 | 47 | // Draw a frame to display. It draw the map, with keypoints and the template if selected. 48 | // It use the function DrawFrame from FrameDrawer of ORBSLAM_2. 49 | cv::Mat virtual DrawFrame(); 50 | 51 | // Update frame drawer from tracker. 52 | void virtual Update(Tracking *pTracker); 53 | 54 | protected: 55 | // Draw the projection of the edges of the template. 56 | // It is very ilustrative but quite dizzy // Set true in line 65. 57 | void DrawTemplate(cv::Mat &im); 58 | 59 | cv::Mat mK, mTcw; // calibration matrix and camera pose 60 | }; 61 | 62 | } // namespace defSLAM 63 | 64 | #endif // FRAMEDRAWER_H 65 | -------------------------------------------------------------------------------- /Modules/Viewer/DefViewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef DEFVIEWER_H 23 | #define DEFVIEWER_H 24 | #include "Viewer.h" 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | class Tracking; 34 | class FrameDrawer; 35 | class MapDrawer; 36 | class System; 37 | class Viewer; 38 | } // namespace ORB_SLAM2 39 | 40 | namespace defSLAM 41 | { 42 | using ORB_SLAM2::FrameDrawer; 43 | using ORB_SLAM2::MapDrawer; 44 | using ORB_SLAM2::System; 45 | using ORB_SLAM2::Tracking; 46 | 47 | class DefViewer : public ORB_SLAM2::Viewer 48 | { 49 | public: 50 | // Constructor. 51 | DefViewer(System *pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath); 52 | 53 | // constructor with setting loader. 54 | DefViewer(System *pSystem, FrameDrawer *pFrameDrawer, 55 | MapDrawer *pMapDrawer, Tracking *pTracking, 56 | const SettingsLoader &); 57 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 58 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 59 | void Run() override; 60 | 61 | private: 62 | double RegInex, RegLap, RegTemp; // Regularizers for deformable tracking. 63 | }; 64 | 65 | } // namespace defSLAM 66 | 67 | #endif // VIEWER_H 68 | -------------------------------------------------------------------------------- /Thirdparty/BBS/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(BBS) 3 | 4 | find_package(Eigen3 REQUIRED) 5 | 6 | include_directories( 7 | ${PROJECT_SOURCE_DIR} 8 | ${EIGEN3_INCLUDE_DIR} 9 | ) 10 | 11 | add_library(${PROJECT_NAME} SHARED 12 | bbs.cc 13 | bbs_coloc.cc 14 | ) 15 | 16 | target_link_libraries(${PROJECT_NAME} ${EIGEN3_LIBS}) 17 | -------------------------------------------------------------------------------- /Thirdparty/BBS/bbs.h: -------------------------------------------------------------------------------- 1 | /* bbs.h 2 | * 3 | * General routines for cubic bidimensional b-splines. 4 | * 5 | * History 6 | * 2009/??/??: First version 7 | * 2010/12/15: Translation to CPP 8 | * Adding OpenMP support 9 | * 2018/12/18: Adaptation to c++ for defSLAM (Jose Lamarca) 10 | * 11 | * (c)2009-2010, Florent Brunet. 12 | */ 13 | 14 | /* 15 | * BBS is free software; you can redistribute it and/or modify 16 | * it under the terms of the GNU General Public License as published by 17 | * the Free Software Foundation; either version 2 of the License, or 18 | * (at your option) any later version. 19 | * 20 | * BBS is distributed in the hope that it will be useful, but 21 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 22 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 23 | * for more details. 24 | * 25 | * You should have received a copy of the GNU General Public License along 26 | * with this program; if not, write to the Free Software Foundation, Inc., 27 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 28 | */ 29 | 30 | #ifndef __BBSS_H__ 31 | #define __BBSS_H__ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace BBS 38 | { 39 | /* The B-Spline structure */ 40 | typedef struct _bbs_t 41 | { 42 | double umin; 43 | double umax; 44 | int nptsu; 45 | double vmin; 46 | double vmax; 47 | int nptsv; 48 | int valdim; 49 | } bbs_t; 50 | 51 | void normalize(double xmin, double xmax, int npts, double *x, int nb_x, double *nx); 52 | void normalize_with_inter(double xmin, double xmax, int npts, double *x, int nb_x, double *nx, int *inter); 53 | 54 | void eval_basis(double nx, double *val_basis); 55 | void eval_basis_d(double nx, double *val_basis); 56 | void eval_basis_dd(double nx, double *val_basis); 57 | 58 | void eval(bbs_t *bbs, double *ctrlpts, double *u, double *v, int nval, double *val, int du, int dv); 59 | 60 | int coloc(bbs_t *bbs, double *u, double *v, int nsites, double *pr, size_t *ir, size_t *jc); 61 | //int coloc(bbs_t *bbs, double *u, double *v, int nsites, std::vector *pr, std::vector *ir, std::vector *jc); 62 | int coloc_deriv(bbs_t *bbs, double *u, double *v, int nsites, int du, int dv, double *pr, size_t *ir, size_t *jc); 63 | 64 | void bending_ur(bbs_t *bbs, double err, double *pr, size_t *ir, size_t *jc); 65 | } // namespace BBS 66 | #endif 67 | -------------------------------------------------------------------------------- /Thirdparty/BBS/bbs_MAC.h: -------------------------------------------------------------------------------- 1 | #ifndef _NumberOfControlPointsU 2 | #define _NumberOfControlPointsU 13 3 | #endif 4 | #ifndef _NumberOfControlPointsV 5 | #define _NumberOfControlPointsV 15 6 | #endif 7 | -------------------------------------------------------------------------------- /Thirdparty/BBS/bbs_coloc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DefSLAM. 3 | * 4 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * DefSLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * DefSLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with DefSLAM. If not, see . 20 | */ 21 | 22 | #ifndef BBSCOLOC_H 23 | #define BBSCOLOC_H 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace BBS 30 | { 31 | void colocEigen(bbs_t *bbs, double *u, double *v, int nsites, 32 | Eigen::MatrixXd &colocMatrix); 33 | 34 | int coloc_derivEigen(bbs_t *bbs, double *u, double *v, int nsites, int du, 35 | int dv, Eigen::MatrixXd &colocMatrix); 36 | 37 | int coloc_derivEigen(bbs_t *bbs, double *u, double *v, int nsites, int du, 38 | int dv, Eigen::MatrixXf &colocMatrix); 39 | 40 | void BendingEigen(bbs_t *bbs, double err, 41 | Eigen::SparseMatrix &benMatrix); 42 | 43 | void BendingEigen(bbs_t *bbs, double err, Eigen::MatrixXd &benMatrix); 44 | 45 | void EvalEigen(bbs_t *bbs, const double *ctrlpts, double *u, double *v, 46 | int nval, Eigen::MatrixXd &val, int du, int dv); 47 | } // namespace BBS 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /Thirdparty/BBS/licence.txt: -------------------------------------------------------------------------------- 1 | BBS (Bicubic B-Splines) 2 | ---------------------------------------------------------------------- 3 | 4 | Copyright 2011 Florent Brunet. 5 | Please contact florent@brnt.eu for more information. 6 | All rights reserved. 7 | 8 | Free Licence 9 | ------------ 10 | 11 | BBS is free software; you can redistribute it and/or modify it under the 12 | terms of the GNU General Public License as published by the Free Software 13 | Foundation; either version 3 of the License, or (at your option) any later 14 | version. 15 | 16 | BBS is distributed in the hope that it will be useful, but WITHOUT ANY 17 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 18 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU General Public License along with 21 | BBS; if not, write to the Free Software Foundation, Inc., 51 Franklin 22 | Street, Fifth Floor, Boston, MA 02110-1301, USA 23 | 24 | Other Types of Licencing 25 | ------------------------ 26 | 27 | Source code is available under the GNU General Public License. 28 | In short, if you distribute a software that uses BBS, 29 | you have to distribute it under GPL with the source code. 30 | Another option is to contact us to purchase a commercial license. 31 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | # Check C++11 or C++0x support 8 | include(CheckCXXCompilerFlag) 9 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 10 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 11 | if(COMPILER_SUPPORTS_CXX14) 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 13 | add_definitions(-DCOMPILEDWITHC14) 14 | message(STATUS "Using flag -std=c++14.") 15 | elseif(COMPILER_SUPPORTS_CXX0X) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 17 | add_definitions(-DCOMPILEDWITHC0X) 18 | message(STATUS "Using flag -std=c++0x.") 19 | else() 20 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") 21 | endif() 22 | 23 | set(HDRS_DBOW2 24 | DBoW2/BowVector.h 25 | DBoW2/FORB.h 26 | DBoW2/FClass.h 27 | DBoW2/FeatureVector.h 28 | DBoW2/ScoringObject.h 29 | DBoW2/TemplatedVocabulary.h) 30 | set(SRCS_DBOW2 31 | DBoW2/BowVector.cpp 32 | DBoW2/FORB.cpp 33 | DBoW2/FeatureVector.cpp 34 | DBoW2/ScoringObject.cpp) 35 | 36 | set(HDRS_DUTILS 37 | DUtils/Random.h 38 | DUtils/Timestamp.h) 39 | set(SRCS_DUTILS 40 | DUtils/Random.cpp 41 | DUtils/Timestamp.cpp) 42 | 43 | find_package(OpenCV 4.0.0 QUIET) 44 | if(NOT OpenCV_FOUND) 45 | find_package(OpenCV 2.4.3 QUIET) 46 | if(NOT OpenCV_FOUND) 47 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 48 | endif() 49 | endif() 50 | 51 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 52 | 53 | include_directories(${OpenCV_INCLUDE_DIRS}) 54 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 55 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 56 | 57 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(ORBSLAM) 3 | 4 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 5 | 6 | find_package(OpenCV 4.0.0 REQUIRED) 7 | find_package(Pangolin REQUIRED) 8 | FIND_PACKAGE(Ceres REQUIRED) 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | ${PROJECT_SOURCE_DIR}/ThirdParty/ORBSLAM2 13 | ${PROJECT_SOURCE_DIR}/../../Modules/Common 14 | ${EIGEN3_INCLUDE_DIR} 15 | ${Pangolin_INCLUDE_DIRS} 16 | ) 17 | 18 | FILE(GLOB SRC_ORBSLAM2_FILES 19 | "src/*.cc" 20 | "../../Modules/Common/SettingsLoader.cc") 21 | 22 | add_library(${PROJECT_NAME} SHARED 23 | ${SRC_ORBSLAM2_FILES} 24 | ) 25 | 26 | target_link_libraries(${PROJECT_NAME} 27 | ${OpenCV_LIBS} 28 | ${EIGEN3_LIBS} 29 | ${Pangolin_LIBRARIES} 30 | ${CERES_LIBRARIES} 31 | DBoW2 32 | g2o 33 | ) 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | ORB-SLAM2 is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ORB-SLAM2. 3 | 4 | For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors. 5 | 6 | If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/raulmur/ORB_SLAM2 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/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"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 28 | #include"Thirdparty/g2o/g2o/types/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 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/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 | namespace ORB_SLAM2 34 | { 35 | 36 | class Tracking; 37 | class Viewer; 38 | 39 | class FrameDrawer 40 | { 41 | public: 42 | FrameDrawer(Map *pMap); 43 | 44 | // Update info from the last processed frame. 45 | void virtual Update(Tracking *pTracker); 46 | 47 | // Draw last processed frame. 48 | cv::Mat virtual DrawFrame(); 49 | 50 | void SetError(double s = 0.0); 51 | 52 | protected: 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, N2; 58 | vector mvCurrentKeys, mvCurrentKeysCorr; 59 | vector mvCurrentLocalMap; 60 | vector mvbMap, mvbMapcorr, mvbVO; 61 | bool mbOnlyTracking; 62 | int mnTracked, mnTrackedVO; 63 | vector mvIniKeys; 64 | vector mvIniMatches; 65 | std::vector>> innovation; 66 | std::vector>> innovationOutlier; 67 | 68 | int mState; 69 | double error; 70 | Map *mpMap; 71 | cv::Mat mask_; 72 | std::mutex mMutex, MutexText; 73 | }; 74 | 75 | } // namespace ORB_SLAM2 76 | 77 | #endif // FRAMEDRAWER_H 78 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/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 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * ORB-SLAM2 is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ORB-SLAM2. If not, see . 20 | */ 21 | 22 | #ifndef MAP_H 23 | #define MAP_H 24 | 25 | #include "KeyFrame.h" 26 | #include "MapPoint.h" 27 | 28 | #include 29 | #include 30 | #include 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class MapPoint; 35 | class KeyFrame; 36 | class Frame; 37 | class Map 38 | { 39 | public: 40 | Map(); 41 | 42 | void AddKeyFrame(KeyFrame *pKF); 43 | void addMapPoint(MapPoint *pMP); 44 | void eraseMapPoint(MapPoint *pMP); 45 | void EraseKeyFrame(KeyFrame *pKF); 46 | void SetReferenceMapPoints(const std::vector &vpMPs); 47 | void InformNewBigChange(); 48 | int GetLastBigChangeIdx(); 49 | virtual void ApplyScale(const float s); 50 | 51 | std::vector GetAllKeyFrames(); 52 | std::vector GetAllMapPoints(); 53 | std::vector GetReferenceMapPoints(); 54 | 55 | long unsigned int MapPointsInMap(); 56 | long unsigned KeyFramesInMap(); 57 | 58 | long unsigned int GetMaxKFid(); 59 | // Set to false all the assigned points 60 | virtual void cleanTracked(); 61 | 62 | virtual void clear(); 63 | 64 | public: 65 | std::vector mvpKeyFrameOrigins; 66 | 67 | std::mutex mMutexMapUpdate; 68 | 69 | // This avoid that two points are created simultaneously in separate threads 70 | // (id conflict) 71 | std::mutex mMutexPointCreation; 72 | 73 | protected: 74 | std::set mspMapPoints; 75 | std::set mspKeyFrames; 76 | 77 | std::vector mvpReferenceMapPoints; 78 | 79 | long unsigned int mnMaxKFid; 80 | 81 | // Index related to a big change in the map (loop closure, global BA) 82 | int mnBigChangeIdx; 83 | 84 | std::mutex mMutexMap; 85 | }; 86 | 87 | } // namespace ORB_SLAM2 88 | 89 | #endif // MAP_H 90 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 6 | * For more information see 7 | * 8 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * ORB-SLAM2 is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ORB-SLAM2. If not, see . 20 | */ 21 | 22 | #ifndef MAPDRAWER_H 23 | #define MAPDRAWER_H 24 | 25 | //#include "Tracking.h" 26 | #include "Frame.h" 27 | #include "KeyFrame.h" 28 | #include "Map.h" 29 | #include "MapPoint.h" 30 | #include 31 | #include "SettingsLoader.h" 32 | #include 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | // class Tracking; 38 | class MapDrawer 39 | { 40 | public: 41 | MapDrawer(Map *pMap, const string &strSettingPath); 42 | 43 | MapDrawer(Map *pMap, const defSLAM::SettingsLoader &settingLoader); 44 | 45 | Map *mpMap; // ptr to the map. 46 | 47 | virtual void DrawMapPoints(); 48 | virtual void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 49 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 50 | void SetCurrentCameraPose(const cv::Mat &Tcw); 51 | void SetReferenceKeyFrame(KeyFrame *pKF); 52 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 53 | void DrawError(); 54 | virtual void reset(); 55 | // It draw the GT points 56 | virtual void DrawGTPoints(); 57 | void virtual UpdatePoints(Frame *pFrame); 58 | void virtual UpdatePoints(Frame *pFrame, float s); 59 | void virtual DrawPoints(); 60 | 61 | protected: 62 | float mKeyFrameSize; 63 | float mKeyFrameLineWidth; 64 | float mGraphLineWidth; 65 | float mPointSize; 66 | float mCameraSize; 67 | float mCameraLineWidth; 68 | 69 | vector PointsMap, PointsSeen, PointsMono, PointsGT, PointsLocal, 70 | PointsStereoFrame, PointsAtRest, PointsStereoAtRest; 71 | vector PointsSeenar, PointsMonoar, PointsGTar; 72 | vector PointsRef; 73 | 74 | std::mutex mPoints; 75 | 76 | cv::Mat mCameraPose; 77 | 78 | std::mutex mMutexCamera; 79 | }; 80 | 81 | } // namespace ORB_SLAM2 82 | 83 | #endif // MAPDRAWER_H 84 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/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 | 25 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 6 | * For more information see 7 | * 8 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * ORB-SLAM2 is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ORB-SLAM2. If not, see . 20 | */ 21 | 22 | #ifndef OPTIMIZER_H 23 | #define OPTIMIZER_H 24 | 25 | #include "Frame.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Map.h" 29 | #include "MapPoint.h" 30 | 31 | #include "Thirdparty/g2o/g2o/core/sparse_optimizer.h" 32 | #include "Thirdparty/g2o/g2o/types/sim3.h" 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class LoopClosing; 38 | 39 | namespace Optimizer 40 | { 41 | // public: 42 | void BundleAdjustment(const std::vector &vpKF, 43 | const std::vector &vpMP, int nIterations = 5, 44 | bool *pbStopFlag = NULL, const unsigned long nLoopKF = 0, 45 | const bool bRobust = true); 46 | void GlobalBundleAdjustemnt(Map *pMap, int nIterations = 5, 47 | bool *pbStopFlag = NULL, 48 | const unsigned long nLoopKF = 0, 49 | const bool bRobust = true); 50 | void LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap); 51 | 52 | int poseOptimization(Frame *pFrame); 53 | 54 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 55 | void OptimizeEssentialGraph( 56 | Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, 57 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 58 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 59 | const map> &LoopConnections, 60 | const bool &bFixScale); 61 | 62 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 63 | int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, 64 | std::vector &vpMatches1, g2o::Sim3 &g2oS12, 65 | const float th2, const bool bFixScale); 66 | } // namespace Optimizer 67 | 68 | } // namespace ORB_SLAM2 69 | 70 | #endif // OPTIMIZER_H 71 | -------------------------------------------------------------------------------- /Thirdparty/ORBSLAM_2/include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 6 | * For more information see 7 | * 8 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * ORB-SLAM2 is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ORB-SLAM2. If not, see . 20 | */ 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "System.h" 28 | #include "Tracking.h" 29 | #include 30 | #include "SettingsLoader.h" 31 | 32 | namespace defSLAM 33 | { 34 | class System; 35 | } 36 | namespace ORB_SLAM2 37 | { 38 | 39 | class Tracking; 40 | class FrameDrawer; 41 | class MapDrawer; 42 | using defSLAM::System; 43 | class Viewer 44 | { 45 | public: 46 | Viewer(System *pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, 47 | Tracking *pTracking, const string &strSettingPath); 48 | 49 | Viewer(System *pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, 50 | Tracking *pTracking, const defSLAM::SettingsLoader &settingLoader); 51 | 52 | // Main thread function. Draw points, keyframes, the current camera pose and 53 | // the last processed frame. Drawing is refreshed according to the camera fps. 54 | // We use Pangolin. 55 | virtual void Run(); 56 | 57 | void RequestFinish(); 58 | 59 | void RequestStop(); 60 | 61 | bool isFinished(); 62 | 63 | bool isStopped(); 64 | 65 | void Release(); 66 | 67 | void Updatetimestamp(double t); 68 | 69 | bool go(); 70 | 71 | protected: 72 | bool Stop(); 73 | 74 | System *mpSystem; 75 | FrameDrawer *mpFrameDrawer; 76 | MapDrawer *mpMapDrawer; 77 | 78 | // Drawer of the mesh 79 | Tracking *mpTracker; 80 | 81 | // 1/fps in ms 82 | double mT; 83 | float mImageWidth, mImageHeight; 84 | 85 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 86 | 87 | bool CheckFinish(); 88 | void SetFinish(); 89 | bool mbFinishRequested; 90 | bool mbFinished; 91 | std::mutex mMutexFinish; 92 | 93 | double timestamp; 94 | bool mbStopped; 95 | bool mbStopRequested; 96 | bool mbSaveResults; 97 | bool next; 98 | std::mutex mMutexStop; 99 | std::mutex mMutexTimeStamp; 100 | std::mutex mMutexNext; 101 | }; 102 | } // namespace ORB_SLAM2 103 | 104 | #endif // VIEWER_H 105 | -------------------------------------------------------------------------------- /Thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with DefSLAM. 2 | the file g2otypes.h contains the classes used in the deformation tracking optimization. 3 | Included the data term (EdgeNodesCamera) and regularizers (EdgeMeanCurvature, EdgesStreching and EdgesReference) 4 | likewise, this g2o version was received along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 5 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 6 | All files included in this g2o version are BSD, see license-bsd.txt 7 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /Thirdparty/g2o/lib/libg2o.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/g2o/lib/libg2o.so -------------------------------------------------------------------------------- /Thirdparty/g2o/license-bsd.txt: -------------------------------------------------------------------------------- 1 | g2o - General Graph Optimization 2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, 3 | Kurt Konolige, and Wolfram Burgard 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are 8 | met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/2_disp.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/2_disp.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # project 2 | cmake_minimum_required (VERSION 3.9) 3 | project (libelas) 4 | 5 | # directories 6 | set (LIBELAS_SRC_DIR src) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS ON) 10 | 11 | 12 | # include directory 13 | include_directories(${LIBELAS_SRC_DIR} 14 | ${PROJECT_SOURCE_DIR}/src) 15 | 16 | # use sse3 instruction set 17 | SET(CMAKE_CXX_FLAGS "-msse3") 18 | 19 | # sources 20 | FILE(GLOB LIBELAS_SRC_FILES "src/*.cpp") 21 | 22 | # Tools dataset 23 | set(DATASET_TOOLS 24 | "dataset/stereorectifier.h" 25 | "dataset/dataloader.h" 26 | ) 27 | 28 | add_library(${PROJECT_NAME} SHARED 29 | ${LIBELAS_SRC_FILES} 30 | ) 31 | 32 | include_directories( 33 | ${PROJECT_SOURCE_DIR}/dataset 34 | ) 35 | # make release version 36 | set(CMAKE_BUILD_TYPE RELEASE) 37 | 38 | find_package(OpenCV 4.2.0 REQUIRED) 39 | find_package(Boost 1.60 REQUIRED COMPONENTS filesystem) 40 | 41 | include_directories(${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) 42 | # build demo program 43 | add_executable(elas main.cpp ) 44 | 45 | target_link_libraries(elas ${LIBELAS_SRC_FILES} ${OpenCV_LIBS}) 46 | 47 | add_executable(elasHamlyn mainHamlyn.cpp ${DATASET_TOOLS}) 48 | 49 | target_link_libraries(elasHamlyn ${LIBELAS_SRC_FILES} ${OpenCV_LIBS} Boost::filesystem) 50 | 51 | add_executable(rectifier generateRectImages.cpp ${DATASET_TOOLS}) 52 | 53 | target_link_libraries(rectifier ${LIBELAS_SRC_FILES} ${OpenCV_LIBS} Boost::filesystem) -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/Matlab image rectification/README: -------------------------------------------------------------------------------- 1 | Matlab scripts: 2 | - video2frames: ·Input: video ·Output: frames 3 | - stereoVideo2frames: ·Input: stereo video ·Output: left and right frames 4 | - stereoVideos2monoVideos: ·Input: stereo video ·Output: left and right stereo videos 5 | - rectifyStereo: ·Input: left and right stereo videos, and calibration parameters ·Output: left and right rectified frames 6 | - rectifyAllStereoVideos: ·Input: folder containing all the left and right stereo pair videos to rectify ·Output: left and right rectified frames of all stereo pair videos 7 | - rectifyStereoFunction: script used by rectifyAllStereoVideos. 8 | 9 | All the videos are .avi and all the images .png. 10 | The name of the stereo videos has to be: calibratedXX.avi and the name of the left and right stereo videos calibratedXX_L.avi and calibratedXX_R.avi. 11 | Due to this name constrains, these scripts can process videos with the numeration between 00 and 99. 12 | To see the calibration parameters .txt style, see the file. 13 | 14 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/Matlab image rectification/rectifyAllStereoVideos.m: -------------------------------------------------------------------------------- 1 | % This script automatically rectifies all the stereo videos inside the 2 | % selected folder. Made by David Recasens 3 | % Input: one left stereo video in the folder. Left and right stereo videos with the names: 4 | % calibratedXX_L.avi and calibratedXX_R.avi 5 | % Output: a new folder with two folders saving each one the left and right 6 | % rectified stereo frames for each stereo pair video 7 | 8 | % Watch out! This script is made for less than 100 videos 9 | 10 | clear variables; close all; 11 | 12 | % Manually select the last left stereo video in the folder 13 | pathVideo = uigetdir('Select parent folder with all the stereo videos and parameters inside'); 14 | if isequal(pathVideo, 0) 15 | disp('User selected Cancel'); 16 | return 17 | else 18 | disp(['User selected ', pathVideo]); 19 | end 20 | 21 | % Automatically select all the left stereo videos and rectify all the 22 | % left and right videos in the selected folder 23 | filePattern = fullfile(pathVideo, '*calibrated*.avi'); 24 | files = dir(filePattern); 25 | for k = 1 : length(files) 26 | if ~isempty(strfind(files(k).name, '_L')) % True if the file name has a _L 27 | videoLeftName = files(k).name; 28 | videoLeft = fullfile(pathVideo, videoLeftName); 29 | try 30 | rectifyStereoFunction; 31 | catch 32 | warning('Probably the video ' + convertCharsToStrings(videoLeftName) + ' has not the proper name style: calibratedXX_L, or there are not proper calibration parameters for the video. If not, the problem is in the script rectifyStereoFunction'); 33 | continue 34 | end 35 | end 36 | end 37 | 38 | 39 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/Matlab image rectification/stereoVideo2frames.m: -------------------------------------------------------------------------------- 1 | % This script extracts the frames from the stereo input video. This divides the video in two parts (left and right) of the same size, cropping exactly in the middle. Made by David Recasens 2 | % Watch out! It is specifically made for .avi video 3 | 4 | clear variables; close all; 5 | 6 | [inputVideo,path] = uigetfile('*.avi'); 7 | if isequal(inputVideo,0) 8 | disp('User selected Cancel'); 9 | return 10 | else 11 | disp(['User selected ', fullfile(path,inputVideo)]); 12 | end 13 | 14 | % Get the frames from the input video 15 | obj = VideoReader(fullfile(path,inputVideo)); 16 | vid = read(obj); 17 | frames = obj.NumFrames; 18 | 19 | % Create a new folder in the same folder as the video to save the frames, 20 | % one for the left camera and another for the right one 21 | for ii = 1 : 2 22 | 23 | % In the first iteration, the loop works with the left side of the video. After, with the right side 24 | if ii == 1 25 | side = 'L'; 26 | else 27 | side = 'R'; 28 | end 29 | 30 | newFolderName = strcat(inputVideo(1:end-4), '_', side, '-frames'); % Remove the .avi from the name 31 | newFolderDirectionAndName = fullfile(path, newFolderName); 32 | if ~exist(newFolderDirectionAndName , 'dir') 33 | % Folder does not exist so create it. 34 | mkdir(newFolderDirectionAndName); 35 | else 36 | disp(['Error creating the new folder to save the frames. The folder already exists.']); 37 | end 38 | 39 | % Calculate the section of the image to crop 40 | [rows, columns, numberOfColorChannels] = size(vid(:,:,:,1)); 41 | if ii == 1 % Left side 42 | rect = [0 0 columns/2 rows ]; % [xmin ymin width height] 43 | else % Right side 44 | rect = [columns/2+1 0 columns/2 rows ]; 45 | end 46 | 47 | % Crop the respective side of each frame and write it in the respective new folder 48 | for x = 1 : frames 49 | J = imcrop(vid(:,:,:,x), rect); 50 | imwrite(J, fullfile(newFolderDirectionAndName, strcat('frame-',num2str(x),'.png')) ); 51 | end 52 | 53 | end 54 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/Matlab image rectification/stereoVideo2monoVideos.m: -------------------------------------------------------------------------------- 1 | % This script divides the input stereo video in left and right videos, cropping exactly in the middle. Made by David Recasens 2 | % Watch out! It is specifically made for .avi video. Also, the output 3 | % video frame rate is 30 Hz, despite the input videos can have another 4 | % value. This is not relevant since we are interested in the frames 5 | % individually, no in the temporal consistency. 6 | 7 | clear variables; close all; 8 | 9 | [inputVideo,path] = uigetfile('*.avi'); 10 | if isequal(inputVideo,0) 11 | disp('User selected Cancel'); 12 | return 13 | else 14 | disp(['User selected ', fullfile(path,inputVideo)]); 15 | end 16 | 17 | % Get the frames from the input video 18 | obj = VideoReader(fullfile(path,inputVideo)); 19 | vid = read(obj); 20 | frames = obj.NumFrames; 21 | 22 | % Create a new folder in the same folder as the video to save the frames, 23 | % one for the left camera and another for the right one 24 | for ii = 1 : 2 25 | 26 | % In the first iteration, the loop works with the left side of the video. After, with the right side 27 | if ii == 1 28 | side = 'L'; 29 | else 30 | side = 'R'; 31 | end 32 | 33 | % Calculate the section of the image to crop 34 | [rows, columns, numberOfColorChannels] = size(vid(:,:,:,1)); 35 | if ii == 1 % Left side 36 | rect = [0 0 columns/2 rows ]; % [xmin ymin width height] 37 | else % Right side 38 | rect = [columns/2+1 0 columns/2 rows ]; 39 | end 40 | 41 | % Crop the respective side of each frame and write it in a video .avi 42 | newVideoName = strcat(inputVideo(1:end-4), '_', side, '.avi'); % Remove the .avi from the name 43 | newVideoDirectionAndName = fullfile(path, newVideoName); 44 | v = VideoWriter(newVideoDirectionAndName,'Uncompressed AVI'); 45 | open(v); 46 | for x = 1 : frames 47 | J = imcrop(vid(:,:,:,x), rect); 48 | writeVideo(v, J); 49 | end 50 | close(v) 51 | 52 | end 53 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/Matlab image rectification/video2frames.m: -------------------------------------------------------------------------------- 1 | % This script extracts the frames from the monocular input video. Made by David Recasens 2 | % Watch out! It is specifically made for .avi video 3 | 4 | clear variables; close all; 5 | 6 | [inputVideo,path] = uigetfile('*.avi'); 7 | if isequal(inputVideo,0) 8 | disp('User selected Cancel'); 9 | return 10 | else 11 | disp(['User selected ', fullfile(path,inputVideo)]); 12 | end 13 | 14 | % Create a new folder in the same folder as the video to save the frames 15 | newFolderName = strcat(inputVideo(1:end-4), '-frames'); % Remove the .avi from the name 16 | newFolderDirectionAndName = fullfile(path, newFolderName); 17 | if ~exist(newFolderDirectionAndName , 'dir') 18 | % Folder does not exist so create it. 19 | mkdir(newFolderDirectionAndName); 20 | else 21 | disp(['Error creating the new folder to save the frames. The folder already exists.']); 22 | end 23 | 24 | obj = VideoReader(fullfile(path,inputVideo)); 25 | vid = read(obj); 26 | frames = obj.NumFrames; 27 | for x = 1 : frames 28 | imwrite(vid(:,:,:,x), fullfile(newFolderDirectionAndName, strcat('frame-',num2str(x),'.png')) ); 29 | end -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/_disp.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/_disp.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/elas: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/elas -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/aloe_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/aloe_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/aloe_left_disp.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/aloe_left_disp.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/aloe_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/aloe_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/aloe_right_disp.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/aloe_right_disp.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/cones_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/cones_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/cones_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/cones_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/raindeer_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/raindeer_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/raindeer_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/raindeer_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban1_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban1_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban1_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban1_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban2_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban2_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban2_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban2_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban3_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban3_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban3_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban3_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban4_left.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban4_left.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/img/urban4_right.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Thirdparty/libelas-for-hamlyn/img/urban4_right.pgm -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/matlab/demo.m: -------------------------------------------------------------------------------- 1 | % Copyright 2011. All rights reserved. 2 | % Institute of Measurement and Control Systems 3 | % Karlsruhe Institute of Technology, Germany 4 | 5 | % This file is part of libelas. 6 | % Authors: Andreas Geiger 7 | 8 | % libelas is free software; you can redistribute it and/or modify it under the 9 | % terms of the GNU General Public License as published by the Free Software 10 | % Foundation; either version 3 of the License, or any later version. 11 | 12 | % libelas is distributed in the hope that it will be useful, but WITHOUT ANY 13 | % WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | % PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | % You should have received a copy of the GNU General Public License along with 17 | % libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | % Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | 20 | % libelas demo file for MATLAB 21 | 22 | dbstop error; clear variables; close all; 23 | disp('========================================'); 24 | 25 | % create figure 26 | figure('Position',[50 100 1000 600],'Color','w'); 27 | aI = axes('Position',[0 0.1 0.5 0.9]); axis off; 28 | aD = axes('Position',[0.5 0.1 0.5 0.9]); axis off; 29 | aT = axes('Position',[0.0 0.9 1.0 0.1]); axis off; 30 | 31 | % initially show cones matching result 32 | process(aT,aI,aD,1,'cones'); 33 | 34 | % buttons 35 | hc = uicontrol('Style', 'checkbox', 'String', 'Subsampling', ... 36 | 'Position', [20 60 100 30],'Value',1); 37 | 38 | uicontrol('Style', 'pushbutton', 'String', 'Cones', ... 39 | 'Position', [20 20 80 30], ... 40 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''cones'');'); 41 | 42 | uicontrol('Style', 'pushbutton', 'String', 'Aloe', ... 43 | 'Position', [120 20 80 30], ... 44 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''aloe'');'); 45 | 46 | uicontrol('Style', 'pushbutton', 'String', 'Raindeer', ... 47 | 'Position', [220 20 80 30], ... 48 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''raindeer'');'); 49 | 50 | uicontrol('Style', 'pushbutton', 'String', 'Urban 1', ... 51 | 'Position', [320 20 80 30], ... 52 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''urban1'');'); 53 | 54 | uicontrol('Style', 'pushbutton', 'String', 'Urban 2', ... 55 | 'Position', [420 20 80 30], ... 56 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''urban2'');'); 57 | 58 | uicontrol('Style', 'pushbutton', 'String', 'Urban 3', ... 59 | 'Position', [520 20 80 30], ... 60 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''urban3'');'); 61 | 62 | uicontrol('Style', 'pushbutton', 'String', 'Urban 4', ... 63 | 'Position', [620 20 80 30], ... 64 | 'Callback', 'process(aT,aI,aD,get(hc,''value''),''urban4'');'); 65 | 66 | uicontrol('Style', 'pushbutton', 'String', 'Load images from files ...', ... 67 | 'Position', [720 20 260 30], ... 68 | 'Callback', 'process(aT,aI,aD,get(hc,''value''));'); 69 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/matlab/make.m: -------------------------------------------------------------------------------- 1 | % Copyright 2011. All rights reserved. 2 | % Institute of Measurement and Control Systems 3 | % Karlsruhe Institute of Technology, Germany 4 | 5 | % This file is part of libelas. 6 | % Authors: Andreas Geiger 7 | 8 | % libelas is free software; you can redistribute it and/or modify it under the 9 | % terms of the GNU General Public License as published by the Free Software 10 | % Foundation; either version 3 of the License, or any later version. 11 | 12 | % libelas is distributed in the hope that it will be useful, but WITHOUT ANY 13 | % WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | % PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | % You should have received a copy of the GNU General Public License along with 17 | % libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | % Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | 20 | % MATLAB makefile for compiling the libelas MATLAB wrappers 21 | 22 | disp('Building wrappers ...'); 23 | 24 | % standard version 25 | mex('elasMex.cpp','../src/elas.cpp','../src/descriptor.cpp', '../src/filter.cpp', ... 26 | '../src/triangle.cpp','../src/matrix.cpp','-I../src','CXXFLAGS=\$CXXFLAGS -msse3 -fPIC'); 27 | 28 | % version for profiling individual timings (only for linux) 29 | %mex('elasMex.cpp','../src/elas.cpp','../src/descriptor.cpp', ... 30 | % '../src/triangle.cpp','../src/matrix.cpp','-I../src','CXXFLAGS=\$CXXFLAGS -msse3 -fPIC','-DPROFILE'); 31 | 32 | disp('...done!'); 33 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/runHamlyn.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | local folder_images_left="$local_dir/camera0" 22 | local folder_images_right="$local_dir/camera1" 23 | local pattern_image_right="stereo_im_l_" 24 | local pattern_image_left="stereo_im_r_" 25 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 26 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 27 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 28 | 29 | echo "Searching left images in : " $folder_images_left " with pattern " $pattern_image_right 30 | echo "Searching right images in : " $folder_images_right " with pattern " $pattern_image_left 31 | echo $folder_images_left $folder_images_right $pattern_image_right $pattern_image_left $leftCalibration $rightCalibration $extrinsicCalibration 32 | ./build/elasHamlyn $folder_images_left $folder_images_right $pattern_image_right $pattern_image_left $leftCalibration $rightCalibration $extrinsicCalibration 33 | return 0 34 | ); } 35 | 36 | main $@ 37 | 38 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/runHamlynRectifier.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | local folder_images_left="$local_dir/camera0" 22 | local folder_images_right="$local_dir/camera1" 23 | local pattern_image_right="stereo_im_l_" 24 | local pattern_image_left="stereo_im_r_" 25 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 26 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 27 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 28 | local output_folder="$local_dir/output" 29 | 30 | echo "Searching left images in : " $folder_images_left " with pattern " $pattern_image_right 31 | echo "Searching right images in : " $folder_images_right " with pattern " $pattern_image_left 32 | local args="$folder_images_left $folder_images_right $pattern_image_right $pattern_image_left $leftCalibration $rightCalibration $extrinsicCalibration $output_folder" 33 | echo $args 34 | echo $folder_images_left $folder_images_right $pattern_image_right $pattern_image_left $leftCalibration $rightCalibration $extrinsicCalibration $output_folder 35 | ./build/rectifier $args 36 | return 0 37 | ); } 38 | 39 | main $@ 40 | 41 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/runHamlynRectifierstd.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | local video="$local_dir/stereo.avi" 22 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 23 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 24 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 25 | local output_folder="$local_dir/output" 26 | 27 | echo "Processing Video : " $video 28 | echo $video $leftCalibration $rightCalibration $extrinsicCalibration $output_folder 29 | ./build/rectifier $video $leftCalibration $rightCalibration $extrinsicCalibration $output_folder 30 | return 0 31 | ); } 32 | 33 | main $@ 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/runHamlynRectifierstd2.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | local videoleft="$local_dir/left.avi" 22 | local videoright="$local_dir/right.avi" 23 | 24 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 25 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 26 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 27 | local output_folder="$local_dir/output" 28 | echo "Processing Video : " $videoleft 29 | local args="$videoleft $videoright $leftCalibration $rightCalibration $extrinsicCalibration $output_folder" 30 | echo $args 31 | ./build/rectifier $args 32 | return 0 33 | ); } 34 | 35 | main $@ 36 | 37 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/runHamlynstd.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | #27local video="$local_dir/capture-20110222T172724Z.avi" 22 | local video="$local_dir/stereo.avi" 23 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 24 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 25 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 26 | 27 | echo "Processing Video : " $video 28 | echo $video $leftCalibration $rightCalibration $extrinsicCalibration 29 | ./build/elasHamlyn $video $leftCalibration $rightCalibration $extrinsicCalibration 30 | return 0 31 | ); } 32 | 33 | main $@ 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/runHamlynstd2.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | ################## 4 | # Include section. 5 | ################## 6 | 7 | 8 | ########################################################## 9 | # Process Hamlyn dataset 10 | # Globals: 11 | # None 12 | # Arguments: 13 | # Local directory to download to. 14 | # Returns: 15 | # 0 if thing was deleted, non-zero on error. 16 | ########################################################## 17 | 18 | main() { ( 19 | local local_dir=${1:-null} 20 | 21 | #local videoleft="$local_dir/f7_dynamic_deint_L.avi" 22 | #local videoright="$local_dir/f7_dynamic_deint_R.avi" 23 | local videoleft="$local_dir/left.avi" 24 | local videoright="$local_dir/right.avi" 25 | local leftCalibration="$local_dir/Left_Camera_Calibration_Intrinsic.txt" 26 | local rightCalibration="$local_dir/Right_Camera_Calibration_Intrinsic.txt" 27 | local extrinsicCalibration="$local_dir/camera_extrinsic.txt" 28 | 29 | echo "Processing Videos : " $videoleft 30 | local args2="$videoleft $videoright $leftCalibration $rightCalibration $extrinsicCalibration" 31 | echo $args2 32 | ./build/elasHamlyn $args2 33 | return 0 34 | ); } 35 | 36 | main $@ 37 | 38 | -------------------------------------------------------------------------------- /Thirdparty/libelas-for-hamlyn/src/descriptor.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libelas. 7 | Authors: Andreas Geiger 8 | 9 | libelas is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | libelas is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | // NOTE: This descripter is a sparse approximation to the 50-dimensional 23 | // descriptor described in the paper. It produces similar results, but 24 | // is faster to compute. 25 | 26 | #ifndef __DESCRIPTOR_H__ 27 | #define __DESCRIPTOR_H__ 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | // Define fixed-width datatypes for Visual Studio projects 36 | #ifndef _MSC_VER 37 | #include 38 | #else 39 | typedef __int8 int8_t; 40 | typedef __int16 int16_t; 41 | typedef __int32 int32_t; 42 | typedef __int64 int64_t; 43 | typedef unsigned __int8 uint8_t; 44 | typedef unsigned __int16 uint16_t; 45 | typedef unsigned __int32 uint32_t; 46 | typedef unsigned __int64 uint64_t; 47 | #endif 48 | 49 | class Descriptor { 50 | 51 | public: 52 | 53 | // constructor creates filters 54 | Descriptor(uint8_t* I,int32_t width,int32_t height,int32_t bpl,bool half_resolution); 55 | 56 | // deconstructor releases memory 57 | ~Descriptor(); 58 | 59 | // descriptors accessible from outside 60 | uint8_t* I_desc; 61 | 62 | private: 63 | 64 | // build descriptor I_desc from I_du and I_dv 65 | void createDescriptor(uint8_t* I_du,uint8_t* I_dv,int32_t width,int32_t height,int32_t bpl,bool half_resolution); 66 | 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /Vocabulary/ORBvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/Vocabulary/ORBvoc.txt.tar.gz -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Uncompress vocabulary ..." 2 | 3 | cd Vocabulary 4 | tar -xf ORBvoc.txt.tar.gz 5 | cd .. 6 | 7 | echo "Configuring and building DefSLAM ..." 8 | 9 | mkdir build 10 | cd build 11 | cmake .. -DCMAKE_BUILD_TYPE=Release 12 | make -j 2 13 | -------------------------------------------------------------------------------- /calibration_files/Calib_12+39.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | # Camera calibration and distortion parameters (OpenCV) 7 | Camera.fx: 717.1835 8 | Camera.fy: 717.353 9 | Camera.cx: 729.1716 10 | Camera.cy: 552.4717 11 | 12 | # Distortion 13 | Camera.k1: -0.1609378 14 | Camera.k2: 0.0601716 15 | Camera.k3: -0.07506441 16 | Camera.k4: 0.03384549 17 | 18 | #Camera.k1: 0.0 19 | #Camera.k2: 0.0 20 | #Camera.p1: 0.0 21 | #Camera.p2: 0.0 22 | 23 | Camera.width: 1440 24 | Camera.height: 1080 25 | 26 | #Camera.width: 640 27 | #Camera.height: 480 28 | 29 | # Camera frames per second 30 | Camera.fps: 40.0 31 | 32 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 33 | Camera.RGB: 1 34 | 35 | #-------------------------------------------------------------------------------------------- 36 | # ORB Parameters 37 | #-------------------------------------------------------------------------------------------- 38 | 39 | # ORB Extractor: Number of features per image 40 | ORBextractor.nFeatures: 4000 41 | 42 | # ORB Extractor: Scale factor between levels in the scale pyramid 43 | ORBextractor.scaleFactor: 1.2 44 | 45 | # ORB Extractor: Number of levels in the scale pyramid 46 | ORBextractor.nLevels: 4 47 | 48 | # ORB Extractor: Fast threshold 49 | ORBextractor.iniThFAST: 20 50 | ORBextractor.minThFAST: 7 51 | 52 | #-------------------------------------------------------------------------------------------- 53 | #Regularizers Parameters 54 | #-------------------------------------------------------------------------------------------- 55 | Regularizer.laplacian : 100 56 | Regularizer.Inextensibility : 6000 57 | Regularizer.temporal : 0.05 58 | Regularizer.LocalZone : 1 59 | Regularizer.PropagationZone : 0 60 | 61 | #-------------------------------------------------------------------------------------------- 62 | # Local Mapping Parameters 63 | #-------------------------------------------------------------------------------------------- 64 | LocalMapping.pointsToTemplate: 60 65 | LocalMapping.chiLimit: 0.4 66 | LocalMapping.Bending: 0.7 67 | LocalMapping.Schwarp.Regularizer: 0.05 68 | 69 | #-------------------------------------------------------------------------------------------- 70 | # Viewer Parameters 71 | #-------------------------------------------------------------------------------------------- 72 | Viewer.KeyFrameSize: 0.02507008996987384997 73 | Viewer.KeyFrameLineWidth: 1 74 | Viewer.GraphLineWidth: 0.9 75 | Viewer.PointSize: 2 76 | Viewer.CameraSize: 0.05507008996987384997 77 | Viewer.CameraLineWidth: 3 78 | Viewer.ViewpointX: -1.3 79 | Viewer.ViewpointY: 0 80 | Viewer.ViewpointZ: -0.4 81 | Viewer.ViewpointF: 600 82 | Viewer.SaveResults: 1 83 | #-------------------------------- 84 | Debug.bool: 0 85 | Filters.file: /home/jose/DefKLTSLAM/filtersCNN.txt 86 | 87 | -------------------------------------------------------------------------------- /calibration_files/default.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # Camera parameters 3 | # Camera calibration and distortion parameters (OpenCV) 4 | Camera.fx: 383.1901395 5 | Camera.fy: 383.1901395 6 | Camera.cx: 155.9921722 7 | Camera.cy: 124.3409128 8 | Camera.k1: 0.0 9 | Camera.k2: 0.0 10 | Camera.p1: 0.0 11 | Camera.p2: 0.0 12 | Camera.width: 320 13 | Camera.height: 240 14 | # Camera frames per second 15 | Camera.fps: 20 16 | # stereo baseline times fx 17 | Camera.bf: 2062.637287163453038 18 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 19 | Camera.RGB: 1 20 | # Close/Far threshold. Baseline times. 21 | ThDepth: 350 22 | #-------------------------------------------------------------------------------------------- 23 | # ORB Parameters 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Extractor: Number of features per image 26 | ORBextractor.nFeatures: 2500 27 | 28 | # ORB Extractor: Scale factor between levels in the scale pyramid 29 | ORBextractor.scaleFactor: 1.2 30 | 31 | # ORB Extractor: Number of levels in the scale pyramid 32 | ORBextractor.nLevels: 6 33 | 34 | # ORB Extractor: Fast threshold 35 | ORBextractor.iniThFAST: 15 36 | ORBextractor.minThFAST: 5 37 | #-------------------------------------------------------------------------------------------- 38 | #Regularizers Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | Regularizer.laplacian : 100 41 | Regularizer.Inextensibility : 8000 42 | Regularizer.temporal : 0.05 43 | Regularizer.LocalZone : 1 44 | Regularizer.PropagationZone : 0 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # Local Mapping Parameters 48 | #-------------------------------------------------------------------------------------------- 49 | LocalMapping.pointsToTemplate: 70 50 | LocalMapping.chiLimit: 0.05 51 | LocalMapping.Bending: 0.70 52 | LocalMapping.Schwarp.Regularizer: 0.1 53 | 54 | #-------------------------------------------------------------------------------------------- 55 | # Viewer Parameters 56 | #-------------------------------------------------------------------------------------------- 57 | Viewer.KeyFrameSize: 0.02507008996987384997 58 | Viewer.KeyFrameLineWidth: 1 59 | Viewer.GraphLineWidth: 0.9 60 | Viewer.PointSize: 2 61 | Viewer.CameraSize: 0.05507008996987384997 62 | Viewer.CameraLineWidth: 3 63 | Viewer.ViewpointX: -1.3 64 | Viewer.ViewpointY: 0 65 | Viewer.ViewpointZ: -0.4 66 | Viewer.ViewpointF: 600 67 | Viewer.SaveResults: 1 68 | #-------------------------------- 69 | Debug.bool: 0 70 | Filters.file: /home/jose/DefKLTSLAM/filtersCNN.txt 71 | -------------------------------------------------------------------------------- /filters.txt: -------------------------------------------------------------------------------- 1 | #CNN /media/jose/NuevoVol/models/unet11_binary_20_0.pt 2 | #BrightFilter 250 3 | BorderFilter 10 10 10 10 10 4 | -------------------------------------------------------------------------------- /filtersCNN.txt: -------------------------------------------------------------------------------- 1 | CNN /media/jose/NuevoVol/models/unet11_binary_20_1.pt 2 | #BrightFilter 200 3 | BorderFilter 10 10 10 10 10 4 | -------------------------------------------------------------------------------- /lib/libBBS.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/lib/libBBS.so -------------------------------------------------------------------------------- /lib/libDBoW2.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/lib/libDBoW2.so -------------------------------------------------------------------------------- /lib/libDeformableSLAM.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/lib/libDeformableSLAM.so -------------------------------------------------------------------------------- /lib/libORBSLAM.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/lib/libORBSLAM.so -------------------------------------------------------------------------------- /lib/libToolsPCL.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/SD-DefSLAM/85341ea8d2b1e780a1483212db6eb31d15a34247/lib/libToolsPCL.so -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | 4 | ########################################################## 5 | # Process the Mandala dataset 6 | ############################# 7 | main() { ( 8 | for VARIABLE in Mandala0 Mandala1 Mandala2 Mandala3 Mandala4 MandalaC 9 | do 10 | mkdir $VARIABLE 11 | cd $VARIABLE 12 | local ARG="/home/jose/DefKLTSLAM/Vocabulary/ORBvoc.txt /home/jose/DeformableSLAM/calibration_files/stereo0.yaml /media/jose/NuevoVol/videosDataset/Jose/$VARIABLE/images /media/jose/NuevoVol/videosDataset/Jose/$VARIABLE/images /media/jose/NuevoVol/videosDataset/Jose/$VARIABLE/timestamps/timestamps.txt" 13 | for i in 1 2 3 4 5 14 | do 15 | mkdir $i 16 | cd $i 17 | cp /home/jose/DefKLTSLAM/Apps/DefSLAMGT tmp 18 | ./tmp $ARG 19 | rm tmp 20 | cd .. 21 | done 22 | cd .. 23 | done 24 | )} 25 | 26 | main $@ 27 | --------------------------------------------------------------------------------