├── Apps ├── DefSLAM ├── DefSLAMGT ├── DefSLAMGTCT ├── rmoutput.sh ├── rungt.sh ├── runvalgrind.sh ├── scriptCT ├── simple_CT.cc ├── simple_camera.cc └── stereo_groundtruth.cc ├── CMakeLists.txt ├── Dependencies.md ├── LICENSE ├── Modules ├── Common │ ├── DefKeyFrame.cc │ ├── DefKeyFrame.h │ ├── DefMap.cc │ ├── DefMap.h │ ├── DefMapPoint.cc │ ├── DefMapPoint.h │ ├── System.cc │ └── System.h ├── GroundTruth │ ├── GroundTruthCalculator.cc │ ├── GroundTruthCalculator.h │ ├── GroundTruthFrame.cc │ ├── GroundTruthFrame.h │ ├── GroundTruthKeyFrame.cc │ ├── GroundTruthKeyFrame.h │ ├── MinMedianFilter.cc │ └── MinMedianFilter.h ├── Mapping │ ├── 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 ├── Matching │ ├── DefORBmatcher.cc │ └── DefORBmatcher.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 │ └── lib │ │ ├── Debug │ │ ├── ToolsPCL.lib │ │ └── ToolsPCL.pdb │ │ └── Release │ │ └── ToolsPCL.lib ├── Tracking │ ├── 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 ├── Vocabulary └── ORBvoc.txt.tar.gz ├── build.cmd ├── build.sh ├── calibration_files └── logitechc922.yaml ├── cmake_modules ├── FindEigen3.cmake └── PCLConfig.cmake ├── scripts ├── Twiddle.py ├── hamlyn_exploration_template.yaml ├── plotting.ipynb └── stereo0_template.yaml └── utils └── runDefSlamAutoTune.cmd /Apps/DefSLAM: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Apps/DefSLAM -------------------------------------------------------------------------------- /Apps/DefSLAMGT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Apps/DefSLAMGT -------------------------------------------------------------------------------- /Apps/DefSLAMGTCT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Apps/DefSLAMGTCT -------------------------------------------------------------------------------- /Apps/rmoutput.sh: -------------------------------------------------------------------------------- 1 | rm 2D* 3D* Error* MapPointUsage* Matches* ScaleVariation.txt 2 | -------------------------------------------------------------------------------- /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/DeformableSLAM/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/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 11 | -------------------------------------------------------------------------------- /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 | #include 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | std::cout << argc << std::endl; 7 | if ((argc != 4) && (argc != 3)) 8 | { 9 | cerr << endl 10 | << "Usage: ./DefSLAM ORBvocabulary calibrationFile video" << endl 11 | << " or ./DefSLAM ORBvocabulary calibrationFile " << endl 12 | << endl; 13 | return 1; 14 | } 15 | 16 | cv::VideoCapture cap; 17 | if (argc == 3) 18 | cap.open(0); 19 | else 20 | cap.open(argv[3]); 21 | 22 | if (!cap.isOpened()) // check if we succeeded 23 | return -1; 24 | 25 | string arg = argv[2]; 26 | string arg2 = argv[1]; 27 | 28 | // Create SLAM system. It initializes all system threads and gets ready to 29 | // process frames. 30 | defSLAM::System SLAM(argv[1], argv[2], true); 31 | 32 | unsigned int i(0); 33 | cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); 34 | cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); 35 | 36 | while (cap.isOpened()) 37 | { 38 | cv::Mat imLeft; 39 | cap >> imLeft; 40 | 41 | if (imLeft.empty()) 42 | { 43 | cerr << endl 44 | << "Failed to load image at: " << to_string(i) << endl; 45 | return 1; 46 | } 47 | 48 | SLAM.TrackMonocular(imLeft, i); 49 | i++; 50 | } 51 | 52 | SLAM.Shutdown(); 53 | 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /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/DefKeyFrame.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 DEFKEYFRAME_H 22 | #define DEFKEYFRAME_H 23 | #include "MapPoint.h" 24 | 25 | #include "KeyFrame.h" 26 | #include "Surface.h" 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | class KeyFrame; 35 | class Map; 36 | class Frame; 37 | class MapPoint; 38 | class Node; 39 | class Template; 40 | class Facet; 41 | 42 | } // namespace ORB_SLAM2 43 | 44 | namespace defSLAM 45 | { 46 | using ORB_SLAM2::Frame; 47 | using ORB_SLAM2::KeyFrame; 48 | using ORB_SLAM2::KeyFrameDatabase; 49 | using ORB_SLAM2::Map; 50 | using ORB_SLAM2::MapPoint; 51 | 52 | class DefKeyFrame : public KeyFrame 53 | { 54 | public: 55 | /********************** 56 | * The contructor register the current position of the map points 57 | * to align the surface and recover the scale later. It also estimates 58 | * the retina coordinates. 59 | * 60 | * Arg: 61 | * Frame* F -- ptr to the current frame 62 | * Map* pMap -- ptr to the map 63 | * keyFrameDatabase -- to put the keyframe in the keyframe database. 64 | * 65 | * It also initialize the surface observed from the keyframe. 66 | * *******************/ 67 | DefKeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB); 68 | 69 | //Destructor of the DefKeyFrame 70 | ~DefKeyFrame(); 71 | /********************** 72 | * Normalisation of the points. The keypoints are saved in retina coordinates. 73 | * *******************/ 74 | void NormaliseKeypoints(); 75 | /************ 76 | * This function define a template as an anchor keyframe in which points 77 | * and template are initialized. 78 | *************************/ 79 | void assignTemplate(); 80 | /************ 81 | * This function returns true if this keyframe has a template associated, 82 | * i.e., if it is anchor keyframe. It is used just for visualization. 83 | *************************/ 84 | bool templateAssigned(); 85 | 86 | public: 87 | double umin; 88 | double umax; 89 | double vmin; 90 | double vmax; 91 | int NCu; 92 | int NCv; 93 | int valdim; 94 | std::vector mpKeypointNorm; 95 | std::vector Outliers; 96 | int KeyframesRelated; 97 | Surface *surface; 98 | float accMean; 99 | 100 | private: 101 | std::mutex mutex; 102 | bool haveATemplate_; 103 | }; 104 | 105 | } // namespace defSLAM 106 | 107 | #endif // DEFORMATION MAPPOINT_H 108 | -------------------------------------------------------------------------------- /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 | // return the template 66 | virtual void clear(); 67 | 68 | // deletes the current template 69 | Template *GetTemplate(); 70 | 71 | // clear the entire map 72 | void clearTemplate(); 73 | 74 | // Return the reference keyframe or the keyframe used to 75 | // create the template 76 | KeyFrame *getLastKFTemplate(); 77 | 78 | // Set the reference keyframe or the keyframe used to 79 | // create the template 80 | void setLastKFTemplate(KeyFrame *); 81 | 82 | public: 83 | bool ThereIsATemplate; 84 | 85 | std::mutex MutexUpdating; 86 | 87 | protected: 88 | Template *Mesh; 89 | KeyFrame *lastKfTemplate_; 90 | std::mutex MutexUsingMap; 91 | std::mutex MutexKf; 92 | }; 93 | 94 | } // namespace defSLAM 95 | 96 | #endif // DEFORMABLE MAP_H 97 | -------------------------------------------------------------------------------- /Modules/Common/DefMapPoint.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 DEFMAPPOINT_H 23 | #define DEFMAPPOINT_H 24 | #include "MapPoint.h" 25 | 26 | #include "DefMap.h" 27 | 28 | #include "Facet.h" 29 | #include 30 | #include 31 | #include 32 | 33 | namespace ORB_SLAM2 34 | { 35 | class KeyFrame; 36 | class Map; 37 | class Frame; 38 | class MapPoint; 39 | } // namespace ORB_SLAM2 40 | 41 | namespace defSLAM 42 | { 43 | using ORB_SLAM2::Frame; 44 | using ORB_SLAM2::KeyFrame; 45 | using ORB_SLAM2::Map; 46 | using ORB_SLAM2::MapPoint; 47 | 48 | class DefMapPoint : public MapPoint 49 | { 50 | public: 51 | DefMapPoint() = delete; 52 | 53 | /// Constructor. See constructor Map Point 54 | DefMapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map *pMap); 55 | 56 | /// Constructor. See constructor Map Point 57 | DefMapPoint(const cv::Mat &Pos, Map *pMap, Frame *pFrame, 58 | const int &idxF); 59 | 60 | /// Constructor by default 61 | virtual ~DefMapPoint() = default; 62 | 63 | /// Remove this map point from the template temp. 64 | void RemoveTemplate(); 65 | 66 | /// Get position of the point at shape-at-rest. 67 | cv::Mat GetWorldPosAtRest(); 68 | 69 | /// Delete map point. 70 | void setBadFlag() override; 71 | 72 | /// Assign facet in the template. 73 | void SetFacet(Facet *face); 74 | 75 | /// Get facet in the template. 76 | Facet *getFacet(); 77 | 78 | /// Set barycentric coordinates in the template. 79 | void SetCoordinates(double, double, double); 80 | 81 | /// Once assigned the baricentric recalculate its position 82 | /// and its normals 83 | void Repose(); 84 | 85 | // Set 3D from barycentric 86 | void RecalculatePosition(); 87 | 88 | // Set world position in keyframe kf. 89 | void SetWorldPos(cv::Mat, KeyFrame *); 90 | 91 | public: 92 | // Register of the points 3D pose when the keyframe was initialized 93 | std::unordered_map PosesKeyframes; 94 | 95 | // Barycentrics 96 | double b1, b2, b3; 97 | 98 | protected: 99 | // Template 100 | Facet *facet; 101 | 102 | protected: 103 | std::mutex MutexFacet; 104 | std::mutex MutexNFace; 105 | }; 106 | 107 | } // namespace defSLAM 108 | 109 | #endif // DEFORMATION MAPPOINT_H 110 | -------------------------------------------------------------------------------- /Modules/GroundTruth/GroundTruthCalculator.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 GROUNDTRUTHCALCULATOR_H 23 | #define GROUNDTRUTHCALCULATOR_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include "opencv2/core/core.hpp" 29 | 30 | namespace defSLAM 31 | { 32 | namespace GroundTruthTools 33 | { 34 | /********************** 35 | * The funtion ScaleMinMedian return the scale estimated 36 | * for two point clouds with a min median filter 37 | * 38 | * Author: Javier Morlana 39 | * 40 | * Arg: 41 | * PosMono std::vector> points to estimated by DefSLAM 42 | * PosStereo std::vector> points estimated by stereo 43 | * 44 | * Return: 45 | * Best scale: best estimation of the scale 46 | * 47 | * *******************/ 48 | float scaleMinMedian(std::vector> &PosMono, 49 | std::vector> &PosStereo); 50 | /********************** 51 | * The funtion SaveResults saves a vector in a text file 52 | * 53 | * Author: Javier Morlana 54 | * 55 | * Arg: 56 | * vectorError std::vector save a vector 57 | * name std::string name for the file 58 | * 59 | * Return: 60 | * Best scale: best estimation of the scale 61 | * 62 | * *******************/ 63 | void saveResults(std::vector &vectorError, std::string &name); 64 | /********************** 65 | * The funtion estimateGT estimates the 3D point from 66 | * a stereo pair and a keypoint 67 | * 68 | * Author: Jose Lamarca 69 | * 70 | * Arg: 71 | * std::vector kp2D 72 | * im1 image of keypoint 73 | * im2 image stereo to query 74 | * mbf: Baseline of the stereo pair 75 | * fx,fy,cx,cy: Focals and optical centers; 76 | * 77 | * Return: 78 | * std::vector point in 3D in camera frame; 79 | * 80 | * *******************/ 81 | std::vector estimateGT(const cv::KeyPoint &kp2D, const cv::Mat &im1, const cv::Mat &im2, double mbf, 82 | double cx, double cy, double fx, double fy); 83 | }; // namespace GroundTruthTools 84 | 85 | } // namespace defSLAM 86 | 87 | #endif // GROUNDTRUTHCALCULATOR_H 88 | -------------------------------------------------------------------------------- /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(std::string output_path); 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/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/SchwarpDatabase.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 SCHWARPDATABASE_H 23 | #define SCHWARPDATABASE_H 24 | #pragma once 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "WarpDatabase.h" 31 | 32 | #include "KeyFrame.h" 33 | #include "MapPoint.h" 34 | 35 | #include 36 | #include 37 | 38 | namespace defSLAM 39 | { 40 | using ORB_SLAM2::Frame; 41 | using ORB_SLAM2::KeyFrame; 42 | using ORB_SLAM2::MapPoint; 43 | 44 | class WarpDatabase; 45 | class DiffProp; 46 | 47 | class SchwarpDatabase : public WarpDatabase 48 | { 49 | public: 50 | // Constructor. lambda is set in the file .yalm 51 | SchwarpDatabase(double lambda) : lambda_(lambda) {} 52 | 53 | /*********** 54 | * It insert the keyframe to the database. There we 55 | * search for its covisible anchor keyframes. Once, 56 | * we have them, if they have a certain number of matches 57 | * it estimates a initial warp between the anchor keyframes 58 | * and the new keyframe. With that warp, it search for more 59 | * matches. Once it is done it recalculate the warp and then 60 | * it save the correspondences and its derivatives 61 | * in mapPointsDB_ (WarpDatabase). 62 | * ******/ 63 | void add(KeyFrame *kf) override; 64 | // Delete kf from database 65 | void erase(KeyFrame *kf) override; 66 | 67 | // clear the database 68 | void clear() override; 69 | 70 | protected: 71 | /************************ 72 | * Function that estimates the warps between two keyframes 73 | * given the correspondences vMatchesIndices and 74 | * saves their correspondances and its derivatives. It uses 75 | * the Schwarzian penalization with a lambda weight. 76 | * ******************/ 77 | void calculateSchwarps( 78 | KeyFrame *KF, KeyFrame *KF2, 79 | vector> &vMatchedIndices, 80 | double (&x)[_NumberOfControlPointsU * _NumberOfControlPointsV * 2], 81 | double lambda_); 82 | 83 | protected: 84 | std::set mpkeyframes; // Keyframes registered 85 | double lambda_; // Schwarzian regularization 86 | }; 87 | 88 | } // namespace defSLAM 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /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/Surface.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 SURFACE_H 23 | #define SURFACE_H 24 | 25 | #include "iostream" 26 | 27 | #include "SurfacePoint.h" 28 | #include "Thirdparty/BBS/bbs.h" 29 | #include 30 | #include 31 | 32 | namespace defSLAM 33 | { 34 | 35 | class SurfacePoint; 36 | 37 | class Surface 38 | { 39 | public: 40 | // No constructor by default 41 | Surface() = delete; 42 | 43 | // Number of keypoints in the image 44 | Surface(uint numberOfPoints); 45 | 46 | // Destructor 47 | ~Surface(); 48 | 49 | /*********** 50 | * Save the array with the depth estimated by 51 | ************/ 52 | void saveArray(const std::vector& Array, BBS::bbs_t &bbss); 53 | 54 | /*********** 55 | * Check if enough normals have been estimated 56 | * to define the surface with Shape-from-normals. 57 | ************/ 58 | bool enoughNormals(); 59 | 60 | // Return normals saved 61 | uint getnumberofNormals(); 62 | 63 | /*********** 64 | * Set a normal saved. In DefSLAM ind makes reference to 65 | *the keypoint index 66 | ************/ 67 | void setNormalSurfacePoint(size_t ind, cv::Vec3f &N); 68 | 69 | // Get the normal of the keypoint with index ind. 70 | bool getNormalSurfacePoint(size_t ind, cv::Vec3f &N); 71 | 72 | // Set the 3D position of the keypoint with index ind. 73 | void set3DSurfacePoint(size_t ind, cv::Vec3f &x3D); 74 | 75 | // Get the 3D position of the keypoint with index ind. 76 | void get3DSurfacePoint(size_t ind, cv::Vec3f &x3D); 77 | 78 | // Apply scale to the entire surface. Called after surface 79 | // registration 80 | void applyScale(double s22); 81 | 82 | // Discretize the surface in xs*ys vertex. It is used to 83 | // create a mesh of xs columns and ys rows. 84 | void getVertex(std::vector &NodesSurface, uint, uint); 85 | 86 | private: 87 | std::vector nodesDepth_; // Depth of the nodes register. 88 | 89 | std::vector surfacePoints_; // Points correspoding to the keypoints of the keyframe 90 | 91 | uint numberofNormals_; // Number of normals registered. 92 | 93 | const uint normalsLimit_; // Limit of normals to estimate a surface. 94 | 95 | BBS::bbs_t bbs; // B-BSpline that represent the surface. 96 | }; 97 | } // namespace defSLAM 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /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 | /** 3 | * This file is part of DefSLAM. 4 | * 5 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 6 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 7 | * For more information see 8 | * 9 | * DefSLAM is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DefSLAM is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DefSLAM. If not, see . 21 | */ 22 | 23 | #ifndef DIFFPROP_H 24 | #define DIFFPROP_H 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "KeyFrame.h" 31 | #include 32 | 33 | namespace defSLAM 34 | { 35 | using ORB_SLAM2::KeyFrame; 36 | 37 | class DiffProp 38 | { 39 | /// Class with the differential properties of the warp 40 | /// used in normal estimation. 41 | public: 42 | DiffProp() = default; 43 | ~DiffProp() = default; 44 | 45 | public: 46 | std::pair KFToKF; // Pair of keyframes warped. 47 | // Indexes 48 | size_t idx1; 49 | size_t idx2; 50 | 51 | // Image Points correspondences 52 | float I1u; 53 | float I1v; 54 | float I2u; 55 | float I2v; 56 | 57 | /*********** 58 | * Jacobian Image Points 59 | * J12 = [[J12a,J12c],[J12b,J12d]] 60 | * J21 = [[J21a,J21c],[J21b,J21d]] 61 | ***********/ 62 | float J12a; 63 | float J12b; 64 | float J12c; 65 | float J12d; 66 | float J21a; 67 | float J21b; 68 | float J21c; 69 | float J21d; 70 | 71 | /*********** 72 | * Hessians Image Points 73 | * H12x = [[H12uux,H12uvx],[H12uvx,H12vvx]] 74 | * H12y = [[H12uuy,H12uvy],[H12uvy,H12vvy]] 75 | ***********/ 76 | float H12uux; 77 | float H12uuy; 78 | float H12uvx; 79 | float H12uvy; 80 | float H12vvx; 81 | float H12vvy; 82 | 83 | // first two normal component in both keyframes 84 | // n1 = [k1, k2, 1-k1u-k2u] 85 | float k1; 86 | float k2; 87 | float k12; 88 | float k22; 89 | 90 | // Outlier flag 91 | bool outlier; 92 | }; 93 | 94 | } // namespace defSLAM 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /Modules/Matching/DefORBmatcher.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 DEFORBMATCHER_H 23 | #define DEFORBMATCHER_H 24 | #pragma once 25 | #include 26 | #include 27 | #include 28 | 29 | namespace defSLAM 30 | { 31 | using ORB_SLAM2::Frame; 32 | using ORB_SLAM2::KeyFrame; 33 | using ORB_SLAM2::ORBmatcher; 34 | 35 | class DefORBmatcher : public ORBmatcher 36 | { 37 | public: 38 | // Constructor. 39 | DefORBmatcher(float nnratio = 0.6, bool checkOri = true); 40 | 41 | // Find more matches using a Schwarp. x is the array with the nodes of the warp. 42 | void findbyWarp( 43 | KeyFrame *Kf1, KeyFrame *Kf2, 44 | vector> &vMatchedIndices, 45 | double (&x)[_NumberOfControlPointsU * _NumberOfControlPointsV * 2], 46 | double lambda); 47 | 48 | /********************* 49 | * Initialize a warp and make a refinement with the Schwarzian equation 50 | * to search for map points. HERE IS INITIALIZED THE WARP. IF YOU DO NOT 51 | * CALL findbyWarp IN SchwarpDatabase.cc, YOU MUST INITIALIZE THE WARP 52 | * BEFORE OPTIMIZING THE SCHWARP. (To initialize a warp, use the function 53 | * Warps::Warp::initialize() from Mapping/Schwarp.h) 54 | **********************/ 55 | void CalculateInitialSchwarp( 56 | KeyFrame *Kf1i, KeyFrame *Kf2i, 57 | vector> &vMatchedIndices, 58 | double (&x)[_NumberOfControlPointsU * _NumberOfControlPointsV * 2], 59 | double lambda); 60 | 61 | // It use the result of the schwarzian warp to search for map points. 62 | int searchBySchwarp( 63 | KeyFrame *pKF1, KeyFrame *pKF2, 64 | double (&x)[_NumberOfControlPointsU * _NumberOfControlPointsV * 2], 65 | std::vector> &vMatchedPairs); 66 | 67 | // Copy of function SearchByProjection of ORBmatcher.h. It is used in 68 | // in the tracking. It only get points embedded in the 3D template 69 | int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, 70 | const float th, const bool bMono); 71 | }; 72 | 73 | } // namespace defSLAM 74 | 75 | #endif // ORBMATCHER_H 76 | -------------------------------------------------------------------------------- /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 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 | //Uncomment to run in parallel. TODO: Do it from Cmake 22 | #define PARALLEL 23 | 24 | //Uncomment to run ORBSLAM adaptation. TODO: Do it from Cmake 25 | //#define ORBSLAM -------------------------------------------------------------------------------- /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/Facet.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 FACET_H 23 | #define FACET_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace ORB_SLAM2 38 | { 39 | class KeyFrame; 40 | class Map; 41 | class Frame; 42 | class MapPoint; 43 | } // namespace ORB_SLAM2 44 | namespace defSLAM 45 | { 46 | using ORB_SLAM2::KeyFrame; 47 | using ORB_SLAM2::Map; 48 | using ORB_SLAM2::MapPoint; 49 | 50 | class Template; 51 | class Node; 52 | class Edge; 53 | 54 | class Facet 55 | { 56 | public: 57 | // Constructor. It create a facet from three ptr to nodes for the 58 | // template temp. 59 | Facet(Node *, Node *, Node *, Template *); 60 | 61 | // Destructor 62 | ~Facet(); 63 | 64 | // get nodes in a facet as a set. 65 | std::set getNodes(); 66 | 67 | // get nodes in a facet as an array. 68 | std::array getNodesArray(); 69 | 70 | // Check there are observations in the facet. 71 | bool noObservations(); 72 | 73 | // Add map point observed in the facet. 74 | void addMapPoint(MapPoint *); 75 | 76 | // Erase map point observed in the facet. 77 | void eraseMapPoint(MapPoint *); 78 | 79 | // Get edges of the facet. 80 | std::set getEdges(); 81 | 82 | // Get map points in the facet. 83 | std::set getMapPoints(); 84 | 85 | // Remove facet 86 | void setBadFlag(); 87 | 88 | // Get texture for the image in the keyframe given 89 | void getTextureCoordinates(KeyFrame *KF); 90 | 91 | public: 92 | std::pair> texture; // Image coordinates for the texture in the viewer. 93 | double R, G, B; // In case of no texture, it is drawed in gray. 94 | static std::map> TexturesDataset; // texture 95 | // for the facets. 96 | 97 | private: 98 | Template *template_; // template this face belongs to. 99 | std::set Nodes; // Nodes in this face as set. 100 | std::array mNodes; // Nodes in this face as array 101 | std::set edges_; // Edges in this face as set. 102 | std::set mPoints; // Map points contained in this facet 103 | std::mutex mMutexKeyframe; 104 | }; 105 | 106 | } // namespace defSLAM 107 | 108 | #endif // FACET_H 109 | -------------------------------------------------------------------------------- /Modules/Template/LaplacianMesh.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * This file is part of DefSLAM. 4 | * 5 | * Copyright (C) 2017-2020 Jose Lamarca Peiro , J.M.M. Montiel (University 6 | *of Zaragoza) && Shaifali Parashar, Adrien Bartoli (Université Clermont Auvergne) 7 | * For more information see 8 | * 9 | * DefSLAM is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DefSLAM is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DefSLAM. If not, see . 21 | */ 22 | 23 | #ifndef LAPLACIANMESH_H 24 | #define LAPLACIANMESH_H 25 | 26 | #include "KeyFrame.h" 27 | #include "MapPoint.h" 28 | #include "TriangularMesh.h" 29 | #include "set" 30 | 31 | namespace Eigen 32 | { 33 | typedef Matrix Vector1d; 34 | } 35 | 36 | namespace ORB_SLAM2 37 | { 38 | class TriangularMesh; 39 | class MapPoint; 40 | class KeyFrame; 41 | } // namespace ORB_SLAM2 42 | namespace defSLAM 43 | { 44 | using ORB_SLAM2::KeyFrame; 45 | using ORB_SLAM2::Map; 46 | using ORB_SLAM2::MapPoint; 47 | 48 | class LaplacianMesh : public TriangularMesh 49 | { 50 | public: 51 | // Constructor from mesh. 52 | LaplacianMesh(std::vector> &vertex, 53 | std::vector> &index, Map *map); 54 | 55 | // Constructor from surface estimated for kF. 56 | LaplacianMesh(std::set &mspMapPoints, Map *map, KeyFrame *kF); 57 | 58 | // Destructor 59 | virtual ~LaplacianMesh(); 60 | 61 | /******** 62 | * This function extract mean curvatures. It is thought for irregular meshes. 63 | * it use the method proposed in to extract Laplacian coordiantes that represent 64 | * the mean curvature. 65 | * ********/ 66 | void ExtractMeanCurvatures(); 67 | 68 | // Get the initial laplacian coordinate(vector of mean curvature) of the node 69 | const Eigen::Vector3d GetLaplacianCoord(Node *); 70 | 71 | // Get the initial mean curvature (norm of mean curvature)) of the node 72 | Eigen::Vector1d const GetMeanCurvatureInitial(Node *n); 73 | 74 | // Get current mean curvature. It consider that the weights remain constant. 75 | const Eigen::Vector1d GetMeanCurvature(Node *n); 76 | 77 | // Get current mean curvature as Laplacian vector 78 | Eigen::Vector3d const GetMeanCurvatureVector(Node *n); 79 | 80 | std::map, 81 | Eigen::aligned_allocator>> 82 | LaplacianCoords; // Laplacian coordinates. 83 | }; 84 | } // namespace defSLAM 85 | #endif 86 | -------------------------------------------------------------------------------- /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/Template/TriangularMesh.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 TRIANGULARMESH_H 23 | #define TRIANGULARMESH_H 24 | #include "opencv2/opencv.hpp" 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include "Template.h" 34 | #include 35 | 36 | namespace defSLAM 37 | { 38 | 39 | class Template; 40 | class MapPoint; 41 | class KeyFrame; 42 | class Frame; 43 | 44 | class TriangularMesh : public Template 45 | { 46 | public: 47 | // Constructor by default. 48 | TriangularMesh() = delete; 49 | 50 | // Constructor from mesh given nodes and facets. 51 | TriangularMesh(std::vector> &, 52 | std::vector> &, Map *map); 53 | 54 | // Constructor for a mesh given a surface estimated in the kf. 55 | TriangularMesh(std::set &mspMapPoints, Map *map, KeyFrame *kf); 56 | 57 | // Destructor 58 | virtual ~TriangularMesh() = default; 59 | 60 | public: 61 | // Get the texture of a facet for drawing it in the viewer. 62 | void getFacetTexture(KeyFrame *KF); 63 | 64 | private: 65 | // Create a regular triangular mesh from the keyframe surface estimated. 66 | std::vector> regularTriangulation(int nodesVer, int nodesHor); 67 | 68 | // Create the nodes and the facets to define the triagular mesh. 69 | void setNodes(std::vector> &, 70 | std::vector> &); 71 | 72 | // Embed the points inside the facets by calculating its barycentric coordinates. 73 | void calculateFeaturesCoordinates(); 74 | //Estimate barycentric coordinates. It returns true if the point is inside the face. 75 | bool pointInTriangle(const Eigen::Vector3f &query_point, 76 | const Eigen::Vector3f &triangle_vertex_0, 77 | const Eigen::Vector3f &triangle_vertex_1, 78 | const Eigen::Vector3f &triangle_vertex_2, 79 | Eigen::Vector3f &barycentric); 80 | 81 | void discardFaces(); 82 | }; 83 | } // namespace defSLAM 84 | #endif 85 | -------------------------------------------------------------------------------- /Modules/ToolsPCL/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | set(CMAKE_CXX_STANDARD 14) 3 | project(ToolsPCL VERSION 0.1 LANGUAGES CXX) 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | find_package(PCL 1.5 REQUIRED COMPONENTS io common features surface) 7 | message(STATUS "Using PCL ${PCL_VERSION}") 8 | include_directories(${PROJECT_SOURCE_DIR} ${PCL_INCLUDE_DIRS}) 9 | #link_directories(${PCL_LIBRARY_DIRS}) 10 | #add_definitions(${PCL_DEFINITIONS}) 11 | 12 | add_library(${PROJECT_NAME} 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 radious) 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(radious); 45 | ne.setViewPoint(0, 0, 0); 46 | 47 | ne.compute(*cloud_normals); 48 | 49 | for (unsigned int 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.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 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | class SmootherMLS 27 | { 28 | /************************************* 29 | * Class to smooth using Moving Least Squares or outlierRemovalRadius. 30 | * 31 | * args: 32 | * polynomialOrder_: polynomial fitting order 33 | * searchRadius_: radius of point to fit the polinomial. 34 | * methods: 35 | * smoothPointCloud: function to smooth pointclouds 36 | * input : std::vector> point cloud to smooth 37 | * output : std::vector> smoothed point cloud 38 | * 39 | * Author: Jose Lamarca 40 | * Inspired in https://pcl-tutorials.readthedocs.io/en/latest/resampling.html?highlight=resampling#smoothing-and-normal-estimation-based-on-polynomial-reconstruction 41 | * 42 | **********************************/ 43 | public: 44 | typedef std::vector> pointcloud; 45 | 46 | // Initialize the smoother. 47 | SmootherMLS(int polynomialOrder_ = 2, double searchRadius_ = 0.03); 48 | 49 | // Smooth a point cloud with MLS 50 | std::vector smoothPointCloud(const pointcloud &); 51 | 52 | // Outlier Removal with radius criteria. 53 | std::vector outlierRemovalRadius(const SmootherMLS::pointcloud &); 54 | 55 | private: 56 | int polynomialOrder_; // polynomial order for the MLS 57 | double searchRadius_; // Seach radius. 58 | }; -------------------------------------------------------------------------------- /Modules/ToolsPCL/lib/Debug/ToolsPCL.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Modules/ToolsPCL/lib/Debug/ToolsPCL.lib -------------------------------------------------------------------------------- /Modules/ToolsPCL/lib/Debug/ToolsPCL.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Modules/ToolsPCL/lib/Debug/ToolsPCL.pdb -------------------------------------------------------------------------------- /Modules/ToolsPCL/lib/Release/ToolsPCL.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Modules/ToolsPCL/lib/Release/ToolsPCL.lib -------------------------------------------------------------------------------- /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);// , ofstream& myfile); 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/DefMapDrawer.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 DEFMAPDRAWER_H 23 | #define DEFMAPDRAWER_H 24 | 25 | #include "MapDrawer.h" 26 | #include "MeshDrawer.h" 27 | 28 | #include 29 | 30 | #include "Tracking.h" 31 | #include 32 | #define MAX_NODES 300000 33 | #define MAX_FACETS 100000 34 | 35 | namespace ORB_SLAM2 36 | { 37 | class MapDrawer; 38 | 39 | } 40 | 41 | namespace defSLAM 42 | { 43 | class Node; 44 | class Edge; 45 | class Facet; 46 | class LaplacianMesh; 47 | using ORB_SLAM2::MapDrawer; 48 | 49 | class DefMapDrawer : public MapDrawer 50 | { 51 | public: 52 | // Constructor. 53 | DefMapDrawer(Map *, const string &); 54 | 55 | // Clear template history. 56 | void reset(); 57 | 58 | // Draw the keyframes 59 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) override; 60 | 61 | // Draw the template through meshdrawer. 62 | void DrawTemplate(); 63 | 64 | // Draw the shape-at-rest of the template. 65 | void DrawTemplateAtRest(uint o); 66 | 67 | // Draw the templates through the execution. 68 | void DrawTemplatehist(); 69 | 70 | // Updates from the tracking. 71 | void updateTemplate(); 72 | 73 | // Update template at rest. 74 | void updateTemplateAtRest(); 75 | 76 | // Draw template with texture. 77 | void ShowTexture(bool); 78 | 79 | protected: 80 | MeshDrawer *MeshDrawers; // Mesh drawer of the template. 81 | std::map> MeshDrawershist; //Mesh drawers of the history of the template 82 | 83 | protected: 84 | bool Texture; // Flag for texture 85 | 86 | // Mutex 87 | std::mutex mTexture; 88 | std::mutex mTemplate; 89 | std::mutex mTemplatehist; 90 | std::mutex mPointsar; 91 | }; 92 | 93 | } // namespace defSLAM 94 | 95 | #endif // DEFORMATION MAPDRAWER_H 96 | -------------------------------------------------------------------------------- /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 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 54 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 55 | void Run() override; 56 | 57 | private: 58 | double RegInex, RegLap, RegTemp; // Regularizers for deformable tracking. 59 | string output_path; 60 | }; 61 | 62 | } // namespace defSLAM 63 | 64 | #endif // VIEWER_H 65 | -------------------------------------------------------------------------------- /Modules/Viewer/MeshDrawer.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 MESHDRAWER_H 23 | #define MESHDRAWER_H 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace defSLAM 33 | { 34 | class MeshDrawer 35 | { 36 | /************* 37 | * Class to draw meshes 38 | ************/ 39 | public: 40 | // Constructor 41 | MeshDrawer() = default; 42 | 43 | // Destructor 44 | ~MeshDrawer() = default; 45 | 46 | /****************** 47 | * In case the mesh was created from a keyframe. The 48 | * image can be used as texture. 49 | * If there is no texture fit it with a random image 50 | * (3 Channels). 51 | *************/ 52 | void addTextureImage(cv::Mat im); 53 | 54 | /****************** 55 | * It registers the edges of the mesh to draw them. 56 | *************/ 57 | void addEdge(std::vector edge); 58 | 59 | /****************** 60 | * Add a node to draw and its projection. The position as 61 | * a vector of doubles with 3 components {x,y,z} and the 62 | * projection as a vector of 2 components. The role defines 63 | * the color of the node. 64 | *************/ 65 | void addNode(std::vector pos, std::vector proj, int role); 66 | 67 | /****************** 68 | * Add a facet as a vector of 3 integers 69 | *************/ 70 | void addFacet(std::vector fac); 71 | 72 | /**************** 73 | * Function to draw the mesh. 74 | * alpha: level of transparency 75 | * drawedges: flag to draw the edges. 76 | *************/ 77 | void drawMesh(double alpha = 1, bool drawedges = true); 78 | 79 | private: 80 | std::vector Facets; // One vector [V1_1 V1_2 V1_3 ..Vn_1 Vn_2 Vn_3 ... VN_1 VN_2 VN_3] Facets 81 | std::vector Edges; // One vector [V1_1 V1_2 ... Vn_1 Vn_2 ... VN_1 VN_2] Edges 82 | std::vector Nodes; // One vector [V1 V2 ... Vn ... VN] Vertex position 83 | std::vector NodesRole; // One vector [R1 R2 ... Rn ... RN] Nodes role 84 | 85 | cv::Mat Texture; 86 | std::vector NodesProjection; // One vector with the 2d projection of the vertex [v1 v2 ... vn ... vN] 87 | 88 | private: 89 | std::mutex mtemp; 90 | }; 91 | 92 | } // namespace defSLAM 93 | #endif // MESHS_H 94 | -------------------------------------------------------------------------------- /Thirdparty/BBS/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | set(CMAKE_CXX_STANDARD 14) 3 | project(BBS VERSION 0.1 LANGUAGES CXX) 4 | find_package(Eigen3 CONFIG REQUIRED) 5 | IF (WIN32) 6 | set(LIB_TYPE) 7 | ELSE() 8 | set(LIB_TYPE SHARED) 9 | ENDIF() 10 | 11 | 12 | 13 | add_library(${PROJECT_NAME} ${LIB_TYPE} 14 | bbs.cc 15 | bbs_coloc.cc 16 | ) 17 | target_include_directories( ${PROJECT_NAME} 18 | PUBLIC 19 | ${PROJECT_SOURCE_DIR} 20 | 21 | ) 22 | 23 | target_link_libraries(${PROJECT_NAME} ${EIGEN3_LIBS}) 24 | -------------------------------------------------------------------------------- /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 | #include 37 | 38 | namespace BBS 39 | { 40 | /* The B-Spline structure */ 41 | typedef struct _bbs_t 42 | { 43 | double umin; 44 | double umax; 45 | int nptsu; 46 | double vmin; 47 | double vmax; 48 | int nptsv; 49 | int valdim; 50 | } bbs_t; 51 | 52 | void normalize(double xmin, double xmax, int npts, double *x, int nb_x, double *nx); 53 | void normalize_with_inter(double xmin, double xmax, int npts, double* x, int nb_x, std::vector& nx, std::vector& inter); 54 | void normalize_with_inter(double xmin, double xmax, int npts, std::vector& x, int nb_x, std::vector& nx, std::vector& inter); 55 | 56 | void eval_basis(double nx, double *val_basis); 57 | void eval_basis_d(double nx, double *val_basis); 58 | void eval_basis_dd(double nx, double *val_basis); 59 | 60 | void eval(bbs_t *bbs, double *ctrlpts, double *u, double *v, int nval, double *val, int du, int dv); 61 | 62 | int coloc(bbs_t *bbs, double *u, double *v, int nsites, double *pr, size_t *ir, size_t *jc); 63 | //int coloc(bbs_t *bbs, double *u, double *v, int nsites, std::vector *pr, std::vector *ir, std::vector *jc); 64 | int coloc_deriv(bbs_t *bbs, double *u, double *v, int nsites, int du, int dv, double *pr, size_t *ir, size_t *jc); 65 | 66 | void bending_ur(bbs_t *bbs, double err, double *pr, size_t *ir, size_t *jc); 67 | } // namespace BBS 68 | #endif 69 | -------------------------------------------------------------------------------- /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, std::vector& u, std::vector& v, int nsites, 32 | Eigen::MatrixXd &colocMatrix); 33 | 34 | int coloc_derivEigen(bbs_t *bbs, std::vector& u, std::vector& v, int nsites, int du, 35 | int dv, Eigen::MatrixXd &colocMatrix); 36 | 37 | // int coloc_derivEigen(bbs_t *bbs, std::vector& u, std::vector& 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 std::vector& ctrlpts, std::vector& u, std::vector& 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 3.15) 2 | set(CMAKE_CXX_STANDARD 14) 3 | project(DBoW2 LANGUAGES CXX) 4 | 5 | 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV 4.0.0 QUIET) 28 | if(NOT OpenCV_FOUND) 29 | find_package(OpenCV 2.4.3 QUIET) 30 | if(NOT OpenCV_FOUND) 31 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 32 | endif() 33 | endif() 34 | 35 | #set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 36 | 37 | #include_directories(${OpenCV_INCLUDE_DIRS}) 38 | add_library(DBoW2 ${HDRS_DBOW2} ${SRCS_DBOW2} ${HDRS_DUTILS} ${SRCS_DUTILS}) 39 | target_link_libraries(DBoW2 PUBLIC ${OpenCV_LIBS}) 40 | 41 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 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 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /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/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /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 3.15) 2 | set(CMAKE_CXX_STANDARD 14) 3 | project(ORBSLAM VERSION 0.1 LANGUAGES CXX) 4 | 5 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 6 | 7 | find_package(OpenCV 4.0.0 REQUIRED) 8 | find_package(Pangolin REQUIRED) 9 | FIND_PACKAGE(Ceres REQUIRED) 10 | find_package(Eigen3 REQUIRED) 11 | 12 | IF (WIN32) 13 | add_compile_options("/openmp:experimental") 14 | set(LIB_TYPE) 15 | ELSE() 16 | set(LIB_TYPE SHARED) 17 | ENDIF() 18 | 19 | include_directories( 20 | ${PROJECT_SOURCE_DIR}/ThirdParty/ORBSLAM2 21 | ${EIGEN3_INCLUDE_DIR} 22 | ${Pangolin_INCLUDE_DIRS} 23 | ) 24 | 25 | FILE(GLOB SRC_ORBSLAM2_FILES 26 | "src/*.cc") 27 | 28 | add_library(${PROJECT_NAME} ${LIB_TYPE} 29 | ${SRC_ORBSLAM2_FILES} 30 | ) 31 | IF (WIN32) 32 | target_compile_options(${PROJECT_NAME} PRIVATE /bigobj) 33 | endif() 34 | target_link_libraries(${PROJECT_NAME} 35 | ${OpenCV_LIBS} 36 | ${EIGEN3_LIBS} 37 | ${Pangolin_LIBRARIES} 38 | ${CERES_LIBRARIES} 39 | DBoW2 40 | g2o 41 | ) 42 | 43 | -------------------------------------------------------------------------------- /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 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | class Viewer; 39 | 40 | class FrameDrawer 41 | { 42 | public: 43 | FrameDrawer(Map* pMap); 44 | 45 | // Update info from the last processed frame. 46 | void virtual Update(Tracking *pTracker); 47 | 48 | // Draw last processed frame. 49 | cv::Mat virtual DrawFrame(); 50 | 51 | void SetError(double s = 0.0); 52 | protected: 53 | 54 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 55 | 56 | // Info of the frame to be drawn 57 | cv::Mat mIm; 58 | int N,N2; 59 | vector mvCurrentKeys,mvCurrentKeysCorr; 60 | vector mvCurrentLocalMap; 61 | vector mvbMap,mvbMapcorr, mvbVO; 62 | bool mbOnlyTracking; 63 | int mnTracked, mnTrackedVO; 64 | vector mvIniKeys; 65 | vector mvIniMatches; 66 | int mState; 67 | double error; 68 | Map* mpMap; 69 | 70 | std::mutex mMutex,MutexText; 71 | }; 72 | 73 | } //namespace ORB_SLAM 74 | 75 | #endif // FRAMEDRAWER_H 76 | -------------------------------------------------------------------------------- /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/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOCALMAPPING_H 22 | #define LOCALMAPPING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "Map.h" 26 | #include "LoopClosing.h" 27 | #include "Tracking.h" 28 | #include "KeyFrameDatabase.h" 29 | #include "DefMapPoint.h" 30 | #include "MapDrawer.h" 31 | #include 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | class LoopClosing; 39 | class Map; 40 | class MapDrawer; 41 | 42 | class LocalMapping 43 | { 44 | public: 45 | LocalMapping(Map* pMap, MapDrawer* mpDrawer,const float bMonocular); 46 | 47 | void SetLoopCloser(LoopClosing* pLoopCloser); 48 | 49 | void SetTracker(Tracking* pTracker); 50 | 51 | // Main function 52 | virtual void Run(); 53 | 54 | void InsertKeyFrame(KeyFrame* pKF); 55 | 56 | // Thread Synch 57 | void RequestStop(); 58 | void RequestReset(); 59 | bool Stop(); 60 | void Release(); 61 | bool isStopped(); 62 | bool stopRequested(); 63 | bool AcceptKeyFrames(); 64 | void SetAcceptKeyFrames(bool flag); 65 | bool SetNotStop(bool flag); 66 | void InterruptBA(); 67 | void RequestFinish(); 68 | bool isFinished(); 69 | 70 | int KeyframesInQueue(){ 71 | unique_lock lock(mMutexNewKFs); 72 | return mlNewKeyFrames.size(); 73 | } 74 | 75 | protected: 76 | 77 | bool CheckNewKeyFrames(); 78 | virtual void ProcessNewKeyFrame(); 79 | virtual void CreateNewMapPoints(); 80 | 81 | virtual void MapPointCulling(); 82 | virtual void SearchInNeighbors(); 83 | 84 | virtual void KeyFrameCulling(); 85 | 86 | cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); 87 | 88 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 89 | 90 | bool mbMonocular; 91 | 92 | virtual void ResetIfRequested(); 93 | bool mbResetRequested; 94 | std::mutex mMutexReset; 95 | 96 | bool CheckFinish(); 97 | void SetFinish(); 98 | bool mbFinishRequested; 99 | bool mbFinished; 100 | std::mutex mMutexFinish; 101 | 102 | Map* mpMap; 103 | 104 | LoopClosing* mpLoopCloser; 105 | Tracking* mpTracker; 106 | MapDrawer* mpMapDrawer; 107 | 108 | std::list mlNewKeyFrames; 109 | 110 | KeyFrame* mpCurrentKeyFrame; 111 | 112 | std::list mlpRecentAddedMapPoints; 113 | 114 | std::mutex mMutexNewKFs; 115 | 116 | bool mbAbortBA; 117 | 118 | bool mbStopped; 119 | bool mbStopRequested; 120 | bool mbNotStop; 121 | std::mutex mMutexStop; 122 | 123 | bool mbAcceptKeyFrames; 124 | std::mutex mMutexAccept; 125 | }; 126 | 127 | } //namespace ORB_SLAM 128 | 129 | #endif // LOCALMAPPING_H 130 | -------------------------------------------------------------------------------- /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 | 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 | Map *mpMap; // ptr to the map. 44 | 45 | virtual void DrawMapPoints(); 46 | virtual void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 47 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 48 | void SetCurrentCameraPose(const cv::Mat &Tcw); 49 | void SetReferenceKeyFrame(KeyFrame *pKF); 50 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 51 | void DrawError(); 52 | virtual void reset(); 53 | // It draw the GT points 54 | virtual void DrawGTPoints(); 55 | void virtual UpdatePoints(Frame *pFrame); 56 | void virtual UpdatePoints(Frame *pFrame, float s); 57 | void virtual DrawPoints(); 58 | 59 | protected: 60 | float mKeyFrameSize; 61 | float mKeyFrameLineWidth; 62 | float mGraphLineWidth; 63 | float mPointSize; 64 | float mCameraSize; 65 | float mCameraLineWidth; 66 | 67 | vector PointsMap, PointsSeen, PointsMono, PointsGT, PointsLocal, 68 | PointsStereoFrame, PointsAtRest, PointsStereoAtRest; 69 | vector PointsSeenar, PointsMonoar, PointsGTar; 70 | vector PointsRef; 71 | 72 | std::mutex mPoints; 73 | 74 | cv::Mat mCameraPose; 75 | 76 | std::mutex mMutexCamera; 77 | }; 78 | 79 | } // namespace ORB_SLAM2 80 | 81 | #endif // MAPDRAWER_H 82 | -------------------------------------------------------------------------------- /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 | 31 | namespace defSLAM 32 | { 33 | class System; 34 | } 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class Tracking; 39 | class FrameDrawer; 40 | class MapDrawer; 41 | using defSLAM::System; 42 | class Viewer 43 | { 44 | public: 45 | Viewer(System *pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, 46 | Tracking *pTracking, const string &strSettingPath); 47 | 48 | // Main thread function. Draw points, keyframes, the current camera pose and 49 | // the last processed frame. Drawing is refreshed according to the camera fps. 50 | // We use Pangolin. 51 | virtual void Run(); 52 | 53 | void RequestFinish(); 54 | 55 | void RequestStop(); 56 | 57 | bool isFinished(); 58 | 59 | bool isStopped(); 60 | 61 | void Release(); 62 | 63 | void Updatetimestamp(double t); 64 | 65 | bool go(); 66 | 67 | protected: 68 | bool Stop(); 69 | 70 | System *mpSystem; 71 | FrameDrawer *mpFrameDrawer; 72 | MapDrawer *mpMapDrawer; 73 | 74 | // Drawer of the mesh 75 | Tracking *mpTracker; 76 | 77 | // 1/fps in ms 78 | double mT; 79 | float mImageWidth, mImageHeight; 80 | 81 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 82 | 83 | bool CheckFinish(); 84 | void SetFinish(); 85 | bool mbFinishRequested; 86 | bool mbFinished; 87 | std::mutex mMutexFinish; 88 | 89 | double timestamp; 90 | bool mbStopped; 91 | bool mbStopRequested; 92 | bool mbSaveResults; 93 | bool next; 94 | std::mutex mMutexStop; 95 | std::mutex mMutexTimeStamp; 96 | std::mutex mMutexNext; 97 | }; 98 | } // namespace ORB_SLAM2 99 | 100 | #endif // VIEWER_H 101 | -------------------------------------------------------------------------------- /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/batch_stats.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 "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /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/jacobian_workspace.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 JACOBIAN_WORKSPACE_H 28 | #define JACOBIAN_WORKSPACE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "hyper_graph.h" 37 | 38 | namespace g2o { 39 | 40 | struct OptimizableGraph; 41 | 42 | /** 43 | * \brief provide memory workspace for computing the Jacobians 44 | * 45 | * The workspace is used by an OptimizableGraph to provide temporary memory 46 | * for computing the Jacobian of the error functions. 47 | * Before calling linearizeOplus on an edge, the workspace needs to be allocated 48 | * by calling allocate(). 49 | */ 50 | class JacobianWorkspace 51 | { 52 | public: 53 | typedef std::vector > WorkspaceVector; 54 | 55 | public: 56 | JacobianWorkspace(); 57 | ~JacobianWorkspace(); 58 | 59 | /** 60 | * allocate the workspace 61 | */ 62 | bool allocate(); 63 | 64 | /** 65 | * update the maximum required workspace needed by taking into account this edge 66 | */ 67 | void updateSize(const HyperGraph::Edge* e); 68 | 69 | /** 70 | * update the required workspace by looking at a full graph 71 | */ 72 | void updateSize(const OptimizableGraph& graph); 73 | 74 | /** 75 | * manually update with the given parameters 76 | */ 77 | void updateSize(int numVertices, int dimension); 78 | 79 | /** 80 | * return the workspace for a vertex in an edge 81 | */ 82 | double* workspaceForVertex(int vertexIndex) 83 | { 84 | assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); 85 | return _workspace[vertexIndex].data(); 86 | } 87 | 88 | protected: 89 | WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians 90 | int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge 91 | int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_operations.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_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 49 | { 50 | y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); 51 | } 52 | 53 | template 54 | inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 55 | { 56 | y.segment(yoff) += A.transpose() * x.segment(xoff); 57 | } 58 | 59 | template 60 | inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 61 | { 62 | y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); 63 | } 64 | 65 | template<> 66 | inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /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/openmp_mutex.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_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "../../config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /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/robust_kernel.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_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::tr1::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /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/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/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 | -------------------------------------------------------------------------------- /Vocabulary/ORBvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UZ-SLAMLab/DefSLAM/83c6a164058842c8c4bda5805acf3fdad149adcb/Vocabulary/ORBvoc.txt.tar.gz -------------------------------------------------------------------------------- /build.cmd: -------------------------------------------------------------------------------- 1 | IF NOT DEFINED VCPKG_ROOT SET VCPKG_ROOT="C:\git\vcpkg" 2 | IF NOT DEFINED VCPKG_DEFAULT_TRIPLET SET VCPKG_DEFAULT_TRIPLET=x64-windows 3 | @del /S/Q build_win 4 | @rmdir /S/Q build_win 5 | @mkdir build_win 6 | @cd build_win 7 | @call cmake -G"Visual Studio 16 2019" -DCMAKE_TOOLCHAIN_FILE="%VCPKG_ROOT%/scripts/buildsystems/vcpkg.cmake" -DVCPKG_TARGET_TRIPLET=%VCPKG_DEFAULT_TRIPLET% .. 8 | @cmake --build . --config Release 9 | @cd .. -------------------------------------------------------------------------------- /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/logitechc922.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | #Camera.fx: 625.52959480710581 9 | #Camera.fy: 624.00833471703072 10 | #Camera.cx: 311.32774383562617 11 | #Camera.cy: 234.87042785219381 12 | Camera.fx: 312.7647974 13 | Camera.fy: 312.0041674 14 | Camera.cx: 155.66387 15 | Camera.cy: 117.4352139 16 | Camera.k1: 1.5107855961135716e-01 17 | Camera.k2: -1.1107367525758085e+00 18 | Camera.p1: 5.3093815799022609e-03 19 | Camera.p2: -9.2058121327810304e-04 20 | Camera.k3: 4.0355298599997473e+00 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 26 | Camera.RGB: 1 27 | 28 | #-------------------------------------------------------------------------------------------- 29 | # ORB Parameters 30 | #-------------------------------------------------------------------------------------------- 31 | 32 | # ORB Extractor: Number of features per image 33 | ORBextractor.nFeatures: 1200 34 | 35 | # ORB Extractor: Scale factor between levels in the scale pyramid 36 | ORBextractor.scaleFactor: 1.2 37 | 38 | # ORB Extractor: Number of levels in the scale pyramid 39 | ORBextractor.nLevels: 8 40 | 41 | # ORB Extractor: Fast threshold 42 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 43 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 44 | # You can lower these values if your images have low contrast 45 | ORBextractor.iniThFAST: 15 46 | ORBextractor.minThFAST: 7 47 | 48 | #-------------------------------------------------------------------------------------------- 49 | # Regularizer Parameters 50 | #-------------------------------------------------------------------------------------------- 51 | 52 | # Laplacian regularizer: It penalizes changes in curvature 53 | Regularizer.laplacian: 25 54 | 55 | # Inextensibility regularizer: It penalizes the streching 56 | Regularizer.Inextensibility: 240 57 | 58 | # Temporal regularizer: Smoothes the changes in the pose of the mesh frame to frame 59 | Regularizer.temporal: 0.21 60 | 61 | #Number of neighbours layers used in the local optimization 62 | Regularizer.LocalZone: 1 63 | 64 | #Number of neighbours layers affected by propagation 65 | Regularizer.PropagationZone: 0 66 | 67 | # Reliability: It uses just the more found keypoints (Reliability = found / "viewed) 68 | Regularizer.Reliability: 0 69 | #-------------------------------------------------------------------------------------------- 70 | # Viewer Parameters 71 | #-------------------------------------------------------------------------------------------- 72 | Viewer.KeyFrameSize: 0.05 73 | Viewer.KeyFrameLineWidth: 1 74 | Viewer.GraphLineWidth: 0.9 75 | Viewer.PointSize: 2 76 | Viewer.CameraSize: 0.08 77 | Viewer.CameraLineWidth: 3 78 | Viewer.ViewpointX: 0 79 | Viewer.ViewpointY: -0.7 80 | Viewer.ViewpointZ: -1.8 81 | Viewer.ViewpointF: 500 82 | Viewer.TemplateLineWidth: 1.4 83 | Viewer.SaveResults: 0 84 | -------------------------------------------------------------------------------- /utils/runDefSlamAutoTune.cmd: -------------------------------------------------------------------------------- 1 | rem @echo off 2 | 3 | SETLOCAL 4 | set exe_dir=%~dp0..\Apps\Release\DefSLAMGT.exe 5 | set voc_dir=%~dp0..\Vocabulary\ORBvoc.txt 6 | set yaml_dir=%~dp0..\..\MandalaDataset\stereo0.yaml 7 | set img_dir=\images 8 | set time_dir=\timestamps\timestamps.txt 9 | 10 | for %%a in (%*) do ( 11 | echo "%%~dpna%img_dir%" 12 | powershell -Command %exe_dir% %voc_dir% %yaml_dir% %%~dpna%img_dir% %%~dpna%img_dir% %%~dpna%time_dir% 13 | ) 14 | ENDLOCAL 15 | pause --------------------------------------------------------------------------------