├── .gitignore ├── CMakeLists.txt ├── Dependencies.md ├── Examples ├── Monocular │ ├── EuRoC.yaml │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ ├── TUM1.yaml │ ├── TUM2.yaml │ ├── TUM3.yaml │ ├── mono_euroc.cc │ ├── mono_kitti.cc │ └── mono_tum.cc ├── RGB-D │ ├── TUM1.yaml │ ├── TUM2.yaml │ ├── TUM3.yaml │ ├── associations │ │ ├── fr1_desk.txt │ │ ├── fr1_desk2.txt │ │ ├── fr1_room.txt │ │ ├── fr1_xyz.txt │ │ ├── fr2_desk.txt │ │ ├── fr2_xyz.txt │ │ ├── fr3_nstr_tex_near.txt │ │ ├── fr3_office.txt │ │ ├── fr3_str_tex_far.txt │ │ └── fr3_str_tex_near.txt │ └── rgbd_tum.cc ├── ROS │ └── ORB_SLAM2 │ │ ├── CMakeLists.txt │ │ ├── manifest.xml │ │ └── src │ │ ├── ros_mono.cc │ │ ├── ros_rgbd.cc │ │ └── ros_stereo.cc └── Stereo │ ├── EuRoC.yaml │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ └── stereo_kitti.cc ├── LICENSE.txt ├── License-gpl.txt ├── README.md ├── Thirdparty ├── 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 ├── EDLines │ ├── BoyAndGirl.pgm │ ├── EDLinesLib.a │ ├── EDLinesTest-x64.tar.gz │ ├── LS.h │ ├── License.txt │ ├── Makefile │ ├── Timer.h │ ├── chairs.pgm │ ├── cigar.pgm │ ├── house.pgm │ ├── libopencv_core.so.2.4.5 │ ├── libopencv_imgproc.so.2.4.5 │ ├── main.cpp │ ├── pasta.pgm │ ├── street.pgm │ └── zebra.pgm ├── EDTest │ ├── CannySR-EdgeMap.pgm │ ├── CannySR.pdf │ ├── CannySRPF-EdgeMap.pgm │ ├── ED-EdgeMap.pgm │ ├── ED.pdf │ ├── EDLib.a │ ├── EDLib.h │ ├── EDPF-EdgeMap.pgm │ ├── EDPF.pdf │ ├── EdgeMap.h │ ├── License.txt │ ├── Makefile │ ├── Timer.h │ ├── lena.pgm │ ├── libopencv_core.so.2.4.5 │ ├── libopencv_imgproc.so.2.4.5 │ └── main.cpp ├── Line3Dpp │ ├── 3rdParty │ │ └── tclap │ │ │ ├── Arg.h │ │ │ ├── ArgException.h │ │ │ ├── ArgTraits.h │ │ │ ├── CmdLine.h │ │ │ ├── CmdLineInterface.h │ │ │ ├── CmdLineOutput.h │ │ │ ├── Constraint.h │ │ │ ├── DocBookOutput.h │ │ │ ├── HelpVisitor.h │ │ │ ├── IgnoreRestVisitor.h │ │ │ ├── Makefile.am │ │ │ ├── Makefile.in │ │ │ ├── MultiArg.h │ │ │ ├── MultiSwitchArg.h │ │ │ ├── OptionalUnlabeledTracker.h │ │ │ ├── StandardTraits.h │ │ │ ├── StdOutput.h │ │ │ ├── SwitchArg.h │ │ │ ├── UnlabeledMultiArg.h │ │ │ ├── UnlabeledValueArg.h │ │ │ ├── ValueArg.h │ │ │ ├── ValuesConstraint.h │ │ │ ├── VersionVisitor.h │ │ │ ├── Visitor.h │ │ │ ├── XorHandler.h │ │ │ ├── ZshCompletionOutput.h │ │ │ └── sstream.h │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── clustering.cc │ ├── clustering.h │ ├── cmake │ │ ├── FindCUDA.cmake │ │ ├── FindCUDA │ │ │ ├── .svn │ │ │ │ ├── all-wcprops │ │ │ │ ├── entries │ │ │ │ ├── prop-base │ │ │ │ │ ├── make2cmake.cmake.svn-base │ │ │ │ │ ├── parse_cubin.cmake.svn-base │ │ │ │ │ └── run_nvcc.cmake.svn-base │ │ │ │ └── text-base │ │ │ │ │ ├── make2cmake.cmake.svn-base │ │ │ │ │ ├── parse_cubin.cmake.svn-base │ │ │ │ │ └── run_nvcc.cmake.svn-base │ │ │ ├── make2cmake.cmake │ │ │ ├── parse_cubin.cmake │ │ │ └── run_nvcc.cmake │ │ ├── FindCUDASDK.cmake │ │ ├── FindEigen.cmake │ │ └── FindQwt.cmake │ ├── commons.h │ ├── configLIBS.h.in │ ├── cudawrapper.cu │ ├── cudawrapper.h │ ├── dataArray.h │ ├── helper_math.h │ ├── libs │ │ └── precompiled │ │ │ └── ceres │ │ │ └── include │ │ │ └── ceres │ │ │ ├── autodiff_cost_function.h │ │ │ ├── autodiff_local_parameterization.h │ │ │ ├── c_api.h │ │ │ ├── ceres.h │ │ │ ├── conditioned_cost_function.h │ │ │ ├── cost_function.h │ │ │ ├── cost_function_to_functor.h │ │ │ ├── covariance.h │ │ │ ├── crs_matrix.h │ │ │ ├── dynamic_autodiff_cost_function.h │ │ │ ├── dynamic_numeric_diff_cost_function.h │ │ │ ├── fpclassify.h │ │ │ ├── gradient_checker.h │ │ │ ├── internal │ │ │ ├── autodiff.h │ │ │ ├── eigen.h │ │ │ ├── fixed_array.h │ │ │ ├── macros.h │ │ │ ├── manual_constructor.h │ │ │ ├── miniglog │ │ │ │ └── glog │ │ │ │ │ └── logging.h │ │ │ ├── numeric_diff.h │ │ │ ├── port.h │ │ │ ├── scoped_ptr.h │ │ │ └── variadic_evaluate.h │ │ │ ├── iteration_callback.h │ │ │ ├── jet.h │ │ │ ├── local_parameterization.h │ │ │ ├── loss_function.h │ │ │ ├── normal_prior.h │ │ │ ├── numeric_diff_cost_function.h │ │ │ ├── numeric_diff_functor.h │ │ │ ├── ordered_groups.h │ │ │ ├── problem.h │ │ │ ├── rotation.h │ │ │ ├── sized_cost_function.h │ │ │ ├── solver.h │ │ │ └── types.h │ ├── line3D.cc │ ├── line3D.h │ ├── lsd │ │ ├── lsd.cpp │ │ ├── lsd.hpp │ │ ├── lsd_opencv.cpp │ │ ├── lsd_opencv.hpp │ │ ├── lsd_wrap.cpp │ │ └── lsd_wrap.hpp │ ├── main_bundler.cpp │ ├── main_colmap.cpp │ ├── main_mavmap.cpp │ ├── main_openmvg.cpp │ ├── main_pix4d.cpp │ ├── main_vsfm.cpp │ ├── optimization.cc │ ├── optimization.h │ ├── segment3D.h │ ├── serialization.h │ ├── sparsematrix.cc │ ├── sparsematrix.h │ ├── teaser.png │ ├── testdata │ │ ├── Line3D++_ref │ │ │ ├── Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__OPTIMIZED__vis_3.bin │ │ │ ├── Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__OPTIMIZED__vis_3.stl │ │ │ ├── Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__OPTIMIZED__vis_3.txt │ │ │ ├── Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__vis_3.bin │ │ │ ├── Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__vis_3.stl │ │ │ └── Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__vis_3.txt │ │ ├── img000055.jpg │ │ ├── img000056.jpg │ │ ├── img000057.jpg │ │ ├── img000058.jpg │ │ ├── img000059.jpg │ │ ├── img000060.jpg │ │ ├── img000061.jpg │ │ ├── img000062.jpg │ │ ├── img000063.jpg │ │ ├── img000064.jpg │ │ ├── img000065.jpg │ │ ├── img000066.jpg │ │ ├── img000067.jpg │ │ ├── img000068.jpg │ │ ├── img000069.jpg │ │ ├── img000070.jpg │ │ ├── img000071.jpg │ │ ├── img000072.jpg │ │ ├── img000073.jpg │ │ ├── img000074.jpg │ │ ├── img000075.jpg │ │ ├── img000076.jpg │ │ ├── img000077.jpg │ │ ├── img000078.jpg │ │ ├── img000079.jpg │ │ ├── img000080.jpg │ │ └── vsfm_result.nvm │ ├── universe.h │ ├── view.cc │ └── view.h └── 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 │ │ ├── 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 │ └── license-bsd.txt ├── Vocabulary └── ORBvoc.bin ├── build.sh ├── cmake_modules └── FindEigen3.cmake ├── cubeslam_results └── cube_slam.md ├── data ├── groundtruth.txt ├── rgb_full_demo.txt ├── rgb_seq_pose.txt ├── t_test.txt └── yolo_txts.tar.gz ├── eval ├── README ├── downsample.m ├── euclideanDistanceTwoPointClouds.m ├── evaluate.m ├── register.m └── scaling.m ├── figures ├── data_association.jpg ├── scale_and_orientation.png └── tum_fr3.png ├── include ├── CARV │ ├── Exception.h │ ├── FreespaceDelaunayAlgorithm.h │ ├── GraphWrapper_Boost.h │ ├── Matrix.h │ ├── Matrix_vect_private.cc │ ├── SFMTranscript.h │ ├── SFMTranscriptInterface_Delaunay.h │ ├── SFMTranscriptInterface_ORBSLAM.h │ ├── StringFunctions.h │ ├── lapack_declarations.h │ └── lovimath.h ├── Converter.h ├── Frame.h ├── FrameDrawer.h ├── Initializer.h ├── KeyFrame.h ├── KeyFrameDatabase.h ├── LineDetector.h ├── LocalMapping.h ├── LoopClosing.h ├── Map.h ├── MapDrawer.h ├── MapPoint.h ├── Modeler.h ├── ORBVocabulary.h ├── ORBextractor.h ├── ORBmatcher.h ├── Object.h ├── Optimizer.h ├── PnPsolver.h ├── ProbabilityMapping.h ├── Sim3Solver.h ├── System.h ├── Tracking.h ├── Viewer.h ├── YOLOv3SE.h ├── detect_3d_cuboid │ ├── detect_3d_cuboid.h │ ├── matrix_utils.h │ └── object_3d_util.h ├── isolation_forest.h └── line_lbd │ ├── line_descriptor.hpp │ ├── line_descriptor │ └── descriptor.hpp │ └── line_lbd_allclass.h ├── src ├── CARV │ ├── Exception.cpp │ ├── FreespaceDelaunayAlgorithm.cc │ ├── GraphWrapper_Boost.cc │ ├── Matrix.cc │ ├── SFMTranscript.cpp │ ├── SFMTranscriptInterface_Delaunay.cpp │ ├── SFMTranscriptInterface_ORBSLAM.cpp │ ├── StringFunctions.cpp │ └── lovimath.cc ├── Converter.cc ├── Frame.cc ├── FrameDrawer.cc ├── Initializer.cc ├── KeyFrame.cc ├── KeyFrameDatabase.cc ├── LineDetector.cc ├── LocalMapping.cc ├── LoopClosing.cc ├── Map.cc ├── MapDrawer.cc ├── MapPoint.cc ├── Modeler.cc ├── ORBextractor.cc ├── ORBmatcher.cc ├── Object.cc ├── Optimizer.cc ├── PnPsolver.cc ├── ProbabilityMapping.cc ├── Sim3Solver.cc ├── System.cc ├── Tracking.cc ├── Viewer.cc ├── detect_3d_cuboid │ ├── box_proposal_detail.cpp │ ├── matrix_utils.cpp │ └── object_3d_util.cpp └── line_detect │ ├── libs │ ├── LSDDetector.cpp │ ├── binary_descriptor.cpp │ ├── binary_descriptor_matcher.cpp │ ├── bitarray.hpp │ ├── bitops.hpp │ ├── draw.cpp │ ├── lsd.cpp │ ├── precomp.hpp │ └── types.hpp │ └── line_lbd_allclass.cpp └── tools └── bin_vocabulary.cc /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | Examples/Monocular/mono_euroc 3 | Examples/Monocular/mono_kitti 4 | Examples/Monocular/mono_tum 5 | Examples/RGB-D/rgbd_tum 6 | Examples/ROS/ORB_SLAM2/CMakeLists.txt.user 7 | Examples/ROS/ORB_SLAM2/Mono 8 | Examples/ROS/ORB_SLAM2/MonoAR 9 | Examples/ROS/ORB_SLAM2/RGBD 10 | Examples/ROS/ORB_SLAM2/Stereo 11 | Examples/ROS/ORB_SLAM2/build/ 12 | Examples/Stereo/stereo_euroc 13 | Examples/Stereo/stereo_kitti 14 | KeyFrameTrajectory.txt 15 | Thirdparty/DBoW2/build/ 16 | Thirdparty/DBoW2/lib/ 17 | Thirdparty/g2o/build/ 18 | # Thirdparty/g2o/config.h 19 | Thirdparty/g2o/lib/ 20 | Thirdparty/Line3Dpp/build/ 21 | #Vocabulary/ORBvoc.txt 22 | #Vocabulary/ORBvoc.bin 23 | #Vocabulary/ORBvoc.txt.tar.gz 24 | build/ 25 | lib/ 26 | results_line_segments/ 27 | tools/bin_vocabulary 28 | OutputObj/ 29 | Thirdparty/Line3Dpp/libs/precompiled/ceres/lib64/vc10/ceres.lib 30 | data_associate.txt 31 | result_associate/ 32 | ./vscode/ 33 | sample/ 34 | data/yolo_txts/ 35 | cubeslam_results/*.png 36 | box/ 37 | 38 | # Prerequisites 39 | *.d 40 | 41 | # Compiled Object files 42 | *.slo 43 | *.lo 44 | *.o 45 | *.obj 46 | 47 | # Precompiled Headers 48 | *.gch 49 | *.pch 50 | 51 | # Compiled Dynamic libraries 52 | *.so 53 | *.dylib 54 | *.dll 55 | 56 | # Fortran module files 57 | *.mod 58 | *.smod 59 | 60 | # Executables 61 | *.exe 62 | *.out 63 | *.app 64 | 65 | *~ 66 | 67 | # CLion 68 | .idea/ 69 | 70 | # VScode 71 | #*/.vscode/ 72 | -------------------------------------------------------------------------------- /Examples/Monocular/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 458.654 9 | Camera.fy: 457.296 10 | Camera.cx: 367.215 11 | Camera.cy: 248.375 12 | 13 | Camera.k1: -0.28340811 14 | Camera.k2: 0.07395907 15 | Camera.p1: 0.00019359 16 | Camera.p2: 1.76187114e-05 17 | 18 | # Camera frames per second 19 | Camera.fps: 20.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #--------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | # Viewer.ViewpointY: -0.7 59 | # Viewer.ViewpointZ: -1.8 60 | # Viewer.ViewpointF: 500 -------------------------------------------------------------------------------- /Examples/Monocular/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 50.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 50.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5208.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 50.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 5000.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | rosbuild_init() 5 | 6 | IF(NOT ROS_BUILD_TYPE) 7 | SET(ROS_BUILD_TYPE Release) 8 | ENDIF() 9 | 10 | MESSAGE("Build type: " ${ROS_BUILD_TYPE}) 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 14 | 15 | # Check C++11 or C++0x support 16 | include(CheckCXXCompilerFlag) 17 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 18 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 19 | if(COMPILER_SUPPORTS_CXX11) 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 21 | add_definitions(-DCOMPILEDWITHC11) 22 | message(STATUS "Using flag -std=c++11.") 23 | elseif(COMPILER_SUPPORTS_CXX0X) 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 25 | add_definitions(-DCOMPILEDWITHC0X) 26 | message(STATUS "Using flag -std=c++0x.") 27 | else() 28 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 29 | endif() 30 | 31 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) 32 | 33 | find_package(OpenCV 2.4.3 REQUIRED) 34 | find_package(Eigen3 3.1.0 REQUIRED) 35 | find_package(Pangolin REQUIRED) 36 | 37 | include_directories( 38 | ${PROJECT_SOURCE_DIR} 39 | ${PROJECT_SOURCE_DIR}/../../../ 40 | ${PROJECT_SOURCE_DIR}/../../../include 41 | ${Pangolin_INCLUDE_DIRS} 42 | ) 43 | 44 | set(LIBS 45 | ${OpenCV_LIBS} 46 | ${EIGEN3_LIBS} 47 | ${Pangolin_LIBRARIES} 48 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 49 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 50 | ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so 51 | ) 52 | 53 | # Node for monocular camera 54 | rosbuild_add_executable(Mono 55 | src/ros_mono.cc 56 | ) 57 | 58 | target_link_libraries(Mono 59 | ${LIBS} 60 | ) 61 | 62 | # Node for stereo camera 63 | rosbuild_add_executable(Stereo 64 | src/ros_stereo.cc 65 | ) 66 | 67 | target_link_libraries(Stereo 68 | ${LIBS} 69 | ) 70 | 71 | # Node for RGB-D camera 72 | rosbuild_add_executable(RGBD 73 | src/ros_rgbd.cc 74 | ) 75 | 76 | target_link_libraries(RGBD 77 | ${LIBS} 78 | ) 79 | 80 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ORB_SLAM2 5 | 6 | 7 | Raul Mur-Artal 8 | GPLv3 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/src/ros_mono.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include"../../../include/System.h" 33 | 34 | using namespace std; 35 | 36 | class ImageGrabber 37 | { 38 | public: 39 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 40 | 41 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 42 | 43 | ORB_SLAM2::System* mpSLAM; 44 | }; 45 | 46 | int main(int argc, char **argv) 47 | { 48 | ros::init(argc, argv, "Mono"); 49 | ros::start(); 50 | 51 | if(argc != 3) 52 | { 53 | cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; 54 | ros::shutdown(); 55 | return 1; 56 | } 57 | 58 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 59 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 60 | 61 | ImageGrabber igb(&SLAM); 62 | 63 | ros::NodeHandle nodeHandler; 64 | ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 65 | 66 | ros::spin(); 67 | 68 | // Stop all threads 69 | SLAM.Shutdown(); 70 | 71 | // Save camera trajectory 72 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 73 | 74 | ros::shutdown(); 75 | 76 | return 0; 77 | } 78 | 79 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 80 | { 81 | // Copy the ros image message to cv::Mat. 82 | cv_bridge::CvImageConstPtr cv_ptr; 83 | try 84 | { 85 | cv_ptr = cv_bridge::toCvShare(msg); 86 | } 87 | catch (cv_bridge::Exception& e) 88 | { 89 | ROS_ERROR("cv_bridge exception: %s", e.what()); 90 | return; 91 | } 92 | 93 | mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); 94 | } 95 | 96 | 97 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 386.1448 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 387.5744 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 379.8145 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 12 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /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/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | 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 REQUIRED) 28 | 29 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 30 | 31 | include_directories(${OpenCV_INCLUDE_DIRS}) 32 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 33 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /Thirdparty/EDLines/BoyAndGirl.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/BoyAndGirl.pgm -------------------------------------------------------------------------------- /Thirdparty/EDLines/EDLinesLib.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/EDLinesLib.a -------------------------------------------------------------------------------- /Thirdparty/EDLines/EDLinesTest-x64.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/EDLinesTest-x64.tar.gz -------------------------------------------------------------------------------- /Thirdparty/EDLines/LS.h: -------------------------------------------------------------------------------- 1 | #ifndef _LS_H_ 2 | #define _LS_H_ 3 | 4 | struct LS { 5 | double sx, sy, ex, ey; // Start & end coordinates of the line segment 6 | }; 7 | 8 | #endif -------------------------------------------------------------------------------- /Thirdparty/EDLines/License.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/License.txt -------------------------------------------------------------------------------- /Thirdparty/EDLines/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | g++ -o EDLinesTest main.cpp EDLinesLib.a libopencv_imgproc.so.2.4.5 libopencv_core.so.2.4.5 3 | 4 | 5 | clean: 6 | rm -rf EDLinesTest core 7 | -------------------------------------------------------------------------------- /Thirdparty/EDLines/Timer.h: -------------------------------------------------------------------------------- 1 | #ifndef _TIMER_H_ 2 | #define _TIMER_H_ 3 | 4 | #include 5 | 6 | class Timer { 7 | private: 8 | struct timeval start, end; 9 | 10 | public: 11 | Timer(){} //end-TimerClass 12 | 13 | void Start(){ 14 | gettimeofday(&start, NULL); 15 | } //end-Start 16 | 17 | void Stop(){ 18 | gettimeofday(&end, NULL); 19 | } //end-Stop 20 | 21 | // Returns time in milliseconds 22 | double ElapsedTime(){ 23 | if (end.tv_sec == start.tv_sec) return (end.tv_usec - start.tv_usec)/1e3; 24 | double elapsedTime = 1e6-start.tv_usec; 25 | elapsedTime += end.tv_usec; 26 | elapsedTime += 1e6*(end.tv_sec-start.tv_sec-1); 27 | 28 | return elapsedTime/1e3; 29 | } //end-Elapsed 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /Thirdparty/EDLines/chairs.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/chairs.pgm -------------------------------------------------------------------------------- /Thirdparty/EDLines/cigar.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/cigar.pgm -------------------------------------------------------------------------------- /Thirdparty/EDLines/house.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/house.pgm -------------------------------------------------------------------------------- /Thirdparty/EDLines/libopencv_core.so.2.4.5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/libopencv_core.so.2.4.5 -------------------------------------------------------------------------------- /Thirdparty/EDLines/libopencv_imgproc.so.2.4.5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/libopencv_imgproc.so.2.4.5 -------------------------------------------------------------------------------- /Thirdparty/EDLines/pasta.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/pasta.pgm -------------------------------------------------------------------------------- /Thirdparty/EDLines/street.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/street.pgm -------------------------------------------------------------------------------- /Thirdparty/EDLines/zebra.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDLines/zebra.pgm -------------------------------------------------------------------------------- /Thirdparty/EDTest/CannySR-EdgeMap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/CannySR-EdgeMap.pgm -------------------------------------------------------------------------------- /Thirdparty/EDTest/CannySR.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/CannySR.pdf -------------------------------------------------------------------------------- /Thirdparty/EDTest/CannySRPF-EdgeMap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/CannySRPF-EdgeMap.pgm -------------------------------------------------------------------------------- /Thirdparty/EDTest/ED-EdgeMap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/ED-EdgeMap.pgm -------------------------------------------------------------------------------- /Thirdparty/EDTest/ED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/ED.pdf -------------------------------------------------------------------------------- /Thirdparty/EDTest/EDLib.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/EDLib.a -------------------------------------------------------------------------------- /Thirdparty/EDTest/EDLib.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDLIB_H_ 2 | #define _EDLIB_H_ 3 | 4 | #include "EdgeMap.h" 5 | 6 | /// Detect Edges by Edge Drawing (ED). Steps of the algorithm: 7 | /// (1) Smooth the image with a 5x5 Gaussian kernel with sigma=smoothingSigma 8 | /// (2) Compute the gradient magnitude and directions using the GradientOperator (can be Prewitt, Sobel, Scharr) 9 | /// (3) Compute the anchors using ANCHOR_THRESH 10 | /// (4) Link the anchors using Edge Drawing's Smart Routing Algorithm to obtain edge segments 11 | /// (5) Return the edge segments to the user 12 | /// Note: smoothingSigma must be >= 1.0 13 | EdgeMap *DetectEdgesByED(unsigned char *srcImg, int width, int height, GradientOperator op, int GRADIENT_THRESH, int ANCHOR_THRESH, double smoothingSigma); 14 | 15 | /// (1) Use DetectEdgesByED(srcImg, width, height, PREWITT_OPERATOR, 16, 0, smoothingSigma) to ontain ALL edge segments in the image 16 | /// (2) Validate the edge segments using the Helmholtz principle, returning only the validated edge segments 17 | /// Note: smoothingSigma must be >= 1.0 18 | EdgeMap *DetectEdgesByEDPF(unsigned char *srcImg, int width, int height, double smoothingSigma); 19 | 20 | /// (1) Smooth srcImg with cvSmooth(srcImg, smoothedImg, 5, 5, smoothingSigma) [smooth the src image with the gaussian kernel] 21 | /// (2) Use cvCanny(smoothedImg, cannyLowThresh, cannyHighThresh, sobelApertureSize) [Obtain Canny binary edge map by cvCanny] 22 | /// (3) Pick the canny edge map points as anchors and use Smart Routing to link the anchor points and obtain the edge segments 23 | /// (4) Return the edge segments to the user 24 | /// Note: smoothingSigma must be >= 1.0 25 | EdgeMap *DetectEdgesByCannySR(unsigned char *srcImg, int width, int height, int cannyLowThresh, int cannyHighThresh, int sobelKernelApertureSize=3, double smoothingSigma=1.0); 26 | 27 | /// (1) Use DetectEdgesByCannySR(srcImg, width, height, 20, 20, sobelKernelApertureSize, smoothingSigma) to obtain ALL edge segments in the image 28 | /// (2) Validate the edge segments using the Helmholtz principle, returning only the validated edge segments 29 | /// Note: smoothingSigma must be >= 1.0 30 | EdgeMap *DetectEdgesByCannySRPF(unsigned char *srcImg, int width, int height, int sobelKernelApertureSize=3, double smoothingSigma=1.0); 31 | 32 | #endif -------------------------------------------------------------------------------- /Thirdparty/EDTest/EDPF-EdgeMap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/EDPF-EdgeMap.pgm -------------------------------------------------------------------------------- /Thirdparty/EDTest/EDPF.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/EDPF.pdf -------------------------------------------------------------------------------- /Thirdparty/EDTest/EdgeMap.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDGE_MAP_H_ 2 | #define _EDGE_MAP_H_ 3 | 4 | #include 5 | 6 | enum GradientOperator {PREWITT_OPERATOR=101, SOBEL_OPERATOR=102, SCHARR_OPERATOR=103}; 7 | 8 | struct Pixel {int r, c;}; 9 | 10 | struct EdgeSegment { 11 | Pixel *pixels; // Pointer to the pixels array 12 | int noPixels; // # of pixels in the edge map 13 | }; 14 | 15 | struct EdgeMap { 16 | public: 17 | int width, height; // Width & height of the image 18 | unsigned char *edgeImg; // BW edge map 19 | 20 | 21 | Pixel *pixels; // Edge map in edge segment form 22 | EdgeSegment *segments; 23 | int noSegments; 24 | 25 | public: 26 | // constructor 27 | EdgeMap(int w, int h){ 28 | width = w; 29 | height = h; 30 | 31 | edgeImg = new unsigned char[width*height]; 32 | 33 | pixels = new Pixel[width*height]; 34 | segments = new EdgeSegment[width*height]; 35 | noSegments = 0; 36 | } //end-EdgeMap 37 | 38 | // Destructor 39 | ~EdgeMap(){ 40 | delete edgeImg; 41 | delete pixels; 42 | delete segments; 43 | } //end-~EdgeMap 44 | 45 | 46 | void ConvertEdgeSegments2EdgeImg(){ 47 | memset(edgeImg, 0, width*height); 48 | 49 | for (int i=0; i 5 | 6 | class Timer { 7 | private: 8 | struct timeval start, end; 9 | 10 | public: 11 | Timer(){} //end-TimerClass 12 | 13 | void Start(){ 14 | gettimeofday(&start, NULL); 15 | } //end-Start 16 | 17 | void Stop(){ 18 | gettimeofday(&end, NULL); 19 | } //end-Stop 20 | 21 | // Returns time in milliseconds 22 | double ElapsedTime(){ 23 | if (end.tv_sec == start.tv_sec) return (end.tv_usec - start.tv_usec)/1e3; 24 | double elapsedTime = 1e6-start.tv_usec; 25 | elapsedTime += end.tv_usec; 26 | elapsedTime += 1e6*(end.tv_sec-start.tv_sec-1); 27 | 28 | return elapsedTime/1e3; 29 | } //end-Elapsed 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /Thirdparty/EDTest/lena.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/lena.pgm -------------------------------------------------------------------------------- /Thirdparty/EDTest/libopencv_core.so.2.4.5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/libopencv_core.so.2.4.5 -------------------------------------------------------------------------------- /Thirdparty/EDTest/libopencv_imgproc.so.2.4.5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/libopencv_imgproc.so.2.4.5 -------------------------------------------------------------------------------- /Thirdparty/EDTest/main.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/EDTest/main.cpp -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/ArgTraits.h: -------------------------------------------------------------------------------- 1 | // -*- Mode: c++; c-basic-offset: 4; tab-width: 4; -*- 2 | 3 | /****************************************************************************** 4 | * 5 | * file: ArgTraits.h 6 | * 7 | * Copyright (c) 2007, Daniel Aarno, Michael E. Smoot . 8 | * All rights reserved. 9 | * 10 | * See the file COPYING in the top directory of this distribution for 11 | * more information. 12 | * 13 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 14 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 18 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 19 | * DEALINGS IN THE SOFTWARE. 20 | * 21 | *****************************************************************************/ 22 | 23 | // This is an internal tclap file, you should probably not have to 24 | // include this directly 25 | 26 | #ifndef TCLAP_ARGTRAITS_H 27 | #define TCLAP_ARGTRAITS_H 28 | 29 | namespace TCLAP { 30 | 31 | // We use two empty structs to get compile type specialization 32 | // function to work 33 | 34 | /** 35 | * A value like argument value type is a value that can be set using 36 | * operator>>. This is the default value type. 37 | */ 38 | struct ValueLike { 39 | typedef ValueLike ValueCategory; 40 | virtual ~ValueLike() {} 41 | }; 42 | 43 | /** 44 | * A string like argument value type is a value that can be set using 45 | * operator=(string). Useful if the value type contains spaces which 46 | * will be broken up into individual tokens by operator>>. 47 | */ 48 | struct StringLike { 49 | virtual ~StringLike() {} 50 | }; 51 | 52 | /** 53 | * A class can inherit from this object to make it have string like 54 | * traits. This is a compile time thing and does not add any overhead 55 | * to the inherenting class. 56 | */ 57 | struct StringLikeTrait { 58 | typedef StringLike ValueCategory; 59 | virtual ~StringLikeTrait() {} 60 | }; 61 | 62 | /** 63 | * A class can inherit from this object to make it have value like 64 | * traits. This is a compile time thing and does not add any overhead 65 | * to the inherenting class. 66 | */ 67 | struct ValueLikeTrait { 68 | typedef ValueLike ValueCategory; 69 | virtual ~ValueLikeTrait() {} 70 | }; 71 | 72 | /** 73 | * Arg traits are used to get compile type specialization when parsing 74 | * argument values. Using an ArgTraits you can specify the way that 75 | * values gets assigned to any particular type during parsing. The two 76 | * supported types are StringLike and ValueLike. 77 | */ 78 | template 79 | struct ArgTraits { 80 | typedef typename T::ValueCategory ValueCategory; 81 | virtual ~ArgTraits() {} 82 | //typedef ValueLike ValueCategory; 83 | }; 84 | 85 | } // namespace 86 | 87 | #endif 88 | 89 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/CmdLineOutput.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | /****************************************************************************** 4 | * 5 | * file: CmdLineOutput.h 6 | * 7 | * Copyright (c) 2004, Michael E. Smoot 8 | * All rights reserved. 9 | * 10 | * See the file COPYING in the top directory of this distribution for 11 | * more information. 12 | * 13 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 14 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 18 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 19 | * DEALINGS IN THE SOFTWARE. 20 | * 21 | *****************************************************************************/ 22 | 23 | #ifndef TCLAP_CMDLINEOUTPUT_H 24 | #define TCLAP_CMDLINEOUTPUT_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace TCLAP { 34 | 35 | class CmdLineInterface; 36 | class ArgException; 37 | 38 | /** 39 | * The interface that any output object must implement. 40 | */ 41 | class CmdLineOutput 42 | { 43 | 44 | public: 45 | 46 | /** 47 | * Virtual destructor. 48 | */ 49 | virtual ~CmdLineOutput() {} 50 | 51 | /** 52 | * Generates some sort of output for the USAGE. 53 | * \param c - The CmdLine object the output is generated for. 54 | */ 55 | virtual void usage(CmdLineInterface& c)=0; 56 | 57 | /** 58 | * Generates some sort of output for the version. 59 | * \param c - The CmdLine object the output is generated for. 60 | */ 61 | virtual void version(CmdLineInterface& c)=0; 62 | 63 | /** 64 | * Generates some sort of output for a failure. 65 | * \param c - The CmdLine object the output is generated for. 66 | * \param e - The ArgException that caused the failure. 67 | */ 68 | virtual void failure( CmdLineInterface& c, 69 | ArgException& e )=0; 70 | 71 | }; 72 | 73 | } //namespace TCLAP 74 | #endif 75 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/Constraint.h: -------------------------------------------------------------------------------- 1 | 2 | /****************************************************************************** 3 | * 4 | * file: Constraint.h 5 | * 6 | * Copyright (c) 2005, Michael E. Smoot 7 | * All rights reserved. 8 | * 9 | * See the file COPYING in the top directory of this distribution for 10 | * more information. 11 | * 12 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 13 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 15 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 17 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 18 | * DEALINGS IN THE SOFTWARE. 19 | * 20 | *****************************************************************************/ 21 | 22 | #ifndef TCLAP_CONSTRAINT_H 23 | #define TCLAP_CONSTRAINT_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace TCLAP { 33 | 34 | /** 35 | * The interface that defines the interaction between the Arg and Constraint. 36 | */ 37 | template 38 | class Constraint 39 | { 40 | 41 | public: 42 | /** 43 | * Returns a description of the Constraint. 44 | */ 45 | virtual std::string description() const =0; 46 | 47 | /** 48 | * Returns the short ID for the Constraint. 49 | */ 50 | virtual std::string shortID() const =0; 51 | 52 | /** 53 | * The method used to verify that the value parsed from the command 54 | * line meets the constraint. 55 | * \param value - The value that will be checked. 56 | */ 57 | virtual bool check(const T& value) const =0; 58 | 59 | /** 60 | * Destructor. 61 | * Silences warnings about Constraint being a base class with virtual 62 | * functions but without a virtual destructor. 63 | */ 64 | virtual ~Constraint() { ; } 65 | }; 66 | 67 | } //namespace TCLAP 68 | #endif 69 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/HelpVisitor.h: -------------------------------------------------------------------------------- 1 | 2 | /****************************************************************************** 3 | * 4 | * file: HelpVisitor.h 5 | * 6 | * Copyright (c) 2003, Michael E. Smoot . 7 | * All rights reserved. 8 | * 9 | * See the file COPYING in the top directory of this distribution for 10 | * more information. 11 | * 12 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 13 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 15 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 17 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 18 | * DEALINGS IN THE SOFTWARE. 19 | * 20 | *****************************************************************************/ 21 | 22 | #ifndef TCLAP_HELP_VISITOR_H 23 | #define TCLAP_HELP_VISITOR_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace TCLAP { 30 | 31 | /** 32 | * A Visitor object that calls the usage method of the given CmdLineOutput 33 | * object for the specified CmdLine object. 34 | */ 35 | class HelpVisitor: public Visitor 36 | { 37 | private: 38 | /** 39 | * Prevent accidental copying. 40 | */ 41 | HelpVisitor(const HelpVisitor& rhs); 42 | HelpVisitor& operator=(const HelpVisitor& rhs); 43 | 44 | protected: 45 | 46 | /** 47 | * The CmdLine the output will be generated for. 48 | */ 49 | CmdLineInterface* _cmd; 50 | 51 | /** 52 | * The output object. 53 | */ 54 | CmdLineOutput** _out; 55 | 56 | public: 57 | 58 | /** 59 | * Constructor. 60 | * \param cmd - The CmdLine the output will be generated for. 61 | * \param out - The type of output. 62 | */ 63 | HelpVisitor(CmdLineInterface* cmd, CmdLineOutput** out) 64 | : Visitor(), _cmd( cmd ), _out( out ) { } 65 | 66 | /** 67 | * Calls the usage method of the CmdLineOutput for the 68 | * specified CmdLine. 69 | */ 70 | void visit() { (*_out)->usage(*_cmd); throw ExitException(0); } 71 | 72 | }; 73 | 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/IgnoreRestVisitor.h: -------------------------------------------------------------------------------- 1 | 2 | /****************************************************************************** 3 | * 4 | * file: IgnoreRestVisitor.h 5 | * 6 | * Copyright (c) 2003, Michael E. Smoot . 7 | * All rights reserved. 8 | * 9 | * See the file COPYING in the top directory of this distribution for 10 | * more information. 11 | * 12 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 13 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 15 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 17 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 18 | * DEALINGS IN THE SOFTWARE. 19 | * 20 | *****************************************************************************/ 21 | 22 | 23 | #ifndef TCLAP_IGNORE_REST_VISITOR_H 24 | #define TCLAP_IGNORE_REST_VISITOR_H 25 | 26 | #include 27 | #include 28 | 29 | namespace TCLAP { 30 | 31 | /** 32 | * A Visitor that tells the CmdLine to begin ignoring arguments after 33 | * this one is parsed. 34 | */ 35 | class IgnoreRestVisitor: public Visitor 36 | { 37 | public: 38 | 39 | /** 40 | * Constructor. 41 | */ 42 | IgnoreRestVisitor() : Visitor() {} 43 | 44 | /** 45 | * Sets Arg::_ignoreRest. 46 | */ 47 | void visit() { Arg::beginIgnoring(); } 48 | }; 49 | 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/Makefile.am: -------------------------------------------------------------------------------- 1 | 2 | libtclapincludedir = $(includedir)/tclap 3 | 4 | libtclapinclude_HEADERS = \ 5 | CmdLineInterface.h \ 6 | ArgException.h \ 7 | CmdLine.h \ 8 | XorHandler.h \ 9 | MultiArg.h \ 10 | UnlabeledMultiArg.h \ 11 | ValueArg.h \ 12 | UnlabeledValueArg.h \ 13 | Visitor.h Arg.h \ 14 | HelpVisitor.h \ 15 | SwitchArg.h \ 16 | MultiSwitchArg.h \ 17 | VersionVisitor.h \ 18 | IgnoreRestVisitor.h \ 19 | CmdLineOutput.h \ 20 | StdOutput.h \ 21 | DocBookOutput.h \ 22 | ZshCompletionOutput.h \ 23 | OptionalUnlabeledTracker.h \ 24 | Constraint.h \ 25 | ValuesConstraint.h \ 26 | ArgTraits.h \ 27 | StandardTraits.h \ 28 | sstream.h 29 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/OptionalUnlabeledTracker.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | /****************************************************************************** 4 | * 5 | * file: OptionalUnlabeledTracker.h 6 | * 7 | * Copyright (c) 2005, Michael E. Smoot . 8 | * All rights reserved. 9 | * 10 | * See the file COPYING in the top directory of this distribution for 11 | * more information. 12 | * 13 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 14 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 18 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 19 | * DEALINGS IN THE SOFTWARE. 20 | * 21 | *****************************************************************************/ 22 | 23 | 24 | #ifndef TCLAP_OPTIONAL_UNLABELED_TRACKER_H 25 | #define TCLAP_OPTIONAL_UNLABELED_TRACKER_H 26 | 27 | #include 28 | 29 | namespace TCLAP { 30 | 31 | class OptionalUnlabeledTracker 32 | { 33 | 34 | public: 35 | 36 | static void check( bool req, const std::string& argName ); 37 | 38 | static void gotOptional() { alreadyOptionalRef() = true; } 39 | 40 | static bool& alreadyOptional() { return alreadyOptionalRef(); } 41 | 42 | private: 43 | 44 | static bool& alreadyOptionalRef() { static bool ct = false; return ct; } 45 | }; 46 | 47 | 48 | inline void OptionalUnlabeledTracker::check( bool req, const std::string& argName ) 49 | { 50 | if ( OptionalUnlabeledTracker::alreadyOptional() ) 51 | throw( SpecificationException( 52 | "You can't specify ANY Unlabeled Arg following an optional Unlabeled Arg", 53 | argName ) ); 54 | 55 | if ( !req ) 56 | OptionalUnlabeledTracker::gotOptional(); 57 | } 58 | 59 | 60 | } // namespace TCLAP 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/VersionVisitor.h: -------------------------------------------------------------------------------- 1 | // -*- Mode: c++; c-basic-offset: 4; tab-width: 4; -*- 2 | 3 | /****************************************************************************** 4 | * 5 | * file: VersionVisitor.h 6 | * 7 | * Copyright (c) 2003, Michael E. Smoot . 8 | * All rights reserved. 9 | * 10 | * See the file COPYING in the top directory of this distribution for 11 | * more information. 12 | * 13 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 14 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 18 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 19 | * DEALINGS IN THE SOFTWARE. 20 | * 21 | *****************************************************************************/ 22 | 23 | 24 | #ifndef TCLAP_VERSION_VISITOR_H 25 | #define TCLAP_VERSION_VISITOR_H 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace TCLAP { 32 | 33 | /** 34 | * A Visitor that will call the version method of the given CmdLineOutput 35 | * for the specified CmdLine object and then exit. 36 | */ 37 | class VersionVisitor: public Visitor 38 | { 39 | private: 40 | /** 41 | * Prevent accidental copying 42 | */ 43 | VersionVisitor(const VersionVisitor& rhs); 44 | VersionVisitor& operator=(const VersionVisitor& rhs); 45 | 46 | protected: 47 | 48 | /** 49 | * The CmdLine of interest. 50 | */ 51 | CmdLineInterface* _cmd; 52 | 53 | /** 54 | * The output object. 55 | */ 56 | CmdLineOutput** _out; 57 | 58 | public: 59 | 60 | /** 61 | * Constructor. 62 | * \param cmd - The CmdLine the output is generated for. 63 | * \param out - The type of output. 64 | */ 65 | VersionVisitor( CmdLineInterface* cmd, CmdLineOutput** out ) 66 | : Visitor(), _cmd( cmd ), _out( out ) { } 67 | 68 | /** 69 | * Calls the version method of the output object using the 70 | * specified CmdLine. 71 | */ 72 | void visit() { 73 | (*_out)->version(*_cmd); 74 | throw ExitException(0); 75 | } 76 | 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/Visitor.h: -------------------------------------------------------------------------------- 1 | 2 | /****************************************************************************** 3 | * 4 | * file: Visitor.h 5 | * 6 | * Copyright (c) 2003, Michael E. Smoot . 7 | * All rights reserved. 8 | * 9 | * See the file COPYING in the top directory of this distribution for 10 | * more information. 11 | * 12 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 13 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 15 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 17 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 18 | * DEALINGS IN THE SOFTWARE. 19 | * 20 | *****************************************************************************/ 21 | 22 | 23 | #ifndef TCLAP_VISITOR_H 24 | #define TCLAP_VISITOR_H 25 | 26 | namespace TCLAP { 27 | 28 | /** 29 | * A base class that defines the interface for visitors. 30 | */ 31 | class Visitor 32 | { 33 | public: 34 | 35 | /** 36 | * Constructor. Does nothing. 37 | */ 38 | Visitor() { } 39 | 40 | /** 41 | * Destructor. Does nothing. 42 | */ 43 | virtual ~Visitor() { } 44 | 45 | /** 46 | * Does nothing. Should be overridden by child. 47 | */ 48 | virtual void visit() { } 49 | }; 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/3rdParty/tclap/sstream.h: -------------------------------------------------------------------------------- 1 | // -*- Mode: c++; c-basic-offset: 4; tab-width: 4; -*- 2 | 3 | /****************************************************************************** 4 | * 5 | * file: sstream.h 6 | * 7 | * Copyright (c) 2003, Michael E. Smoot . 8 | * Copyright (c) 2004, Michael E. Smoot, Daniel Aarno . 9 | * Copyright (c) 2017 Google Inc. 10 | * All rights reserved. 11 | * 12 | * See the file COPYING in the top directory of this distribution for 13 | * more information. 14 | * 15 | * THE SOFTWARE IS PROVIDED _AS IS_, WITHOUT WARRANTY OF ANY KIND, EXPRESS 16 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 21 | * DEALINGS IN THE SOFTWARE. 22 | * 23 | *****************************************************************************/ 24 | 25 | #ifndef TCLAP_SSTREAM_H 26 | #define TCLAP_SSTREAM_H 27 | 28 | #if !defined(HAVE_STRSTREAM) 29 | // Assume sstream is available if strstream is not specified 30 | // (https://sourceforge.net/p/tclap/bugs/23/) 31 | #define HAVE_SSTREAM 32 | #endif 33 | 34 | #if defined(HAVE_SSTREAM) 35 | #include 36 | namespace TCLAP { 37 | typedef std::istringstream istringstream; 38 | typedef std::ostringstream ostringstream; 39 | } 40 | #elif defined(HAVE_STRSTREAM) 41 | #include 42 | namespace TCLAP { 43 | typedef std::istrstream istringstream; 44 | typedef std::ostrstream ostringstream; 45 | } 46 | #else 47 | #error "Need a stringstream (sstream or strstream) to compile!" 48 | #endif 49 | 50 | #endif // TCLAP_SSTREAM_H 51 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/clustering.cc: -------------------------------------------------------------------------------- 1 | #include "clustering.h" 2 | 3 | namespace L3DPP 4 | { 5 | //------------------------------------------------------------------------------ 6 | CLUniverse* performClustering(std::list& edges, int numNodes, 7 | float c) 8 | { 9 | if(edges.size() == 0) 10 | return NULL; 11 | 12 | // sort edges by weight (increasing) 13 | edges.sort(L3DPP::sortCLEdgesByWeight); 14 | 15 | // init universe 16 | CLUniverse *u = new CLUniverse(numNodes); 17 | 18 | // init thresholds 19 | float* threshold = new float[numNodes]; 20 | for(int i=0; i < numNodes; ++i) 21 | threshold[i] = c; 22 | 23 | // perform clustering 24 | std::list::const_iterator it = edges.begin(); 25 | for(; it!=edges.end(); ++it) 26 | { 27 | CLEdge e = *it; 28 | 29 | // components conected by this edge 30 | int a = u->find(e.i_); 31 | int b = u->find(e.j_); 32 | if (a != b) 33 | { 34 | // not in the same segment yet 35 | if((e.w_ <= threshold[a]) && (e.w_ <= threshold[b])) 36 | { 37 | // join nodes 38 | u->join(a,b); 39 | a = u->find(a); 40 | threshold[a] = e.w_ + c/u->size(a); 41 | } 42 | } 43 | } 44 | 45 | // cleanup 46 | delete threshold; 47 | return u; 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/clustering.h: -------------------------------------------------------------------------------- 1 | #ifndef I3D_LINE3D_PP_CLUSTERING_H_ 2 | #define I3D_LINE3D_PP_CLUSTERING_H_ 3 | 4 | /* 5 | * Disclaimer: 6 | * Re-implementation of the algorithm described in the paper 7 | * 8 | * Efficient Graph-based Image Segmentation, 9 | * P. Fezenszwalb, F. Huttenlocher, 10 | * International Journal of Computer Vision, 2004. 11 | * 12 | * and based on their official source code, which can be found at 13 | * from http://cs.brown.edu/~pff/segment/ 14 | * 15 | * Their code is put under the GNU GENERAL PUBLIC LICENSE, 16 | * and so is this version. 17 | */ 18 | 19 | /* 20 | Line3D++ - Line-based Multi View Stereo 21 | Copyright (C) 2015 Manuel Hofer 22 | 23 | This program is free software: you can redistribute it and/or modify 24 | it under the terms of the GNU General Public License as published by 25 | the Free Software Foundation, either version 3 of the License, or 26 | (at your option) any later version. 27 | 28 | This program is distributed in the hope that it will be useful, 29 | but WITHOUT ANY WARRANTY; without even the implied warranty of 30 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 31 | GNU General Public License for more details. 32 | 33 | You should have received a copy of the GNU General Public License 34 | along with this program. If not, see . 35 | */ 36 | 37 | #include 38 | #include 39 | 40 | #include "universe.h" 41 | 42 | /** 43 | * Clustering 44 | * ==================== 45 | * Clustering/Segmentation Algorithm. 46 | * [Felzenswalb & Huttenlocher, IJCV 2004] 47 | * 48 | * This code is an adaption of their original source code, 49 | * to fit into our datastructures. 50 | * ==================== 51 | * Author: M.Hofer, 2015 52 | */ 53 | 54 | namespace L3DPP 55 | { 56 | // edge in affinity matrix 57 | typedef struct { 58 | int i_; 59 | int j_; 60 | float w_; 61 | } CLEdge; 62 | 63 | // sorting function for edges 64 | static bool sortCLEdgesByWeight(const CLEdge& a, const CLEdge& b) 65 | { 66 | return a.w_ < b.w_; 67 | } 68 | 69 | // sort entries for sparse affinity matrix (CLEdges) 70 | static bool sortCLEdgesByCol(const CLEdge& a1, const CLEdge& a2) 71 | { 72 | return ((a1.j_ < a2.j_) || (a1.j_ == a2.j_ && a1.i_ < a2.i_)); 73 | } 74 | 75 | static bool sortCLEdgesByRow(const CLEdge& a1, const CLEdge& a2) 76 | { 77 | return ((a1.i_ < a2.i_) || (a1.i_ == a2.i_ && a1.j_ < a2.j_)); 78 | } 79 | 80 | // perform graph clustering 81 | // NOTE: edges are sorted during the process! 82 | CLUniverse* performClustering(std::list& edges, int numNodes, 83 | float c); 84 | 85 | } 86 | 87 | #endif //I3D_LINE3D_PP_CLUSTERING_H_ 88 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/cmake/FindCUDA/.svn/all-wcprops: -------------------------------------------------------------------------------- 1 | K 25 2 | svn:wc:ra_dav:version-url 3 | V 53 4 | /svn/findcuda/!svn/ver/1220/trunk/CMake/cuda/FindCUDA 5 | END 6 | parse_cubin.cmake 7 | K 25 8 | svn:wc:ra_dav:version-url 9 | V 71 10 | /svn/findcuda/!svn/ver/1210/trunk/CMake/cuda/FindCUDA/parse_cubin.cmake 11 | END 12 | run_nvcc.cmake 13 | K 25 14 | svn:wc:ra_dav:version-url 15 | V 68 16 | /svn/findcuda/!svn/ver/1220/trunk/CMake/cuda/FindCUDA/run_nvcc.cmake 17 | END 18 | make2cmake.cmake 19 | K 25 20 | svn:wc:ra_dav:version-url 21 | V 70 22 | /svn/findcuda/!svn/ver/1210/trunk/CMake/cuda/FindCUDA/make2cmake.cmake 23 | END 24 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/cmake/FindCUDA/.svn/entries: -------------------------------------------------------------------------------- 1 | 10 2 | 3 | dir 4 | 1227 5 | https://gforge.sci.utah.edu/svn/findcuda/trunk/CMake/cuda/FindCUDA 6 | https://gforge.sci.utah.edu/svn/findcuda 7 | 8 | 9 | 10 | 2012-12-19T20:07:28.616378Z 11 | 1220 12 | bigler 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 81322f20-870f-0410-845c-a4cd4664c908 28 | 29 | parse_cubin.cmake 30 | file 31 | 32 | 33 | 34 | 35 | 2014-06-03T07:18:47.000000Z 36 | f233707b4334672f3556587babf3e336 37 | 2012-08-20T22:47:59.683791Z 38 | 1210 39 | bigler 40 | has-props 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 3666 62 | 63 | run_nvcc.cmake 64 | file 65 | 66 | 67 | 68 | 69 | 2014-06-03T07:18:47.000000Z 70 | 13d0777ff373d91976023d1a6d35e6f5 71 | 2012-12-19T20:07:28.616378Z 72 | 1220 73 | bigler 74 | has-props 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 12755 96 | 97 | make2cmake.cmake 98 | file 99 | 100 | 101 | 102 | 103 | 2014-06-03T07:18:47.000000Z 104 | 52601701409164fa716b8834f722aeef 105 | 2012-08-20T22:47:59.683791Z 106 | 1210 107 | bigler 108 | has-props 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 3532 130 | 131 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/cmake/FindCUDA/.svn/prop-base/make2cmake.cmake.svn-base: -------------------------------------------------------------------------------- 1 | K 13 2 | svn:eol-style 3 | V 6 4 | native 5 | K 14 6 | svn:executable 7 | V 1 8 | * 9 | END 10 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/cmake/FindCUDA/.svn/prop-base/parse_cubin.cmake.svn-base: -------------------------------------------------------------------------------- 1 | K 13 2 | svn:eol-style 3 | V 6 4 | native 5 | END 6 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/cmake/FindCUDA/.svn/prop-base/run_nvcc.cmake.svn-base: -------------------------------------------------------------------------------- 1 | K 13 2 | svn:eol-style 3 | V 6 4 | native 5 | K 14 6 | svn:executable 7 | V 1 8 | * 9 | END 10 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/configLIBS.h.in: -------------------------------------------------------------------------------- 1 | #ifndef I3D_LINE3D_PP_LIBS_CONFIG_H_ 2 | #define I3D_LINE3D_PP_LIBS_CONFIG_H_ 3 | 4 | #cmakedefine L3DPP_OPENMP 1 5 | #cmakedefine L3DPP_CUDA 1 6 | #cmakedefine L3DPP_CERES 1 7 | #cmakedefine L3DPP_OPENCV3 1 8 | 9 | #endif //I3D_LINE3D_PP_LIBS_CONFIG_H_ 10 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/libs/precompiled/ceres/include/ceres/ceres.h: -------------------------------------------------------------------------------- 1 | // Ceres Solver - A fast non-linear least squares minimizer 2 | // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 | // http://code.google.com/p/ceres-solver/ 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // * Neither the name of Google Inc. nor the names of its contributors may be 14 | // used to endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // Author: keir@google.com (Keir Mierle) 30 | // 31 | // This is a forwarding header containing the public symbols exported from 32 | // Ceres. Anything in the "ceres" namespace is available for use. 33 | 34 | #ifndef CERES_PUBLIC_CERES_H_ 35 | #define CERES_PUBLIC_CERES_H_ 36 | 37 | #define CERES_VERSION 1.8.0 38 | #define CERES_ABI_VERSION 1.8.0 39 | 40 | #include "ceres/autodiff_cost_function.h" 41 | #include "ceres/autodiff_local_parameterization.h" 42 | #include "ceres/cost_function.h" 43 | #include "ceres/cost_function_to_functor.h" 44 | #include "ceres/covariance.h" 45 | #include "ceres/crs_matrix.h" 46 | #include "ceres/dynamic_autodiff_cost_function.h" 47 | #include "ceres/dynamic_numeric_diff_cost_function.h" 48 | #include "ceres/iteration_callback.h" 49 | #include "ceres/jet.h" 50 | #include "ceres/local_parameterization.h" 51 | #include "ceres/loss_function.h" 52 | #include "ceres/numeric_diff_cost_function.h" 53 | #include "ceres/numeric_diff_functor.h" 54 | #include "ceres/ordered_groups.h" 55 | #include "ceres/problem.h" 56 | #include "ceres/sized_cost_function.h" 57 | #include "ceres/solver.h" 58 | #include "ceres/types.h" 59 | 60 | #endif // CERES_PUBLIC_CERES_H_ 61 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/libs/precompiled/ceres/include/ceres/internal/port.h: -------------------------------------------------------------------------------- 1 | // Ceres Solver - A fast non-linear least squares minimizer 2 | // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 | // http://code.google.com/p/ceres-solver/ 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // * Neither the name of Google Inc. nor the names of its contributors may be 14 | // used to endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // Author: keir@google.com (Keir Mierle) 30 | 31 | #ifndef CERES_PUBLIC_INTERNAL_PORT_H_ 32 | #define CERES_PUBLIC_INTERNAL_PORT_H_ 33 | 34 | #include 35 | 36 | namespace ceres { 37 | 38 | // It is unfortunate that this import of the entire standard namespace is 39 | // necessary. The reasons are historical and won't be explained here, but 40 | // suffice to say it is not a mistake and can't be removed without breaking 41 | // things outside of the Ceres optimization package. 42 | using namespace std; 43 | 44 | // This is necessary to properly handle the case that there is a different 45 | // "string" implementation in the global namespace. 46 | using std::string; 47 | 48 | } // namespace ceres 49 | 50 | #endif // CERES_PUBLIC_INTERNAL_PORT_H_ 51 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/serialization.h: -------------------------------------------------------------------------------- 1 | #ifndef I3D_LINE3D_PP_SERIALIZATION_H_ 2 | #define I3D_LINE3D_PP_SERIALIZATION_H_ 3 | 4 | /* 5 | Line3D++ - Line-based Multi View Stereo 6 | Copyright (C) 2015 Manuel Hofer 7 | 8 | This program 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 | This program 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 this program. If not, see . 20 | */ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | /** 35 | * Line3D++ - Serialization 36 | * ==================== 37 | * Handles serialization of view- and 38 | * matching-data to the hard drive 39 | * using boost. 40 | * ==================== 41 | * Author: M.Hofer, 2015 42 | */ 43 | 44 | namespace L3DPP 45 | { 46 | // serialization 47 | template 48 | inline void 49 | serializeToFile(std::string file, T const& data, bool binary = true) 50 | { 51 | std::ofstream os(file.c_str(), binary? std::ios::binary : std::ios::out); 52 | boost::archive::binary_oarchive ar(os); 53 | ar & boost::serialization::make_nvp("data", data); 54 | } 55 | 56 | template 57 | inline void 58 | serializeFromFile(std::string file, T& data, bool binary = true) 59 | { 60 | std::ifstream is(file.c_str(), binary? std::ios::binary : std::ios::in); 61 | if(is.bad()) { 62 | std::cout << "[L3D++] serializeFromFileArchive(): File '" << file << "'' could not be opened " << std::endl; 63 | exit(1); 64 | } 65 | boost::archive::binary_iarchive ar(is); 66 | ar & boost::serialization::make_nvp("data", data); 67 | } 68 | } 69 | 70 | #endif //I3D_LINE3D_PP_SERIALIZATION_H_ 71 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/sparsematrix.h: -------------------------------------------------------------------------------- 1 | #ifndef I3D_LINE3D_PP_SPARSEMATRIX_H_ 2 | #define I3D_LINE3D_PP_SPARSEMATRIX_H_ 3 | 4 | /* 5 | Line3D++ - Line-based Multi View Stereo 6 | Copyright (C) 2015 Manuel Hofer 7 | 8 | This program 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 | This program 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 this program. If not, see . 20 | */ 21 | 22 | // check for CUDA 23 | #include "configLIBS.h" 24 | 25 | #ifdef L3DPP_CUDA 26 | 27 | // internal 28 | #include "clustering.h" 29 | #include "dataArray.h" 30 | 31 | /** 32 | * Line3D++ - Sparsematrix 33 | * ==================== 34 | * Sparse GPU matrix. 35 | * ==================== 36 | * Author: M.Hofer, 2015 37 | */ 38 | 39 | namespace L3DPP 40 | { 41 | class SparseMatrix 42 | { 43 | public: 44 | SparseMatrix(std::list& entries, const unsigned int num_rows_cols, 45 | const float normalization_factor=1.0f, 46 | const bool sort_by_row=false, const bool already_sorted=false); 47 | SparseMatrix(SparseMatrix* M, const bool change_sorting=false); 48 | ~SparseMatrix(); 49 | 50 | // check element sorting 51 | bool isRowSorted(){ 52 | return row_sorted_; 53 | } 54 | bool isColSorted(){ 55 | return !row_sorted_; 56 | } 57 | 58 | // data access 59 | unsigned int num_rows_cols(){ 60 | return num_rows_cols_; 61 | } 62 | unsigned int num_entries(){ 63 | return num_entries_; 64 | } 65 | 66 | // CPU/GPU data 67 | L3DPP::DataArray* entries(){ 68 | return entries_; 69 | } 70 | L3DPP::DataArray* start_indices(){ 71 | return start_indices_; 72 | } 73 | 74 | // download entries to CPU 75 | void download(){ 76 | entries_->download(); 77 | } 78 | 79 | private: 80 | // CPU/GPU data 81 | L3DPP::DataArray* entries_; 82 | L3DPP::DataArray* start_indices_; 83 | 84 | bool row_sorted_; 85 | unsigned int num_rows_cols_; 86 | unsigned int num_entries_; 87 | }; 88 | } 89 | 90 | #endif //L3DPP_CUDA 91 | 92 | #endif //I3D_LINE3D_PP_SPARSEMATRIX_H_ 93 | -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/teaser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/teaser.png -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/Line3D++_ref/Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__OPTIMIZED__vis_3.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/Line3D++_ref/Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__OPTIMIZED__vis_3.bin -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/Line3D++_ref/Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__vis_3.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/Line3D++_ref/Line3D++__W_FULL__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__vis_3.bin -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000055.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000055.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000056.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000056.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000057.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000057.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000058.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000058.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000059.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000059.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000060.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000060.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000061.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000061.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000062.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000062.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000063.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000063.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000064.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000064.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000065.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000065.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000066.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000066.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000067.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000067.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000068.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000068.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000069.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000069.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000070.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000070.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000071.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000071.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000072.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000072.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000073.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000073.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000074.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000074.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000075.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000075.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000076.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000076.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000077.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000077.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000078.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000078.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000079.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000079.jpg -------------------------------------------------------------------------------- /Thirdparty/Line3Dpp/testdata/img000080.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Thirdparty/Line3Dpp/testdata/img000080.jpg -------------------------------------------------------------------------------- /Thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /Thirdparty/g2o/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.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/Vocabulary/ORBvoc.bin -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/DBoW2 ..." 2 | 3 | cd Thirdparty/DBoW2 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../g2o 10 | 11 | echo "Configuring and building Thirdparty/g2o ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../Line3Dpp 19 | 20 | echo "Configuring and building Thirdparty/Line3Dpp ..." 21 | 22 | mkdir build 23 | cd build 24 | cmake .. -DCMAKE_BUILD_TYPE=Release 25 | make -j 26 | 27 | #cd ../../../ 28 | echo "Uncompress yolo_txts ..." 29 | cd ../../../data 30 | tar -xf yolo_txts.tar.gz 31 | cd .. 32 | 33 | # echo "Uncompress vocabulary ..." 34 | 35 | # cd Vocabulary 36 | # tar -xf ORBvoc.txt.tar.gz 37 | # cd .. 38 | 39 | echo "Configuring and building ORB_SLAM2 ..." 40 | 41 | mkdir build 42 | cd build 43 | cmake .. -DCMAKE_BUILD_TYPE=Release 44 | make -j 2 45 | 46 | # cd .. 47 | # echo "Converting vocabulary to binary" 48 | # ./tools/bin_vocabulary 49 | -------------------------------------------------------------------------------- /cubeslam_results/cube_slam.md: -------------------------------------------------------------------------------- 1 | + Comparison with CubeSLAM (only single view): you can make `bCubeslam = true` in `Tracking.cc`, and see the result in `cubeslam_results` folder. -------------------------------------------------------------------------------- /data/yolo_txts.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/data/yolo_txts.tar.gz -------------------------------------------------------------------------------- /eval/README: -------------------------------------------------------------------------------- 1 | These are scripts used to calculate the average distance of vertices to ground truth. 2 | 3 | First please convert all .obj files to .ply in order to use these scripts. ply files can be correctly read by MATLAB. Meshlab can be used to convert .obj files to .ply files. 4 | 5 | Ground truth point cloud file data.ply can be found in PATH_TO_SEQUENCE/mav0/pointcloud0 for sequences from EuRoC MAV dataset. Please put your results and ground truth point cloud into the same directory (e.g. result_euroc_v201/) to use these scripts. 6 | 7 | Run downsample.m, register.m, and evaluate.m in order. Please change the directory names and file names in the scripts to process your results. 8 | 9 | downsample.m: down sample ground truth point cloud and semi-dense point cloud to reduce processing time. 10 | 11 | register.m: compute the correct transform and scale change between ground truth point cloud and semi-dense point cloud.The result is saved in file tforms.mat. ICP is used to estimate the euclidean transform. Scale is estimated by minimizing the distance after running ICP. 12 | Note: initial scale and initial transform are critical to correctly estimating the transform. They may need to be changed when running on different data. If you change the initial transform, please also change the initial transform in file scaling.m. 13 | Note: please inspect visually that semi_test.ply has the correct transform, i.e., it has large overlap with the ground truth point cloud. 14 | 15 | evaluate.m: use the estimated transform to evaluate the result of other files. Results are printed on screen. 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /eval/downsample.m: -------------------------------------------------------------------------------- 1 | clearvars 2 | close all 3 | clc 4 | 5 | result_dir = 'result_euroc_v201'; 6 | 7 | downsample_rate = 0.1; 8 | 9 | ptCloud = pcread(strcat(result_dir,'/data.ply')); 10 | ptCloudOut = pcdownsample(ptCloud,'random',downsample_rate); 11 | figure 12 | pcshow(ptCloudOut); 13 | title('Ground Truth'); 14 | 15 | semidense = pcread(strcat(result_dir,'/semi_pointcloud.obj.ply')); 16 | semidenseOut = pcdownsample(semidense,'random',downsample_rate); 17 | figure 18 | pcshow(semidenseOut); 19 | title('Semidense Pointcloud'); 20 | 21 | pcwrite(ptCloudOut, strcat(result_dir,'/data_down.ply')); 22 | pcwrite(semidenseOut,strcat(result_dir,'/semi_down.ply')); 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /eval/euclideanDistanceTwoPointClouds.m: -------------------------------------------------------------------------------- 1 | function distMat = euclideanDistanceTwoPointClouds(reference,sample) 2 | % calculate the euclidean distance for every point in sample point cloud to 3 | % the closest point in the reference point cloud (number of columns must 4 | % match) 5 | 6 | % INPUT: 7 | % reference = M x N matrix 8 | % sample = P x N matrix 9 | 10 | % OUTPUT: 11 | % distMat = P x N matrix 12 | 13 | distMat = zeros(size(sample,1),1); 14 | for row_idx = 1:size(sample,1) 15 | diff = reference-repmat(sample(row_idx,:),size(reference,1),1); 16 | D = sqrt(sum(diff.^2,2)); 17 | distMat(row_idx) = min(D); 18 | end 19 | -------------------------------------------------------------------------------- /eval/evaluate.m: -------------------------------------------------------------------------------- 1 | clearvars 2 | close all 3 | clc 4 | 5 | % evaluate line segment result of euroc vicon dataset 6 | result_dir = 'result_euroc_v201'; 7 | 8 | load(strcat(result_dir,'/tforms.mat')) 9 | 10 | data = pcread(strcat(result_dir,'/data.ply')); 11 | 12 | line3d_load = pcread(strcat(result_dir,'/Line3D++__W_1500__N_10__sigmaP_2.5__sigmaA_10__epiOverlap_0.25__kNN_10__OPTIMIZED__vis_3.obj.ply')); 13 | long_load = pcread(strcat(result_dir,'/line_segments_edlines.obj.ply')); 14 | short_load = pcread(strcat(result_dir,'/line_segments.obj.ply')); 15 | short_c_load = pcread(strcat(result_dir,'/line_segments_clustered_incr.obj.ply')); 16 | 17 | % scaling 18 | line3d_points = line3d_load.Location * scaleMin; 19 | line3d = pointCloud(line3d_points); 20 | 21 | long_points = long_load.Location * scaleMin; 22 | long = pointCloud(long_points); 23 | 24 | short_points = short_load.Location * scaleMin; 25 | short = pointCloud(short_points); 26 | 27 | short_c_points = short_c_load.Location * scaleMin; 28 | short_c = pointCloud(short_c_points); 29 | 30 | % apply transform 31 | line3d = pctransform(line3d, tform1); 32 | line3d = pctransform(line3d, tform2); 33 | line3d = pctransform(line3d, tform3); 34 | 35 | long = pctransform(long, tform1); 36 | long = pctransform(long, tform2); 37 | long = pctransform(long, tform3); 38 | 39 | short = pctransform(short, tform1); 40 | short = pctransform(short, tform2); 41 | short = pctransform(short, tform3); 42 | 43 | short_c = pctransform(short_c, tform1); 44 | short_c = pctransform(short_c, tform2); 45 | short_c = pctransform(short_c, tform3); 46 | 47 | % compute distance to closest ground truth point: brute force 48 | line3d_dist = euclideanDistanceTwoPointClouds(data.Location,line3d.Location); 49 | long_dist = euclideanDistanceTwoPointClouds(data.Location,long.Location); 50 | short_dist = euclideanDistanceTwoPointClouds(data.Location,short.Location); 51 | short_c_dist = euclideanDistanceTwoPointClouds(data.Location,short_c.Location); 52 | 53 | 54 | fprintf('avgDist line3d: %f\n',mean(line3d_dist)); 55 | fprintf('avgDist long: %f\n',mean(long_dist)); 56 | fprintf('avgDist short: %f\n',mean(short_dist)); 57 | fprintf('avgDist short_c: %f\n',mean(short_c_dist)); 58 | 59 | 60 | -------------------------------------------------------------------------------- /eval/register.m: -------------------------------------------------------------------------------- 1 | clearvars 2 | close all 3 | clc 4 | 5 | result_dir = 'result_euroc_v201'; 6 | 7 | init_scale = 2.500; 8 | 9 | ptCloudOut = pcread(strcat(result_dir,'/data_down.ply')); 10 | semidenseOut = pcread(strcat(result_dir,'/semi_down.ply')); 11 | 12 | fun = @(scale)scaling(ptCloudOut,semidenseOut,scale); 13 | 14 | scaleMin = fminsearch(fun, init_scale); 15 | 16 | xyzpoints = semidenseOut.Location * scaleMin; 17 | 18 | semidenseScale = pointCloud(xyzpoints); 19 | 20 | % init transform 21 | % around x axis 22 | A = [1 0 0 0; ... 23 | 0 cos(pi/2) -sin(pi/2) 0; ... 24 | 0 sin(pi/2) cos(pi/2) 0; ... 25 | 0 0 0 1]; 26 | tform1 = affine3d(A); 27 | semidenseScale = pctransform(semidenseScale,tform1); 28 | 29 | % around z axis 30 | B = [cos(pi/2) sin(pi/2) 0 0; ... 31 | -sin(pi/2) cos(pi/2) 0 0; ... 32 | 0 0 1 0; ... 33 | 0 0 0 1]; 34 | tform2 = affine3d(B); 35 | semidenseScale = pctransform(semidenseScale,tform2); 36 | 37 | pcwrite(semidenseScale,strcat(result_dir,'/semi_init.ply')); 38 | 39 | [tform3,semiTransform,rmse] = pcregrigid(semidenseScale,ptCloudOut,'Extrapolate',true); 40 | 41 | pcwrite(semiTransform,strcat(result_dir,'/semi_transform.ply')); 42 | 43 | semiTest = pctransform(semidenseScale, tform3); 44 | pcwrite(semiTest,strcat(result_dir,'/semi_test.ply')); 45 | 46 | save(strcat(result_dir,'/tforms'),'scaleMin','tform1','tform2','tform3'); 47 | 48 | 49 | -------------------------------------------------------------------------------- /eval/scaling.m: -------------------------------------------------------------------------------- 1 | function rmse = scaling(ptCloud, semidense, scale) 2 | 3 | xyzpoints = semidense.Location * scale; 4 | 5 | semidenseScale = pointCloud(xyzpoints); 6 | 7 | % init transform 8 | % around x axis 9 | A = [1 0 0 0; ... 10 | 0 cos(pi/2) -sin(pi/2) 0; ... 11 | 0 sin(pi/2) cos(pi/2) 0; ... 12 | 0 0 0 1]; 13 | tform1 = affine3d(A); 14 | semidenseScale = pctransform(semidenseScale,tform1); 15 | 16 | % around z axis 17 | B = [cos(pi/2) sin(pi/2) 0 0; ... 18 | -sin(pi/2) cos(pi/2) 0 0; ... 19 | 0 0 1 0; ... 20 | 0 0 0 1]; 21 | tform2 = affine3d(B); 22 | semidenseScale = pctransform(semidenseScale,tform2); 23 | 24 | 25 | [tform,semiTransform,rmse] = pcregrigid(semidenseScale,ptCloud,'Extrapolate',true); 26 | 27 | display(scale) 28 | display(rmse) 29 | -------------------------------------------------------------------------------- /figures/data_association.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/figures/data_association.jpg -------------------------------------------------------------------------------- /figures/scale_and_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/figures/scale_and_orientation.png -------------------------------------------------------------------------------- /figures/tum_fr3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanmin-wu/EAO-SLAM/3145e877e2934b5f67ff7be4da43f66185850798/figures/tum_fr3.png -------------------------------------------------------------------------------- /include/CARV/Exception.h: -------------------------------------------------------------------------------- 1 | #ifndef __DLOVI_EXCEPTION_H 2 | #define __DLOVI_EXCEPTION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace dlovi{ 8 | class Exception : public std::exception{ 9 | public: 10 | // Constructors and Destructors 11 | Exception(const std::string & strMessage); 12 | virtual ~Exception() throw(); 13 | 14 | // Public Methods 15 | virtual const char * what() const throw(); 16 | virtual void raise(); 17 | void tag(const std::string & strClass, const std::string & strFunction) throw (); 18 | void tag(const std::string & strFunction) throw (); 19 | 20 | protected: 21 | // Members 22 | std::string m_strMessage; 23 | mutable std::string m_strFormattedMessage; 24 | bool m_bTagged; 25 | }; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /include/CARV/GraphWrapper_Boost.h: -------------------------------------------------------------------------------- 1 | #ifndef __GRAPHWRAPPER_BOOST_H 2 | #define __GRAPHWRAPPER_BOOST_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace dlovi { 11 | using namespace std; 12 | using namespace boost; 13 | 14 | class GraphWrapper_Boost; 15 | class GraphWrapper_Boost { 16 | public: 17 | // Typedefs and Types 18 | typedef adjacency_list_traits Traits; 19 | typedef adjacency_list > > > >, 25 | 26 | property < edge_capacity_t, double, 27 | property < edge_residual_capacity_t, double, 28 | property < edge_reverse_t, Traits::edge_descriptor > > > > Graph; 29 | 30 | 31 | // Constructors and Destructors 32 | GraphWrapper_Boost(); 33 | GraphWrapper_Boost(int nVertices, int nEdges = 0); 34 | ~GraphWrapper_Boost(); 35 | 36 | // Getters 37 | bool whatSegment(int node) const; // Returns true if source segment (vertex color is black), otherwise false 38 | int getSource() const; 39 | int getSink() const; 40 | 41 | // Public Methods 42 | void addNodes(int numVertices); 43 | void addSource(); 44 | void addSink(); 45 | void addTWeights(int node, double sourceWeight, double sinkWeight); 46 | void addEdge(int node1, int node2, double weight, double revWeight = 0.0); 47 | double maxflow(); 48 | void print() const; 49 | private: 50 | // Private Members 51 | Graph m_g; 52 | Traits::vertex_descriptor m_s, m_t; 53 | property_map::type m_capacity; 54 | property_map::type m_color; 55 | property_map::type m_rev; 56 | }; 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /include/CARV/SFMTranscriptInterface_ORBSLAM.h: -------------------------------------------------------------------------------- 1 | #ifndef __SFMTRANSCRIPTINTERFACE_ORBSLAM_H 2 | #define __SFMTRANSCRIPTINTERFACE_ORBSLAM_H 3 | 4 | #include "CARV/SFMTranscript.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace ORB_SLAM2 { 11 | class KeyFrame; 12 | class MapPoint; 13 | } 14 | class Cluster; 15 | 16 | class SFMTranscriptInterface_ORBSLAM; 17 | class SFMTranscriptInterface_ORBSLAM{ 18 | public: 19 | // Constructors and Destructors 20 | SFMTranscriptInterface_ORBSLAM(); 21 | ~SFMTranscriptInterface_ORBSLAM(); 22 | 23 | // Getters 24 | dlovi::compvis::SFMTranscript * getTranscriptRef(); 25 | dlovi::compvis::SFMTranscript * getTranscriptToProcessRef(); 26 | 27 | // Public Methods 28 | void addResetEntry(); 29 | void addPointDeletionEntry(ORB_SLAM2::MapPoint *p); 30 | void addVisibilityRayInsertionEntry(ORB_SLAM2::KeyFrame *k, ORB_SLAM2::MapPoint *p); 31 | void addVisibilityRayDeletionEntry(ORB_SLAM2::KeyFrame *k, ORB_SLAM2::MapPoint *p); 32 | void addFirstKeyFrameInsertionEntry(ORB_SLAM2::KeyFrame *k); 33 | void addKeyFrameInsertionEntry(ORB_SLAM2::KeyFrame *k); 34 | 35 | void addSemiDenseKeyFrameInsertionEntry(ORB_SLAM2::KeyFrame *k); 36 | void addLineSegmentInsertionEntry(ORB_SLAM2::KeyFrame *kf, Cluster *pCluster); 37 | void addLineSegmentKeyFrameInsertionEntry(ORB_SLAM2::KeyFrame *kf); 38 | 39 | void addBundleAdjustmentEntry(std::set & sAdjustSet, std::set & sMapPoints); 40 | void writeToFile(const std::string & strFileName) const; 41 | void suppressBundleAdjustmentLogging(); 42 | void unsuppressBundleAdjustmentLogging(); 43 | void suppressRefindLogging(); 44 | void unsuppressRefindLogging(); 45 | 46 | void UpdateTranscriptToProcess(); 47 | 48 | std::vector GetNewPoints(ORB_SLAM2::KeyFrame *pKF); 49 | 50 | private: 51 | // Member Variables 52 | dlovi::compvis::SFMTranscript m_SFMTranscript; 53 | 54 | // transcript that is guarded by mutex 55 | dlovi::compvis::SFMTranscript m_SFMTranscriptToProcess; 56 | 57 | bool m_bSuppressRefindLogging; 58 | bool m_bSuppressBundleAdjustmentLogging; 59 | 60 | std::map m_mMapPoint_Index; // TODO: possibly change to hashed? 61 | std::map m_mKeyFrame_Index; // TODO: possibly change to hashed? 62 | // Store ID instead of pointer 63 | std::map m_mFrame_Index; 64 | 65 | // correspondence between added keyframes and mappoints 66 | std::map> m_mKeyFrame_MapPoint; 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /include/CARV/StringFunctions.h: -------------------------------------------------------------------------------- 1 | #ifndef __DLOVI_STRINGFUNCTIONS_H 2 | #define __DLOVI_STRINGFUNCTIONS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dlovi{ 9 | namespace stringfunctions{ 10 | std::vector split(const std::string & strOriginal, const std::string & strDelimiter); 11 | std::string join(const std::vector & vStrArray, const std::string & strDelimiter); 12 | std::string trim(const std::string & strOriginal); 13 | } 14 | } 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /include/CARV/lapack_declarations.h: -------------------------------------------------------------------------------- 1 | #ifndef __LAPACK_DECLARATIONS_H 2 | #define __LAPACK_DECLARATIONS_H 3 | 4 | extern "C" { 5 | // Linear solvers 6 | // =============================================================================================== 7 | 8 | // Solution of square linear system(s) AX=B, by LU decomp. 9 | void dgesv_(long *n, long *nrhs, double *A, long *lda, long *iPiv, double *B, long *ldb, long *info); 10 | 11 | // Solves triangular systems AX=B or A**T * X = B 12 | void dtrtrs_(char *uplo, char *trans, char *diag, long *n, long *nrhs, double *A, long *lda, double *B, long *ldb, long *info); 13 | 14 | // Decompositions 15 | // =============================================================================================== 16 | 17 | // LU factorization of a general M-by-N matrix A using partial pivoting with row interchanges. 18 | void dgetrf_(long *m, long *n, double *a, long *lda, long *ipiv, long *info); 19 | 20 | // QR factorization, without pivoting. 21 | void dgeqrf_(long *m, long *n, double *A, long *lda, double *tau, double *work, long *lwork, long *info); 22 | 23 | // QR With Column Pivoting 24 | void dgeqp3_(long *m, long *n, double *A, long *lda, long *jpvt, double *tau, double *work, long *lwork, long *info); 25 | 26 | // SVD 27 | void dgesvd_(char *jobu, char *jobvt, long *m, long *n, double *A, long *lda, double *s, double *U, long *ldu, double *VT, long *ldvt, 28 | double *work, long *lwork, long *info); 29 | 30 | 31 | // Misc. Helper routines 32 | // =============================================================================================== 33 | 34 | // Fast orthogonal matrix w/ real matrix multiplication routine 35 | void dormqr_(char *side, char *trans, long *m, long *n, long *k, double *A, long *lda, double *tau, double *C, long *ldc, double *work, 36 | long *lwork, long *info); 37 | 38 | // Constructs orthogonal matrix from householder transformation vectors / scalars, as returned by LAPACK qr factorization routines 39 | void dorgqr_(long *m, long *n, long *k, double *A, long *lda, double *tau, double *work, long *lwork, long *info); 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /include/CARV/lovimath.h: -------------------------------------------------------------------------------- 1 | #ifndef __LOVIMATH_H 2 | #define __LOVIMATH_H 3 | 4 | #include 5 | #include 6 | 7 | //#include 8 | 9 | using namespace std; 10 | 11 | namespace dlovi{ 12 | //#ifndef EPS 13 | // #define EPS (10e-6) 14 | //#endif 15 | 16 | //#ifndef PI 17 | // #define PI (3.141592653589793238462643) 18 | //#endif 19 | 20 | // General mathematical constants 21 | const double pi = 3.141592653589793238462643; 22 | const double ln2 = 0.69314718055994530942; 23 | const double e = 2.718281828459045235360287; 24 | 25 | // Constants for golden section searches, etc 26 | const double golden_ratio = 1.6180339887498948482; 27 | const double golden_ratio_conjugate = 0.618033988749895; 28 | const double golden_section = 0.381966011250105; 29 | 30 | // Machine precision related constants 31 | const double eps_d = pow(2.0, -52.0); 32 | const double eps_f = pow(2.0, -23.0); 33 | const double sqrt_eps_d = 1.49011611938477e-08; 34 | const double sqrt_eps_f = 3.45266983001244e-04; 35 | const double realmin_d = ldexp(1.0, -1022); 36 | const double realmin_f = (float)ldexp(1.0, -126); 37 | 38 | // Constants related to Inf and NaN 39 | const double NaN = numeric_limits::quiet_NaN(); 40 | const double Inf = numeric_limits::infinity(); 41 | const double inf = numeric_limits::infinity(); 42 | 43 | // (Most) Functions Declarations 44 | int floatEquals(float, float); 45 | int doubleEquals(double, double); 46 | 47 | double eps(double x); 48 | float eps(float x); 49 | 50 | int round(double x); 51 | int round(float x); 52 | 53 | bool isNaN(double x); 54 | 55 | // Special functions 56 | double sinc(double x); 57 | 58 | // Templated functions: 59 | 60 | template 61 | T sign(const T x){ // computes the signum 62 | if(x > T(0)) 63 | return T(1); 64 | else if(x < T(0)) 65 | return T(-1); 66 | else 67 | return T(0); 68 | } 69 | 70 | template 71 | T replaceSign(const T a, const T b){ // returns a value with the same magnitude as a and the same sign as b. (if sign(b) == 0, sign of a unchanged) 72 | T nSignb = sign(b); 73 | T nSigna = sign(a); 74 | return a * nSigna * ( (nSignb == T(0)) ? nSigna : nSignb ); 75 | } 76 | } 77 | #endif 78 | 79 | -------------------------------------------------------------------------------- /include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 4 | * For more information see 5 | * 6 | * Modification: EAO-SLAM 7 | * Version: 1.0 8 | * Created: 05/21/2019 9 | * Author: Yanmin Wu 10 | * E-mail: wuyanminmax@gmail.com 11 | */ 12 | 13 | #ifndef CONVERTER_H 14 | #define CONVERTER_H 15 | 16 | // opencv 17 | #include 18 | #include 19 | #include 20 | #include "opencv2/imgproc/imgproc.hpp" 21 | 22 | #include 23 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 24 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 25 | 26 | namespace ORB_SLAM2 27 | { 28 | 29 | class Converter 30 | { 31 | public: 32 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 33 | 34 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 35 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 36 | 37 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 38 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 39 | static cv::Mat toCvMat(const Eigen::Matrix &m); 40 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 41 | static cv::Mat toCvMat(const Eigen::Matrix &m); 42 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 43 | 44 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 45 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 46 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 47 | 48 | static std::vector toQuaternion(const cv::Mat &M); 49 | 50 | // NOTE [EAO-SLAM] 51 | static Eigen::MatrixXd toEigenMatrixXd(const cv::Mat &cvMat); 52 | static cv::Mat toCvMatInverse(const cv::Mat &Tcw); 53 | static cv::Mat toCvMat(const Eigen::Vector3f &PosEigen); 54 | static Eigen::Vector3f toEigenVector(const cv::Mat &PosMat); 55 | 56 | // NOTE [EAO-SLAM] compute IoU overlap ratio between two rectangles [x y w h] 57 | static float bboxOverlapratio(const cv::Rect& rect1, const cv::Rect& rect2); 58 | static float bboxOverlapratioFormer(const cv::Rect& rect1, const cv::Rect& rect2); 59 | static float bboxOverlapratioLatter(const cv::Rect& rect1, const cv::Rect& rect2); 60 | }; 61 | 62 | }// namespace ORB_SLAM 63 | 64 | #endif // CONVERTER_H 65 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 4 | * For more information see 5 | * 6 | * Modification: EAO-SLAM 7 | * Version: 1.0 8 | * Created: 07/18/2019 9 | * Author: Yanmin Wu 10 | * E-mail: wuyanminmax@gmail.com 11 | */ 12 | 13 | #ifndef MAP_H 14 | #define MAP_H 15 | 16 | #include "MapPoint.h" 17 | #include "KeyFrame.h" 18 | #include 19 | 20 | #include "Modeler.h" 21 | 22 | #include 23 | 24 | namespace ORB_SLAM2 25 | { 26 | 27 | class MapPoint; 28 | class KeyFrame; 29 | class Object_Map; 30 | 31 | class Map 32 | { 33 | public: 34 | Map(); 35 | 36 | void SetModeler(Modeler *pModeler); 37 | Modeler* GetModeler(); 38 | 39 | void AddKeyFrame(KeyFrame* pKF); 40 | void AddMapPoint(MapPoint* pMP); 41 | void EraseMapPoint(MapPoint* pMP); 42 | void EraseKeyFrame(KeyFrame* pKF); 43 | void SetReferenceMapPoints(const std::vector &vpMPs); 44 | 45 | // void AddObjectMapPoints(MapPoint* pMP); 46 | 47 | void UpdateModel(Model* pModel); 48 | Model* GetModel(); 49 | 50 | std::vector GetAllKeyFrames(); 51 | std::vector GetAllMapPoints(); 52 | std::vector GetReferenceMapPoints(); 53 | 54 | std::vector GetObjects(); 55 | std::vector GetCubeCenters(); 56 | 57 | long unsigned int MapPointsInMap(); 58 | long unsigned KeyFramesInMap(); 59 | 60 | long unsigned int GetMaxKFid(); 61 | 62 | void clear(); 63 | 64 | vector mvpKeyFrameOrigins; 65 | 66 | std::mutex mMutexMapUpdate; 67 | 68 | // This avoid that two points are created simultaneously in separate threads (id conflict) 69 | std::mutex mMutexPointCreation; 70 | 71 | vector mvObjectMap; // objects in the map. 72 | vector cube_center; 73 | 74 | protected: 75 | std::set mspMapPoints; 76 | std::set mspKeyFrames; 77 | 78 | std::vector mvpReferenceMapPoints; 79 | 80 | std::set mvpObjectMapPoints; 81 | 82 | long unsigned int mnMaxKFid; 83 | 84 | Model* mpModel; 85 | 86 | Modeler* mpModeler; 87 | 88 | std::mutex mMutexMap; 89 | }; 90 | 91 | } //namespace ORB_SLAM 92 | 93 | #endif // MAP_H 94 | -------------------------------------------------------------------------------- /include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 4 | * For more information see 5 | * 6 | * Modification: EAO-SLAM 7 | * Version: 1.0 8 | * Created: 11/23/2019 9 | * Author: Yanmin Wu 10 | * E-mail: wuyanminmax@gmail.com 11 | */ 12 | 13 | #ifndef MAPDRAWER_H 14 | #define MAPDRAWER_H 15 | 16 | #include"Map.h" 17 | #include"MapPoint.h" 18 | #include"KeyFrame.h" 19 | #include 20 | 21 | #include 22 | 23 | namespace ORB_SLAM2 24 | { 25 | 26 | class MapDrawer 27 | { 28 | public: 29 | MapDrawer(Map* pMap, const string &strSettingPath); 30 | 31 | Map* mpMap; 32 | 33 | void DrawSemiDense(const double sigma); 34 | void DrawModel(); 35 | void DrawTriangles(pangolin::OpenGlMatrix &Twc); 36 | void DrawFrame(); 37 | 38 | void DrawMapPoints(); 39 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 40 | 41 | // BRIEF [EAO-SLAM] draw objects. 42 | void DrawObject(const bool bCubeObj, const bool QuadricObj, 43 | const string &flag, 44 | const bool bShowBottle, const bool bShowChair, const bool bShowTvmonitors, 45 | const bool bShowKeyboard,const bool bShowMouse, const bool bShowBook, const bool bShowBear); 46 | 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 | 52 | private: 53 | float mKeyFrameSize; 54 | float mKeyFrameLineWidth; 55 | float mGraphLineWidth; 56 | float mPointSize; 57 | float mCameraSize; 58 | float mCameraLineWidth; 59 | 60 | cv::Mat mCameraPose; 61 | 62 | std::mutex mMutexCamera; 63 | }; 64 | 65 | } //namespace ORB_SLAM 66 | 67 | #endif // MAPDRAWER_H 68 | -------------------------------------------------------------------------------- /include/Modeler.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __MODELER_H 3 | #define __MODELER_H 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include "CARV/Matrix.h" 10 | #include "CARV/SFMTranscriptInterface_ORBSLAM.h" 11 | #include "CARV/SFMTranscriptInterface_Delaunay.h" 12 | 13 | class Cluster; 14 | 15 | namespace ORB_SLAM2 { 16 | class KeyFrame; 17 | class Map; 18 | class MapPoint; 19 | } 20 | 21 | 22 | class Model { 23 | public: 24 | Model(const vector & modelPoints, const list & modelTris); 25 | vector & GetPoints(); 26 | list & GetTris(); 27 | 28 | void SetNotErase(); 29 | void SetErase(); 30 | void Release(); 31 | 32 | private: 33 | std::mutex mMutexErase; 34 | bool mbNotErase; 35 | bool mbToBeErased; 36 | std::pair, std::list> mData; 37 | 38 | }; 39 | 40 | 41 | // interface class for surface reconstruction using CARV system 42 | class Modeler { 43 | public: 44 | Modeler(ORB_SLAM2::Map* pMap); 45 | 46 | void AddKeyFrameEntry(ORB_SLAM2::KeyFrame* pKF); 47 | void AddLineSegmentKeyFrameEntry(ORB_SLAM2::KeyFrame* pKF); 48 | 49 | bool CheckNewTranscriptEntry(); 50 | 51 | void RunRemainder(); 52 | void RunOnce(); 53 | void UpdateModel(); 54 | 55 | void WriteModel(std::string filename); 56 | 57 | public: 58 | //CARV interface 59 | SFMTranscriptInterface_ORBSLAM mTranscriptInterface; // An interface to a transcript / log of the map's work. 60 | //CARV runner instance 61 | dlovi::FreespaceDelaunayAlgorithm mObjAlgorithm; 62 | SFMTranscriptInterface_Delaunay mAlgInterface; // An encapsulation of the interface between the transcript and the surface inferring algorithm. 63 | 64 | ORB_SLAM2::Map* mpMap; 65 | 66 | // This avoid that two transcript entries are created simultaneously in separate threads 67 | std::mutex mMutexTranscript; 68 | 69 | //number of lines in transcript last time checked 70 | int mnLastNumLines; 71 | 72 | bool mbFirstKeyFrame; 73 | 74 | }; 75 | 76 | 77 | 78 | #endif //__MODELVIEWER_H 79 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | 30 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 31 | #include 32 | 33 | namespace ORB_SLAM2 34 | { 35 | 36 | class LoopClosing; 37 | 38 | class Optimizer 39 | { 40 | public: 41 | // void static ComputeObjsLine(Frame *pFrame); 42 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 43 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 44 | const bool bRobust = true); 45 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 46 | const unsigned long nLoopKF=0, const bool bRobust = true); 47 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 48 | int static PoseOptimization(Frame* pFrame); 49 | 50 | // int static PoseOptimizationByObjs(Frame* pFrame); 51 | 52 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 53 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 54 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 55 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 56 | const map > &LoopConnections, 57 | const bool &bFixScale); 58 | 59 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 60 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 61 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 62 | 63 | protected: 64 | std::mutex mMutexObj; 65 | }; 66 | 67 | } //namespace ORB_SLAM 68 | 69 | #endif // OPTIMIZER_H 70 | -------------------------------------------------------------------------------- /include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Tracking; 36 | class FrameDrawer; 37 | class MapDrawer; 38 | class System; 39 | 40 | class Viewer 41 | { 42 | public: 43 | Viewer( System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, 44 | Tracking *pTracking, const string &strSettingPath, const string &flag); 45 | 46 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 47 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 48 | void Run(); 49 | 50 | void RequestFinish(); 51 | 52 | void RequestStop(); 53 | 54 | bool isFinished(); 55 | 56 | bool isStopped(); 57 | 58 | void Release(); 59 | 60 | private: 61 | 62 | bool Stop(); 63 | 64 | System* mpSystem; 65 | FrameDrawer* mpFrameDrawer; 66 | MapDrawer* mpMapDrawer; 67 | Tracking* mpTracker; 68 | 69 | // 1/fps in ms 70 | double mT; 71 | float mImageWidth, mImageHeight; 72 | 73 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 74 | float mfx, mfy, mcx, mcy; 75 | 76 | bool CheckFinish(); 77 | void SetFinish(); 78 | bool mbFinishRequested; 79 | bool mbFinished; 80 | std::mutex mMutexFinish; 81 | 82 | bool mbStopped; 83 | bool mbStopRequested; 84 | std::mutex mMutexStop; 85 | 86 | // demo. 87 | string mflag; 88 | }; 89 | 90 | } 91 | 92 | 93 | #endif // VIEWER_H 94 | 95 | 96 | -------------------------------------------------------------------------------- /src/CARV/Exception.cpp: -------------------------------------------------------------------------------- 1 | #ifndef __DLOVI_EXCEPTION_CPP 2 | #define __DLOVI_EXCEPTION_CPP 3 | 4 | #include "CARV/Exception.h" 5 | #include 6 | #include 7 | 8 | namespace dlovi{ 9 | 10 | // Constructors and Destructors 11 | 12 | Exception::Exception(const std::string & strMessage) 13 | : exception(), m_strMessage(strMessage), m_bTagged(false) { if(m_strMessage.empty()) m_strMessage = "no message"; } 14 | 15 | Exception::~Exception() throw() { } 16 | 17 | // Public Methods: 18 | 19 | const char * Exception::what() const throw(){ 20 | try{ 21 | std::stringstream ssMessage; 22 | 23 | if(m_bTagged) 24 | ssMessage << "Exception in " << m_strMessage; 25 | else 26 | ssMessage << "Exception: " << m_strMessage; 27 | 28 | m_strFormattedMessage = ssMessage.str(); 29 | return m_strFormattedMessage.c_str(); 30 | } 31 | catch(std::exception & e){ 32 | std::cerr << "Warning in: dlovi::Exception::what(): Failed to format message. (msg = " << m_strMessage << ")" << std::endl; 33 | return "Exception: error formatting exception text"; 34 | } 35 | } 36 | 37 | void Exception::raise(){ 38 | throw *this; 39 | } 40 | 41 | void Exception::tag(const std::string & strClass, const std::string & strFunction) throw (){ 42 | try{ 43 | std::stringstream ssMessage; 44 | ssMessage << strClass << "::" << strFunction << "()" << (m_bTagged ? " -> " : ": ") << m_strMessage; 45 | m_strMessage = ssMessage.str(); 46 | m_bTagged = true; 47 | } 48 | catch(std::exception & e){ 49 | std::cerr << "Warning in: dlovi::Exception::tag(): Failed to tag. (class = " << strClass << ", func = " << strFunction << ")" << std::endl; 50 | } 51 | } 52 | 53 | void Exception::tag(const std::string & strFunction) throw (){ 54 | try{ 55 | std::stringstream ssMessage; 56 | ssMessage << strFunction << "()" << (m_bTagged ? " -> " : ": ") << m_strMessage; 57 | m_strMessage = ssMessage.str(); 58 | m_bTagged = true; 59 | } 60 | catch(std::exception & e){ 61 | std::cerr << "Warning in: dlovi::Exception::tag(): Failed to tag. (func = " << strFunction << ")" << std::endl; 62 | } 63 | } 64 | 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /src/CARV/GraphWrapper_Boost.cc: -------------------------------------------------------------------------------- 1 | #ifndef __GRAPHWRAPPER_BOOST_CPP 2 | #define __GRAPHWRAPPER_BOOST_CPP 3 | 4 | #include "CARV/GraphWrapper_Boost.h" 5 | 6 | namespace dlovi { 7 | 8 | // Constructors and Destructors 9 | 10 | GraphWrapper_Boost::GraphWrapper_Boost() { 11 | m_capacity = get(edge_capacity, m_g); 12 | m_color = get(vertex_color, m_g); 13 | m_rev = get(edge_reverse, m_g); 14 | } 15 | GraphWrapper_Boost::GraphWrapper_Boost(int nVertices, int nEdges) : m_g(nVertices) { 16 | m_capacity = get(edge_capacity, m_g); 17 | m_color = get(vertex_color, m_g); 18 | m_rev = get(edge_reverse, m_g); 19 | } 20 | GraphWrapper_Boost::~GraphWrapper_Boost() {} 21 | 22 | // Getters 23 | 24 | bool GraphWrapper_Boost::whatSegment(int node) const { 25 | return m_color[node] != m_color[m_t]; 26 | } 27 | int GraphWrapper_Boost::getSource() const { 28 | return m_s; 29 | } 30 | int GraphWrapper_Boost::getSink() const { 31 | return m_t; 32 | } 33 | 34 | // Public Methods 35 | 36 | void GraphWrapper_Boost::addNodes(int numVertices) { 37 | for(int i = 0; i < numVertices; ++i) 38 | add_vertex(m_g); 39 | } 40 | void GraphWrapper_Boost::addSource() { 41 | m_s = add_vertex(m_g); 42 | } 43 | void GraphWrapper_Boost::addSink() { 44 | m_t = add_vertex(m_g); 45 | } 46 | void GraphWrapper_Boost::addTWeights(int node, double sourceWeight, double sinkWeight) { 47 | addEdge(m_s, node, sourceWeight); 48 | addEdge(node, m_t, sinkWeight); 49 | } 50 | void GraphWrapper_Boost::addEdge(int node1, int node2, double weight, double revWeight) { 51 | Traits::edge_descriptor edge; 52 | Traits::edge_descriptor revEdge; 53 | bool bExists; 54 | 55 | tie(edge, bExists) = boost::edge(node1, node2, m_g); 56 | if (! bExists) { 57 | edge = add_edge(node1, node2, m_g).first; 58 | revEdge = add_edge(node2, node1, m_g).first; 59 | m_rev[edge] = revEdge; 60 | m_rev[revEdge] = edge; 61 | } 62 | else 63 | revEdge = m_rev[edge]; 64 | 65 | m_capacity[edge] += weight; 66 | m_capacity[revEdge] += revWeight; 67 | } 68 | double GraphWrapper_Boost::maxflow() { 69 | return boykov_kolmogorov_max_flow(m_g, m_s, m_t); 70 | } 71 | void GraphWrapper_Boost::print() const { 72 | graph_traits::vertex_iterator u_iter, u_end; 73 | graph_traits::out_edge_iterator ei, e_end; 74 | 75 | cout << "NumVertices = " << num_vertices(m_g) << endl; 76 | 77 | for (tie(u_iter, u_end) = vertices(m_g); u_iter != u_end; ++u_iter) { 78 | cout << "Vertex: " << *u_iter << " cut: " << (whatSegment(*u_iter) ? "source" : "sink") << endl; 79 | //cout << "Vertex: " << *u_iter << " color: " << color[*u_iter] << endl; 80 | for (tie(ei, e_end) = out_edges(*u_iter, m_g); ei != e_end; ++ei) { 81 | cout << "Edge: (" << *u_iter << ", " << target(*ei, m_g) << ")" << " capacity: " << m_capacity[*ei] << " rev: (" << source(m_rev[*ei], m_g) << ", " << target(m_rev[*ei], m_g) << ")" << endl; 82 | } 83 | } 84 | } 85 | } 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/CARV/StringFunctions.cpp: -------------------------------------------------------------------------------- 1 | #ifndef __DLOVI_STRINGFUNCTIONS_CPP 2 | #define __DLOVI_STRINGFUNCTIONS_CPP 3 | 4 | #include "CARV/StringFunctions.h" 5 | #include "CARV/Exception.h" 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace dlovi{ 11 | namespace stringfunctions{ 12 | std::vector split(const std::string & strOriginal, const std::string & strDelimiter){ 13 | vector retVal; 14 | try{ 15 | string::size_type nOriginalLength = strOriginal.length(); 16 | string::size_type nDelimiterLength = strDelimiter.length(); 17 | string::size_type nFirst = 0; 18 | string::size_type nSecond; 19 | 20 | while(nFirst < nOriginalLength){ 21 | nSecond = strOriginal.find(strDelimiter, nFirst); 22 | if(nSecond == string::npos){ 23 | retVal.push_back(strOriginal.substr(nFirst)); 24 | return retVal; 25 | } 26 | retVal.push_back(strOriginal.substr(nFirst, nSecond - nFirst)); 27 | nFirst = nSecond + nDelimiterLength; 28 | } 29 | 30 | return retVal; 31 | } 32 | catch(std::exception & ex){ 33 | dlovi::Exception ex2(ex.what()); ex2.tag("stringfunctions", "split"); ex2.raise(); 34 | } 35 | return retVal; 36 | } 37 | 38 | std::string join(const std::vector & vStrArray, const std::string & strDelimiter){ 39 | stringstream ssRetVal; 40 | try{ 41 | int nNumStrings = vStrArray.size(); 42 | 43 | if(nNumStrings > 0) 44 | ssRetVal << vStrArray[0]; 45 | for(int i = 1; i < (int)vStrArray.size(); i++) 46 | ssRetVal << strDelimiter << vStrArray[i]; 47 | 48 | return ssRetVal.str(); 49 | } 50 | catch(std::exception & ex){ 51 | dlovi::Exception ex2(ex.what()); ex2.tag("stringfunctions", "join"); ex2.raise(); 52 | return ssRetVal.str(); 53 | } 54 | } 55 | 56 | std::string trim(const std::string & strOriginal){ 57 | try{ 58 | int nFirstPos, nSecondPos; 59 | const int nLastPos = strOriginal.size() - 1; 60 | 61 | // find first non-whitespace position 62 | for(nFirstPos = 0; nFirstPos <= nLastPos && std::isspace((unsigned char)strOriginal[nFirstPos]); nFirstPos++); 63 | //find last non-whitespace position 64 | for(nSecondPos = nLastPos; nSecondPos >= 0 && std::isspace((unsigned char)strOriginal[nSecondPos]); nSecondPos--); 65 | 66 | if(nSecondPos >= nFirstPos) //if the string has non-whitespace characters then return the substring 67 | return strOriginal.substr(nFirstPos, nSecondPos - nFirstPos + 1); 68 | return std::string(); // otherwise return empty string 69 | } 70 | catch(std::exception & ex){ 71 | dlovi::Exception ex2(ex.what()); ex2.tag("stringfunctions", "trim"); ex2.raise(); 72 | return std::string(); 73 | } 74 | } 75 | } 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /src/CARV/lovimath.cc: -------------------------------------------------------------------------------- 1 | #ifndef __LOVIMATH_CPP 2 | #define __LOVIMATH_CPP 3 | 4 | #include "CARV/lovimath.h" 5 | 6 | namespace dlovi{ 7 | 8 | int floatEquals(float x, float y){ 9 | return fabs(x - y) < eps_f; 10 | } 11 | 12 | int doubleEquals(double x, double y){ 13 | return fabs(x - y) < eps_d; 14 | } 15 | 16 | double eps(double x){ 17 | if (fabs(x) <= realmin_d) 18 | return pow(2.0, -1074.0); 19 | else{ 20 | //2^E <= abs(X) < 2^(E+1) --> E <= log_2(abs(X)) < E+1.... 21 | //true iff E = floor(log_2(abs(X))). Floor can be done by casting //to int. 22 | //so...... 23 | //int E = (int)log_2(fabs(x)); 24 | 25 | return pow(2.0, (double)((int)(log(fabs(x)/ln2)) - 52)); 26 | } 27 | } 28 | 29 | float eps(float x){ 30 | if (fabs(x) <= realmin_f) 31 | return pow(2.0, -149.0); 32 | else{ 33 | //2^E <= abs(X) < 2^(E+1) --> E <= log_2(abs(X)) < E+1.... 34 | //true iff E = floor(log_2(abs(X))). Floor can be done by casting //to int. 35 | //so...... 36 | //int E = (int)log_2(fabs(x)); 37 | 38 | return (float)pow(2.0, (double)((int)(log(fabs(x)/ln2)) - 23)); 39 | } 40 | } 41 | 42 | int round(double x){ 43 | if(x >= 0.0) 44 | return (int)(x + 0.5); 45 | return (int)(x - 0.5); 46 | } 47 | 48 | int round(float x){ 49 | if(x >= 0.0f) 50 | return (int)(x + 0.5f); 51 | return (int)(x - 0.5f); 52 | } 53 | 54 | bool isNaN(double x){ 55 | return x != x; 56 | } 57 | 58 | double sinc(double x){ 59 | if(x == 0.0) 60 | return 1; 61 | return sin(x) / x; 62 | } 63 | } 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /src/line_detect/libs/types.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #if defined _MSC_VER && _MSC_VER <= 1700 44 | #include 45 | #else 46 | #include 47 | #endif 48 | 49 | #ifndef __OPENCV_TYPES_HPP 50 | #define __OPENCV_TYPES_HPP 51 | 52 | #ifdef _MSC_VER 53 | #pragma warning( disable : 4267 ) 54 | #endif 55 | 56 | /* define data types */ 57 | typedef uint64_t UINT64; 58 | typedef uint32_t UINT32; 59 | typedef uint16_t UINT16; 60 | typedef uint8_t UINT8; 61 | 62 | /* define constants */ 63 | #define UINT64_1 ((UINT64)0x01) 64 | #define UINT32_1 ((UINT32)0x01) 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /tools/bin_vocabulary.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ORBVocabulary.h" 4 | using namespace std; 5 | 6 | bool load_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) { 7 | clock_t tStart = clock(); 8 | bool res = voc->loadFromTextFile(infile); 9 | printf("Loading fom text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); 10 | return res; 11 | } 12 | 13 | void load_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) { 14 | clock_t tStart = clock(); 15 | voc->load(infile); 16 | printf("Loading fom xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); 17 | } 18 | 19 | void load_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) { 20 | clock_t tStart = clock(); 21 | voc->loadFromBinaryFile(infile); 22 | printf("Loading fom binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); 23 | } 24 | 25 | void save_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) { 26 | clock_t tStart = clock(); 27 | voc->save(outfile); 28 | printf("Saving as xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); 29 | } 30 | 31 | void save_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) { 32 | clock_t tStart = clock(); 33 | voc->saveToTextFile(outfile); 34 | printf("Saving as text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); 35 | } 36 | 37 | void save_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) { 38 | clock_t tStart = clock(); 39 | voc->saveToBinaryFile(outfile); 40 | printf("Saving as binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); 41 | } 42 | 43 | 44 | int main(int argc, char **argv) { 45 | cout << "BoW load/save benchmark" << endl; 46 | ORB_SLAM2::ORBVocabulary* voc = new ORB_SLAM2::ORBVocabulary(); 47 | 48 | load_as_text(voc, "Vocabulary/ORBvoc.txt"); 49 | save_as_binary(voc, "Vocabulary/ORBvoc.bin"); 50 | 51 | return 0; 52 | } 53 | --------------------------------------------------------------------------------