├── .DS_Store ├── README.md ├── orbslamios.xcodeproj ├── project.pbxproj ├── project.xcworkspace │ ├── contents.xcworkspacedata │ └── xcuserdata │ │ └── Yinggx.xcuserdatad │ │ └── UserInterfaceState.xcuserstate └── xcuserdata │ └── Yinggx.xcuserdatad │ ├── xcdebugger │ └── Breakpoints_v2.xcbkptlist │ └── xcschemes │ ├── orbslamios.xcscheme │ └── xcschememanagement.plist ├── orbslamios ├── .DS_Store ├── AppDelegate.h ├── AppDelegate.m ├── Assets.xcassets │ └── AppIcon.appiconset │ │ └── Contents.json ├── Base.lproj │ ├── LaunchScreen.storyboard │ └── Main.storyboard ├── Info.plist ├── ORB_SLAM │ ├── .DS_Store │ ├── Thirdparty │ │ ├── .DS_Store │ │ ├── DBoW2 │ │ │ ├── .DS_Store │ │ │ ├── CMakeLists.txt │ │ │ ├── DBoW2 │ │ │ │ ├── BowVector.cpp │ │ │ │ ├── BowVector.h │ │ │ │ ├── FClass.h │ │ │ │ ├── FORB.cpp │ │ │ │ ├── FORB.h │ │ │ │ ├── FeatureVector.cpp │ │ │ │ ├── FeatureVector.h │ │ │ │ ├── LICENSE.txt │ │ │ │ ├── ScoringObject.cpp │ │ │ │ ├── ScoringObject.h │ │ │ │ └── TemplatedVocabulary.h │ │ │ ├── DUtils │ │ │ │ ├── LICENSE.txt │ │ │ │ ├── Random.cpp │ │ │ │ ├── Random.h │ │ │ │ ├── Timestamp.cpp │ │ │ │ └── Timestamp.h │ │ │ └── LICENSE.txt │ │ └── g2o │ │ │ ├── .DS_Store │ │ │ ├── config.h │ │ │ └── g2o │ │ │ ├── .DS_Store │ │ │ ├── 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 │ ├── include │ │ ├── Converter.h │ │ ├── Frame.h │ │ ├── FrameDrawer.h │ │ ├── Initializer.h │ │ ├── KeyFrame.h │ │ ├── KeyFrameDatabase.h │ │ ├── LocalMapping.h │ │ ├── LoopClosing.h │ │ ├── Map.h │ │ ├── MapDrawer.h │ │ ├── MapPoint.h │ │ ├── ORBVocabulary.h │ │ ├── ORBextractor.h │ │ ├── ORBmatcher.h │ │ ├── Optimizer.h │ │ ├── PnPsolver.h │ │ ├── Sim3Solver.h │ │ ├── System.h │ │ ├── Tracking.h │ │ └── Viewer.h │ └── src │ │ ├── Converter.cc │ │ ├── Frame.cc │ │ ├── FrameDrawer.cc │ │ ├── Initializer.cc │ │ ├── KeyFrame.cc │ │ ├── KeyFrameDatabase.cc │ │ ├── LocalMapping.cc │ │ ├── LoopClosing.cc │ │ ├── Map.cc │ │ ├── MapDrawer.cc │ │ ├── MapPoint.cc │ │ ├── ORBextractor.cc │ │ ├── ORBmatcher.cc │ │ ├── Optimizer.cc │ │ ├── PnPsolver.cc │ │ ├── Sim3Solver.cc │ │ ├── System.cc │ │ ├── Tracking.cc │ │ └── Viewer.cc ├── ViewController.h ├── ViewController.mm ├── eigen │ ├── .DS_Store │ └── Eigen │ │ ├── .DS_Store │ │ ├── Array │ │ ├── CMakeLists.txt │ │ ├── Cholesky │ │ ├── CholmodSupport │ │ ├── Core │ │ ├── Dense │ │ ├── Eigen │ │ ├── Eigen2Support │ │ ├── Eigenvalues │ │ ├── Geometry │ │ ├── Householder │ │ ├── IterativeLinearSolvers │ │ ├── Jacobi │ │ ├── LU │ │ ├── LeastSquares │ │ ├── MetisSupport │ │ ├── OrderingMethods │ │ ├── PaStiXSupport │ │ ├── PardisoSupport │ │ ├── QR │ │ ├── QtAlignedMalloc │ │ ├── SPQRSupport │ │ ├── SVD │ │ ├── Sparse │ │ ├── SparseCholesky │ │ ├── SparseCore │ │ ├── SparseLU │ │ ├── SparseQR │ │ ├── StdDeque │ │ ├── StdList │ │ ├── StdVector │ │ ├── SuperLUSupport │ │ ├── UmfPackSupport │ │ └── src │ │ ├── .DS_Store │ │ ├── CMakeLists.txt │ │ ├── Cholesky │ │ ├── CMakeLists.txt │ │ ├── LDLT.h │ │ ├── LLT.h │ │ └── LLT_MKL.h │ │ ├── CholmodSupport │ │ ├── CMakeLists.txt │ │ └── CholmodSupport.h │ │ ├── Core │ │ ├── Array.h │ │ ├── ArrayBase.h │ │ ├── ArrayWrapper.h │ │ ├── Assign.h │ │ ├── Assign_MKL.h │ │ ├── BandMatrix.h │ │ ├── Block.h │ │ ├── BooleanRedux.h │ │ ├── CMakeLists.txt │ │ ├── CommaInitializer.h │ │ ├── CoreIterators.h │ │ ├── CwiseBinaryOp.h │ │ ├── CwiseNullaryOp.h │ │ ├── CwiseUnaryOp.h │ │ ├── CwiseUnaryView.h │ │ ├── DenseBase.h │ │ ├── DenseCoeffsBase.h │ │ ├── DenseStorage.h │ │ ├── Diagonal.h │ │ ├── DiagonalMatrix.h │ │ ├── DiagonalProduct.h │ │ ├── Dot.h │ │ ├── EigenBase.h │ │ ├── Flagged.h │ │ ├── ForceAlignedAccess.h │ │ ├── Functors.h │ │ ├── Fuzzy.h │ │ ├── GeneralProduct.h │ │ ├── GenericPacketMath.h │ │ ├── GlobalFunctions.h │ │ ├── IO.h │ │ ├── Map.h │ │ ├── MapBase.h │ │ ├── MathFunctions.h │ │ ├── Matrix.h │ │ ├── MatrixBase.h │ │ ├── NestByValue.h │ │ ├── NoAlias.h │ │ ├── NumTraits.h │ │ ├── PermutationMatrix.h │ │ ├── PlainObjectBase.h │ │ ├── ProductBase.h │ │ ├── Random.h │ │ ├── Redux.h │ │ ├── Ref.h │ │ ├── Replicate.h │ │ ├── ReturnByValue.h │ │ ├── Reverse.h │ │ ├── Select.h │ │ ├── SelfAdjointView.h │ │ ├── SelfCwiseBinaryOp.h │ │ ├── SolveTriangular.h │ │ ├── StableNorm.h │ │ ├── Stride.h │ │ ├── Swap.h │ │ ├── Transpose.h │ │ ├── Transpositions.h │ │ ├── TriangularMatrix.h │ │ ├── VectorBlock.h │ │ ├── VectorwiseOp.h │ │ ├── Visitor.h │ │ ├── arch │ │ │ ├── AltiVec │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── Complex.h │ │ │ │ └── PacketMath.h │ │ │ ├── CMakeLists.txt │ │ │ ├── Default │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── Settings.h │ │ │ ├── NEON │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── Complex.h │ │ │ │ └── PacketMath.h │ │ │ └── SSE │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── Complex.h │ │ │ │ ├── MathFunctions.h │ │ │ │ └── PacketMath.h │ │ ├── products │ │ │ ├── CMakeLists.txt │ │ │ ├── CoeffBasedProduct.h │ │ │ ├── GeneralBlockPanelKernel.h │ │ │ ├── GeneralMatrixMatrix.h │ │ │ ├── GeneralMatrixMatrixTriangular.h │ │ │ ├── GeneralMatrixMatrixTriangular_MKL.h │ │ │ ├── GeneralMatrixMatrix_MKL.h │ │ │ ├── GeneralMatrixVector.h │ │ │ ├── GeneralMatrixVector_MKL.h │ │ │ ├── Parallelizer.h │ │ │ ├── SelfadjointMatrixMatrix.h │ │ │ ├── SelfadjointMatrixMatrix_MKL.h │ │ │ ├── SelfadjointMatrixVector.h │ │ │ ├── SelfadjointMatrixVector_MKL.h │ │ │ ├── SelfadjointProduct.h │ │ │ ├── SelfadjointRank2Update.h │ │ │ ├── TriangularMatrixMatrix.h │ │ │ ├── TriangularMatrixMatrix_MKL.h │ │ │ ├── TriangularMatrixVector.h │ │ │ ├── TriangularMatrixVector_MKL.h │ │ │ ├── TriangularSolverMatrix.h │ │ │ ├── TriangularSolverMatrix_MKL.h │ │ │ └── TriangularSolverVector.h │ │ └── util │ │ │ ├── BlasUtil.h │ │ │ ├── CMakeLists.txt │ │ │ ├── Constants.h │ │ │ ├── DisableStupidWarnings.h │ │ │ ├── ForwardDeclarations.h │ │ │ ├── MKL_support.h │ │ │ ├── Macros.h │ │ │ ├── Memory.h │ │ │ ├── Meta.h │ │ │ ├── NonMPL2.h │ │ │ ├── ReenableStupidWarnings.h │ │ │ ├── StaticAssert.h │ │ │ └── XprHelper.h │ │ ├── Eigen2Support │ │ ├── Block.h │ │ ├── CMakeLists.txt │ │ ├── Cwise.h │ │ ├── CwiseOperators.h │ │ ├── Geometry │ │ │ ├── AlignedBox.h │ │ │ ├── All.h │ │ │ ├── AngleAxis.h │ │ │ ├── CMakeLists.txt │ │ │ ├── Hyperplane.h │ │ │ ├── ParametrizedLine.h │ │ │ ├── Quaternion.h │ │ │ ├── Rotation2D.h │ │ │ ├── RotationBase.h │ │ │ ├── Scaling.h │ │ │ ├── Transform.h │ │ │ └── Translation.h │ │ ├── LU.h │ │ ├── Lazy.h │ │ ├── LeastSquares.h │ │ ├── Macros.h │ │ ├── MathFunctions.h │ │ ├── Memory.h │ │ ├── Meta.h │ │ ├── Minor.h │ │ ├── QR.h │ │ ├── SVD.h │ │ ├── TriangularSolver.h │ │ └── VectorBlock.h │ │ ├── Eigenvalues │ │ ├── CMakeLists.txt │ │ ├── ComplexEigenSolver.h │ │ ├── ComplexSchur.h │ │ ├── ComplexSchur_MKL.h │ │ ├── EigenSolver.h │ │ ├── GeneralizedEigenSolver.h │ │ ├── GeneralizedSelfAdjointEigenSolver.h │ │ ├── HessenbergDecomposition.h │ │ ├── MatrixBaseEigenvalues.h │ │ ├── RealQZ.h │ │ ├── RealSchur.h │ │ ├── RealSchur_MKL.h │ │ ├── SelfAdjointEigenSolver.h │ │ ├── SelfAdjointEigenSolver_MKL.h │ │ └── Tridiagonalization.h │ │ ├── Geometry │ │ ├── AlignedBox.h │ │ ├── AngleAxis.h │ │ ├── CMakeLists.txt │ │ ├── EulerAngles.h │ │ ├── Homogeneous.h │ │ ├── Hyperplane.h │ │ ├── OrthoMethods.h │ │ ├── ParametrizedLine.h │ │ ├── Quaternion.h │ │ ├── Rotation2D.h │ │ ├── RotationBase.h │ │ ├── Scaling.h │ │ ├── Transform.h │ │ ├── Translation.h │ │ ├── Umeyama.h │ │ └── arch │ │ │ ├── CMakeLists.txt │ │ │ └── Geometry_SSE.h │ │ ├── Householder │ │ ├── BlockHouseholder.h │ │ ├── CMakeLists.txt │ │ ├── Householder.h │ │ └── HouseholderSequence.h │ │ ├── IterativeLinearSolvers │ │ ├── BasicPreconditioners.h │ │ ├── BiCGSTAB.h │ │ ├── CMakeLists.txt │ │ ├── ConjugateGradient.h │ │ ├── IncompleteLUT.h │ │ └── IterativeSolverBase.h │ │ ├── Jacobi │ │ ├── CMakeLists.txt │ │ └── Jacobi.h │ │ ├── LU │ │ ├── CMakeLists.txt │ │ ├── Determinant.h │ │ ├── FullPivLU.h │ │ ├── Inverse.h │ │ ├── PartialPivLU.h │ │ ├── PartialPivLU_MKL.h │ │ └── arch │ │ │ ├── CMakeLists.txt │ │ │ └── Inverse_SSE.h │ │ ├── MetisSupport │ │ ├── CMakeLists.txt │ │ └── MetisSupport.h │ │ ├── OrderingMethods │ │ ├── Amd.h │ │ ├── CMakeLists.txt │ │ ├── Eigen_Colamd.h │ │ └── Ordering.h │ │ ├── PaStiXSupport │ │ ├── CMakeLists.txt │ │ └── PaStiXSupport.h │ │ ├── PardisoSupport │ │ ├── CMakeLists.txt │ │ └── PardisoSupport.h │ │ ├── QR │ │ ├── CMakeLists.txt │ │ ├── ColPivHouseholderQR.h │ │ ├── ColPivHouseholderQR_MKL.h │ │ ├── FullPivHouseholderQR.h │ │ ├── HouseholderQR.h │ │ └── HouseholderQR_MKL.h │ │ ├── SPQRSupport │ │ ├── CMakeLists.txt │ │ └── SuiteSparseQRSupport.h │ │ ├── SVD │ │ ├── CMakeLists.txt │ │ ├── JacobiSVD.h │ │ ├── JacobiSVD_MKL.h │ │ └── UpperBidiagonalization.h │ │ ├── SparseCholesky │ │ ├── CMakeLists.txt │ │ ├── SimplicialCholesky.h │ │ └── SimplicialCholesky_impl.h │ │ ├── SparseCore │ │ ├── AmbiVector.h │ │ ├── CMakeLists.txt │ │ ├── CompressedStorage.h │ │ ├── ConservativeSparseSparseProduct.h │ │ ├── MappedSparseMatrix.h │ │ ├── SparseBlock.h │ │ ├── SparseColEtree.h │ │ ├── SparseCwiseBinaryOp.h │ │ ├── SparseCwiseUnaryOp.h │ │ ├── SparseDenseProduct.h │ │ ├── SparseDiagonalProduct.h │ │ ├── SparseDot.h │ │ ├── SparseFuzzy.h │ │ ├── SparseMatrix.h │ │ ├── SparseMatrixBase.h │ │ ├── SparsePermutation.h │ │ ├── SparseProduct.h │ │ ├── SparseRedux.h │ │ ├── SparseSelfAdjointView.h │ │ ├── SparseSparseProductWithPruning.h │ │ ├── SparseTranspose.h │ │ ├── SparseTriangularView.h │ │ ├── SparseUtil.h │ │ ├── SparseVector.h │ │ ├── SparseView.h │ │ └── TriangularSolver.h │ │ ├── SparseLU │ │ ├── CMakeLists.txt │ │ ├── SparseLU.h │ │ ├── SparseLUImpl.h │ │ ├── SparseLU_Memory.h │ │ ├── SparseLU_Structs.h │ │ ├── SparseLU_SupernodalMatrix.h │ │ ├── SparseLU_Utils.h │ │ ├── SparseLU_column_bmod.h │ │ ├── SparseLU_column_dfs.h │ │ ├── SparseLU_copy_to_ucol.h │ │ ├── SparseLU_gemm_kernel.h │ │ ├── SparseLU_heap_relax_snode.h │ │ ├── SparseLU_kernel_bmod.h │ │ ├── SparseLU_panel_bmod.h │ │ ├── SparseLU_panel_dfs.h │ │ ├── SparseLU_pivotL.h │ │ ├── SparseLU_pruneL.h │ │ └── SparseLU_relax_snode.h │ │ ├── SparseQR │ │ ├── CMakeLists.txt │ │ └── SparseQR.h │ │ ├── StlSupport │ │ ├── CMakeLists.txt │ │ ├── StdDeque.h │ │ ├── StdList.h │ │ ├── StdVector.h │ │ └── details.h │ │ ├── SuperLUSupport │ │ ├── CMakeLists.txt │ │ └── SuperLUSupport.h │ │ ├── UmfPackSupport │ │ ├── CMakeLists.txt │ │ └── UmfPackSupport.h │ │ ├── misc │ │ ├── CMakeLists.txt │ │ ├── Image.h │ │ ├── Kernel.h │ │ ├── Solve.h │ │ ├── SparseSolve.h │ │ └── blas.h │ │ └── plugins │ │ ├── ArrayCwiseBinaryOps.h │ │ ├── ArrayCwiseUnaryOps.h │ │ ├── BlockMethods.h │ │ ├── CMakeLists.txt │ │ ├── CommonCwiseBinaryOps.h │ │ ├── CommonCwiseUnaryOps.h │ │ ├── MatrixCwiseBinaryOps.h │ │ └── MatrixCwiseUnaryOps.h ├── icon.jpg └── main.m ├── orbslamiosTests ├── Info.plist └── orbslamiosTests.m ├── orbslamiosUITests ├── Info.plist └── orbslamiosUITests.m └── ygxslam ├── .DS_Store ├── ORBvoc.bin └── Settings.yaml /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/.DS_Store -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ORB_SLAM2-IOS 2 | This is iOS port of [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2) with modified dependency for iOS. 3 | 4 | And this link is iOS port of [ORB-SLAM1](https://github.com/ygx2011/ORB_SLAM-IOS). 5 | 6 | You need to download some dependency and opencv2.framework from 7 | https://pan.baidu.com/s/1i5fsyot 8 | 9 | Unzip orbslam2ios.zip and put the dependency folder and opencv2.framework in the orbslamios folder. 10 | 11 | You can do some AR projects. If you are interested in SLAM-AR, you can see my another project [Stereo-SLAM-AR](https://github.com/ygx2011/Stereo_SLAM_AR). 12 | 13 | slam-ar 16 | -------------------------------------------------------------------------------- /orbslamios.xcodeproj/project.xcworkspace/contents.xcworkspacedata: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /orbslamios.xcodeproj/project.xcworkspace/xcuserdata/Yinggx.xcuserdatad/UserInterfaceState.xcuserstate: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios.xcodeproj/project.xcworkspace/xcuserdata/Yinggx.xcuserdatad/UserInterfaceState.xcuserstate -------------------------------------------------------------------------------- /orbslamios.xcodeproj/xcuserdata/Yinggx.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /orbslamios.xcodeproj/xcuserdata/Yinggx.xcuserdatad/xcschemes/xcschememanagement.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | SchemeUserState 6 | 7 | orbslamios.xcscheme 8 | 9 | orderHint 10 | 0 11 | 12 | 13 | SuppressBuildableAutocreation 14 | 15 | 60FF20331DC883E7003B783E 16 | 17 | primary 18 | 19 | 20 | 60FF204C1DC883E7003B783E 21 | 22 | primary 23 | 24 | 25 | 60FF20571DC883E7003B783E 26 | 27 | primary 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /orbslamios/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/.DS_Store -------------------------------------------------------------------------------- /orbslamios/AppDelegate.h: -------------------------------------------------------------------------------- 1 | // 2 | // AppDelegate.h 3 | // orbslamios 4 | // 5 | // Created by Ying Gaoxuan on 16/11/1. 6 | // Copyright © 2016年 Ying Gaoxuan. All rights reserved. 7 | // 8 | 9 | #import 10 | 11 | @interface AppDelegate : UIResponder 12 | 13 | @property (strong, nonatomic) UIWindow *window; 14 | 15 | 16 | @end 17 | 18 | -------------------------------------------------------------------------------- /orbslamios/AppDelegate.m: -------------------------------------------------------------------------------- 1 | // 2 | // AppDelegate.m 3 | // orbslamios 4 | // 5 | // Created by Ying Gaoxuan on 16/11/1. 6 | // Copyright © 2016年 Ying Gaoxuan. All rights reserved. 7 | // 8 | 9 | #import "AppDelegate.h" 10 | 11 | @interface AppDelegate () 12 | 13 | @end 14 | 15 | @implementation AppDelegate 16 | 17 | 18 | - (BOOL)application:(UIApplication *)application didFinishLaunchingWithOptions:(NSDictionary *)launchOptions { 19 | // Override point for customization after application launch. 20 | return YES; 21 | } 22 | 23 | 24 | - (void)applicationWillResignActive:(UIApplication *)application { 25 | // Sent when the application is about to move from active to inactive state. This can occur for certain types of temporary interruptions (such as an incoming phone call or SMS message) or when the user quits the application and it begins the transition to the background state. 26 | // Use this method to pause ongoing tasks, disable timers, and invalidate graphics rendering callbacks. Games should use this method to pause the game. 27 | } 28 | 29 | 30 | - (void)applicationDidEnterBackground:(UIApplication *)application { 31 | // Use this method to release shared resources, save user data, invalidate timers, and store enough application state information to restore your application to its current state in case it is terminated later. 32 | // If your application supports background execution, this method is called instead of applicationWillTerminate: when the user quits. 33 | } 34 | 35 | 36 | - (void)applicationWillEnterForeground:(UIApplication *)application { 37 | // Called as part of the transition from the background to the active state; here you can undo many of the changes made on entering the background. 38 | } 39 | 40 | 41 | - (void)applicationDidBecomeActive:(UIApplication *)application { 42 | // Restart any tasks that were paused (or not yet started) while the application was inactive. If the application was previously in the background, optionally refresh the user interface. 43 | } 44 | 45 | 46 | - (void)applicationWillTerminate:(UIApplication *)application { 47 | // Called when the application is about to terminate. Save data if appropriate. See also applicationDidEnterBackground:. 48 | } 49 | 50 | 51 | @end 52 | -------------------------------------------------------------------------------- /orbslamios/Assets.xcassets/AppIcon.appiconset/Contents.json: -------------------------------------------------------------------------------- 1 | { 2 | "images" : [ 3 | { 4 | "idiom" : "iphone", 5 | "size" : "20x20", 6 | "scale" : "2x" 7 | }, 8 | { 9 | "idiom" : "iphone", 10 | "size" : "20x20", 11 | "scale" : "3x" 12 | }, 13 | { 14 | "idiom" : "iphone", 15 | "size" : "29x29", 16 | "scale" : "2x" 17 | }, 18 | { 19 | "idiom" : "iphone", 20 | "size" : "29x29", 21 | "scale" : "3x" 22 | }, 23 | { 24 | "idiom" : "iphone", 25 | "size" : "40x40", 26 | "scale" : "2x" 27 | }, 28 | { 29 | "idiom" : "iphone", 30 | "size" : "40x40", 31 | "scale" : "3x" 32 | }, 33 | { 34 | "idiom" : "iphone", 35 | "size" : "60x60", 36 | "scale" : "2x" 37 | }, 38 | { 39 | "idiom" : "iphone", 40 | "size" : "60x60", 41 | "scale" : "3x" 42 | }, 43 | { 44 | "idiom" : "ipad", 45 | "size" : "20x20", 46 | "scale" : "1x" 47 | }, 48 | { 49 | "idiom" : "ipad", 50 | "size" : "20x20", 51 | "scale" : "2x" 52 | }, 53 | { 54 | "idiom" : "ipad", 55 | "size" : "29x29", 56 | "scale" : "1x" 57 | }, 58 | { 59 | "idiom" : "ipad", 60 | "size" : "29x29", 61 | "scale" : "2x" 62 | }, 63 | { 64 | "idiom" : "ipad", 65 | "size" : "40x40", 66 | "scale" : "1x" 67 | }, 68 | { 69 | "idiom" : "ipad", 70 | "size" : "40x40", 71 | "scale" : "2x" 72 | }, 73 | { 74 | "idiom" : "ipad", 75 | "size" : "76x76", 76 | "scale" : "1x" 77 | }, 78 | { 79 | "idiom" : "ipad", 80 | "size" : "76x76", 81 | "scale" : "2x" 82 | }, 83 | { 84 | "idiom" : "ipad", 85 | "size" : "83.5x83.5", 86 | "scale" : "2x" 87 | } 88 | ], 89 | "info" : { 90 | "version" : 1, 91 | "author" : "xcode" 92 | } 93 | } -------------------------------------------------------------------------------- /orbslamios/Base.lproj/LaunchScreen.storyboard: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /orbslamios/Info.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | CFBundleDevelopmentRegion 6 | en 7 | CFBundleExecutable 8 | $(EXECUTABLE_NAME) 9 | CFBundleIconFiles 10 | 11 | icon.jpg 12 | 13 | CFBundleIdentifier 14 | $(PRODUCT_BUNDLE_IDENTIFIER) 15 | CFBundleInfoDictionaryVersion 16 | 6.0 17 | CFBundleName 18 | $(PRODUCT_NAME) 19 | CFBundlePackageType 20 | APPL 21 | CFBundleShortVersionString 22 | 1.0 23 | CFBundleVersion 24 | 1 25 | LSRequiresIPhoneOS 26 | 27 | NSCameraUsageDescription 28 | 让我用下相机呗 29 | UILaunchStoryboardName 30 | LaunchScreen 31 | UIMainStoryboardFile 32 | Main 33 | UIRequiredDeviceCapabilities 34 | 35 | armv7 36 | 37 | UISupportedInterfaceOrientations 38 | 39 | UIInterfaceOrientationLandscapeRight 40 | 41 | UISupportedInterfaceOrientations~ipad 42 | 43 | UIInterfaceOrientationPortrait 44 | UIInterfaceOrientationPortraitUpsideDown 45 | UIInterfaceOrientationLandscapeLeft 46 | UIInterfaceOrientationLandscapeRight 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/ORB_SLAM/.DS_Store -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/ORB_SLAM/Thirdparty/.DS_Store -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/DBoW2/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/ORB_SLAM/Thirdparty/DBoW2/.DS_Store -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:74db2d8bdcc8a22afb73fff5854cf3871a1758673d823c4cc9d5d84b5a785524 3 | size 800 4 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/DBoW2/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:95cd3e2ade6564783d4f2bdee3a13c9855be1ce8674da896424be31a0f91729c 3 | size 2336 4 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | }; 44 | 45 | /** 46 | * Macro for defining Scoring classes 47 | * @param NAME name of class 48 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 49 | * @param NORM type of norm to use when MUSTNORMALIZE 50 | */ 51 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 52 | NAME: public GeneralScoring \ 53 | { public: \ 54 | /** \ 55 | * Computes score between two vectors \ 56 | * @param v \ 57 | * @param w \ 58 | * @return score between v and w \ 59 | */ \ 60 | virtual double score(const BowVector &v, const BowVector &w) const; \ 61 | \ 62 | /** \ 63 | * Says if a vector must be normalized according to the scoring function \ 64 | * @param norm (out) if true, norm to use 65 | * @return true iff vectors must be normalized \ 66 | */ \ 67 | virtual inline bool mustNormalize(LNorm &norm) const \ 68 | { norm = NORM; return MUSTNORMALIZE; } \ 69 | } 70 | 71 | /// L1 Scoring object 72 | class __SCORING_CLASS(L1Scoring, true, L1); 73 | 74 | /// L2 Scoring object 75 | class __SCORING_CLASS(L2Scoring, true, L2); 76 | 77 | /// Chi square Scoring object 78 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 79 | 80 | /// KL divergence Scoring object 81 | class __SCORING_CLASS(KLScoring, true, L1); 82 | 83 | /// Bhattacharyya Scoring object 84 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 85 | 86 | /// Dot product Scoring object 87 | class __SCORING_CLASS(DotProductScoring, false, L1); 88 | 89 | #undef __SCORING_CLASS 90 | 91 | } // namespace DBoW2 92 | 93 | #endif 94 | 95 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/DBoW2/DUtils/LICENSE.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:057606c0398f1a5e5d55956afc31b058dd2ce8ff983bc3b678e39170d9f91a78 3 | size 1755 4 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b04a94bb8a6e50edf9874377751fbaa63139ede6dd6305cd06aad3b5d5a1d340 3 | size 2342 4 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/g2o/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/ORB_SLAM/Thirdparty/g2o/.DS_Store -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/g2o/g2o/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/ORB_SLAM/Thirdparty/g2o/g2o/.DS_Store -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/Thirdparty/g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | #include"types_six_dof_expmap.h" 28 | #include"types_seven_dof_expmap.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class Converter 34 | { 35 | public: 36 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 37 | 38 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 39 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 40 | 41 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 42 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 43 | static cv::Mat toCvMat(const Eigen::Matrix &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 47 | 48 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 49 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 50 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 51 | 52 | static std::vector toQuaternion(const cv::Mat &M); 53 | }; 54 | 55 | }// namespace ORB_SLAM 56 | 57 | #endif // CONVERTER_H 58 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEDRAWER_H 22 | #define FRAMEDRAWER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | class Viewer; 39 | 40 | class FrameDrawer 41 | { 42 | public: 43 | FrameDrawer(Map* pMap); 44 | 45 | // Update info from the last processed frame. 46 | void Update(Tracking *pTracker); 47 | 48 | // Draw last processed frame. 49 | cv::Mat DrawFrame(); 50 | 51 | protected: 52 | 53 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 54 | 55 | // Info of the frame to be drawn 56 | cv::Mat mIm; 57 | int N; 58 | vector mvCurrentKeys; 59 | vector mvbMap, mvbVO; 60 | bool mbOnlyTracking; 61 | int mnTracked, mnTrackedVO; 62 | vector mvIniKeys; 63 | vector mvIniMatches; 64 | int mState; 65 | 66 | Map* mpMap; 67 | 68 | std::mutex mMutex; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif // FRAMEDRAWER_H 74 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | 42 | class KeyFrameDatabase 43 | { 44 | public: 45 | 46 | KeyFrameDatabase(const ORBVocabulary &voc); 47 | 48 | void add(KeyFrame* pKF); 49 | 50 | void erase(KeyFrame* pKF); 51 | 52 | void clear(); 53 | 54 | // Loop Detection 55 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 56 | 57 | // Relocalization 58 | std::vector DetectRelocalizationCandidates(Frame* F); 59 | 60 | protected: 61 | 62 | // Associated vocabulary 63 | const ORBVocabulary* mpVoc; 64 | 65 | // Inverted file 66 | std::vector > mvInvertedFile; 67 | 68 | // Mutex 69 | std::mutex mMutex; 70 | }; 71 | 72 | } //namespace ORB_SLAM 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAP_H 22 | #define MAP_H 23 | 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include 27 | 28 | #include 29 | 30 | 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class MapPoint; 36 | class KeyFrame; 37 | 38 | class Map 39 | { 40 | public: 41 | Map(); 42 | 43 | void AddKeyFrame(KeyFrame* pKF); 44 | void AddMapPoint(MapPoint* pMP); 45 | void EraseMapPoint(MapPoint* pMP); 46 | void EraseKeyFrame(KeyFrame* pKF); 47 | void SetReferenceMapPoints(const std::vector &vpMPs); 48 | void InformNewBigChange(); 49 | int GetLastBigChangeIdx(); 50 | 51 | std::vector GetAllKeyFrames(); 52 | std::vector GetAllMapPoints(); 53 | std::vector GetReferenceMapPoints(); 54 | 55 | long unsigned int MapPointsInMap(); 56 | long unsigned KeyFramesInMap(); 57 | 58 | long unsigned int GetMaxKFid(); 59 | 60 | void clear(); 61 | 62 | vector mvpKeyFrameOrigins; 63 | 64 | std::mutex mMutexMapUpdate; 65 | 66 | // This avoid that two points are created simultaneously in separate threads (id conflict) 67 | std::mutex mMutexPointCreation; 68 | 69 | protected: 70 | std::set mspMapPoints; 71 | std::set mspKeyFrames; 72 | 73 | std::vector mvpReferenceMapPoints; 74 | 75 | long unsigned int mnMaxKFid; 76 | 77 | // Index related to a big change in the map (loop closure, global BA) 78 | int mnBigChangeIdx; 79 | 80 | std::mutex mMutexMap; 81 | }; 82 | 83 | } //namespace ORB_SLAM 84 | 85 | #endif // MAP_H 86 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPDRAWER_H 22 | #define MAPDRAWER_H 23 | 24 | #include"Map.h" 25 | #include"MapPoint.h" 26 | #include"KeyFrame.h" 27 | //#include 28 | 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class MapDrawer 35 | { 36 | public: 37 | MapDrawer(Map* pMap, const string &strSettingPath); 38 | 39 | Map* mpMap; 40 | 41 | void DrawMapPoints(); 42 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 43 | //void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 44 | void SetCurrentCameraPose(const cv::Mat &Tcw); 45 | void SetReferenceKeyFrame(KeyFrame *pKF); 46 | //void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 47 | 48 | private: 49 | 50 | float mKeyFrameSize; 51 | float mKeyFrameLineWidth; 52 | float mGraphLineWidth; 53 | float mPointSize; 54 | float mCameraSize; 55 | float mCameraLineWidth; 56 | 57 | cv::Mat mCameraPose; 58 | 59 | std::mutex mMutexCamera; 60 | }; 61 | 62 | } //namespace ORB_SLAM 63 | 64 | #endif // MAPDRAWER_H 65 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include"FORB.h" 26 | #include"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 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | 30 | #include "types_seven_dof_expmap.h" 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class LoopClosing; 36 | 37 | class Optimizer 38 | { 39 | public: 40 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 41 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 42 | const bool bRobust = true); 43 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 44 | const unsigned long nLoopKF=0, const bool bRobust = true); 45 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 46 | int static PoseOptimization(Frame* pFrame); 47 | 48 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 49 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 50 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 51 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 52 | const map > &LoopConnections, 53 | const bool &bFixScale); 54 | 55 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 56 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 57 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 58 | }; 59 | 60 | } //namespace ORB_SLAM 61 | 62 | #endif // OPTIMIZER_H 63 | -------------------------------------------------------------------------------- /orbslamios/ORB_SLAM/include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Tracking; 36 | class FrameDrawer; 37 | class MapDrawer; 38 | class System; 39 | 40 | class Viewer 41 | { 42 | public: 43 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); 44 | 45 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 46 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 47 | void Run(); 48 | 49 | void RequestFinish(); 50 | 51 | void RequestStop(); 52 | 53 | bool isFinished(); 54 | 55 | bool isStopped(); 56 | 57 | void Release(); 58 | 59 | private: 60 | 61 | bool Stop(); 62 | 63 | System* mpSystem; 64 | FrameDrawer* mpFrameDrawer; 65 | MapDrawer* mpMapDrawer; 66 | Tracking* mpTracker; 67 | 68 | // 1/fps in ms 69 | double mT; 70 | float mImageWidth, mImageHeight; 71 | 72 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 73 | 74 | bool CheckFinish(); 75 | void SetFinish(); 76 | bool mbFinishRequested; 77 | bool mbFinished; 78 | std::mutex mMutexFinish; 79 | 80 | bool mbStopped; 81 | bool mbStopRequested; 82 | std::mutex mMutexStop; 83 | 84 | }; 85 | 86 | } 87 | 88 | 89 | #endif // VIEWER_H 90 | 91 | 92 | -------------------------------------------------------------------------------- /orbslamios/ViewController.h: -------------------------------------------------------------------------------- 1 | // 2 | // ViewController.h 3 | // orbslamios 4 | // 5 | // Created by Ying Gaoxuan on 16/11/1. 6 | // Copyright © 2016年 Ying Gaoxuan. All rights reserved. 7 | // 8 | 9 | #import 10 | 11 | #import 12 | 13 | @interface ViewController : UIViewController 14 | 15 | - (IBAction)startButtonPressed:(id)sender; 16 | @property (weak, nonatomic) IBOutlet UIButton *start; 17 | @property (weak, nonatomic) IBOutlet UIImageView *imageView; 18 | @property (nonatomic, strong) CvVideoCamera* videoCamera; 19 | 20 | @end 21 | -------------------------------------------------------------------------------- /orbslamios/eigen/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/eigen/.DS_Store -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/eigen/Eigen/.DS_Store -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Array: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_ARRAY_MODULE_H 2 | #define EIGEN_ARRAY_MODULE_H 3 | 4 | // include Core first to handle Eigen2 support macros 5 | #include "Core" 6 | 7 | #ifndef EIGEN2_SUPPORT 8 | #error The Eigen/Array header does no longer exist in Eigen3. All that functionality has moved to Eigen/Core. 9 | #endif 10 | 11 | #endif // EIGEN_ARRAY_MODULE_H 12 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2c463a5095ac6472752fd8de4c4af111ada4d2f0ada526bc198d6d1e7a6c98fa 3 | size 607 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Cholesky: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_CHOLESKY_MODULE_H 2 | #define EIGEN_CHOLESKY_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | /** \defgroup Cholesky_Module Cholesky module 9 | * 10 | * 11 | * 12 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 13 | * Those decompositions are accessible via the following MatrixBase methods: 14 | * - MatrixBase::llt(), 15 | * - MatrixBase::ldlt() 16 | * 17 | * \code 18 | * #include 19 | * \endcode 20 | */ 21 | 22 | #include "src/misc/Solve.h" 23 | #include "src/Cholesky/LLT.h" 24 | #include "src/Cholesky/LDLT.h" 25 | #ifdef EIGEN_USE_LAPACKE 26 | #include "src/Cholesky/LLT_MKL.h" 27 | #endif 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif // EIGEN_CHOLESKY_MODULE_H 32 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 33 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/CholmodSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H 2 | #define EIGEN_CHOLMODSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | extern "C" { 9 | #include 10 | } 11 | 12 | /** \ingroup Support_modules 13 | * \defgroup CholmodSupport_Module CholmodSupport module 14 | * 15 | * This module provides an interface to the Cholmod library which is part of the suitesparse package. 16 | * It provides the two following main factorization classes: 17 | * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. 18 | * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). 19 | * 20 | * For the sake of completeness, this module also propose the two following classes: 21 | * - class CholmodSimplicialLLT 22 | * - class CholmodSimplicialLDLT 23 | * Note that these classes does not bring any particular advantage compared to the built-in 24 | * SimplicialLLT and SimplicialLDLT factorization classes. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | * 30 | * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies. 31 | * The dependencies depend on how cholmod has been compiled. 32 | * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. 33 | * 34 | */ 35 | 36 | #include "src/misc/Solve.h" 37 | #include "src/misc/SparseSolve.h" 38 | 39 | #include "src/CholmodSupport/CholmodSupport.h" 40 | 41 | 42 | #include "src/Core/util/ReenableStupidWarnings.h" 43 | 44 | #endif // EIGEN_CHOLMODSUPPORT_MODULE_H 45 | 46 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | //#include "Sparse" 3 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Eigenvalues: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_EIGENVALUES_MODULE_H 2 | #define EIGEN_EIGENVALUES_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include "Cholesky" 9 | #include "Jacobi" 10 | #include "Householder" 11 | #include "LU" 12 | #include "Geometry" 13 | 14 | /** \defgroup Eigenvalues_Module Eigenvalues module 15 | * 16 | * 17 | * 18 | * This module mainly provides various eigenvalue solvers. 19 | * This module also provides some MatrixBase methods, including: 20 | * - MatrixBase::eigenvalues(), 21 | * - MatrixBase::operatorNorm() 22 | * 23 | * \code 24 | * #include 25 | * \endcode 26 | */ 27 | 28 | #include "src/Eigenvalues/Tridiagonalization.h" 29 | #include "src/Eigenvalues/RealSchur.h" 30 | #include "src/Eigenvalues/EigenSolver.h" 31 | #include "src/Eigenvalues/SelfAdjointEigenSolver.h" 32 | #include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" 33 | #include "src/Eigenvalues/HessenbergDecomposition.h" 34 | #include "src/Eigenvalues/ComplexSchur.h" 35 | #include "src/Eigenvalues/ComplexEigenSolver.h" 36 | #include "src/Eigenvalues/RealQZ.h" 37 | #include "src/Eigenvalues/GeneralizedEigenSolver.h" 38 | #include "src/Eigenvalues/MatrixBaseEigenvalues.h" 39 | #ifdef EIGEN_USE_LAPACKE 40 | #include "src/Eigenvalues/RealSchur_MKL.h" 41 | #include "src/Eigenvalues/ComplexSchur_MKL.h" 42 | #include "src/Eigenvalues/SelfAdjointEigenSolver_MKL.h" 43 | #endif 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_EIGENVALUES_MODULE_H 48 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 49 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Geometry: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_GEOMETRY_MODULE_H 2 | #define EIGEN_GEOMETRY_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include "SVD" 9 | #include "LU" 10 | #include 11 | 12 | #ifndef M_PI 13 | #define M_PI 3.14159265358979323846 14 | #endif 15 | 16 | /** \defgroup Geometry_Module Geometry module 17 | * 18 | * 19 | * 20 | * This module provides support for: 21 | * - fixed-size homogeneous transformations 22 | * - translation, scaling, 2D and 3D rotations 23 | * - quaternions 24 | * - \ref MatrixBase::cross() "cross product" 25 | * - \ref MatrixBase::unitOrthogonal() "orthognal vector generation" 26 | * - some linear components: parametrized-lines and hyperplanes 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | */ 32 | 33 | #include "src/Geometry/OrthoMethods.h" 34 | #include "src/Geometry/EulerAngles.h" 35 | 36 | #if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS 37 | #include "src/Geometry/Homogeneous.h" 38 | #include "src/Geometry/RotationBase.h" 39 | #include "src/Geometry/Rotation2D.h" 40 | #include "src/Geometry/Quaternion.h" 41 | #include "src/Geometry/AngleAxis.h" 42 | #include "src/Geometry/Transform.h" 43 | #include "src/Geometry/Translation.h" 44 | #include "src/Geometry/Scaling.h" 45 | #include "src/Geometry/Hyperplane.h" 46 | #include "src/Geometry/ParametrizedLine.h" 47 | #include "src/Geometry/AlignedBox.h" 48 | #include "src/Geometry/Umeyama.h" 49 | 50 | #if defined EIGEN_VECTORIZE_SSE 51 | #include "src/Geometry/arch/Geometry_SSE.h" 52 | #endif 53 | #endif 54 | 55 | #ifdef EIGEN2_SUPPORT 56 | #include "src/Eigen2Support/Geometry/All.h" 57 | #endif 58 | 59 | #include "src/Core/util/ReenableStupidWarnings.h" 60 | 61 | #endif // EIGEN_GEOMETRY_MODULE_H 62 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 63 | 64 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Householder: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_HOUSEHOLDER_MODULE_H 2 | #define EIGEN_HOUSEHOLDER_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | /** \defgroup Householder_Module Householder module 9 | * This module provides Householder transformations. 10 | * 11 | * \code 12 | * #include 13 | * \endcode 14 | */ 15 | 16 | #include "src/Householder/Householder.h" 17 | #include "src/Householder/HouseholderSequence.h" 18 | #include "src/Householder/BlockHouseholder.h" 19 | 20 | #include "src/Core/util/ReenableStupidWarnings.h" 21 | 22 | #endif // EIGEN_HOUSEHOLDER_MODULE_H 23 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 24 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/IterativeLinearSolvers: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 2 | #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 3 | 4 | #include "SparseCore" 5 | #include "OrderingMethods" 6 | 7 | #include "src/Core/util/DisableStupidWarnings.h" 8 | 9 | /** 10 | * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module 11 | * 12 | * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. 13 | * Those solvers are accessible via the following classes: 14 | * - ConjugateGradient for selfadjoint (hermitian) matrices, 15 | * - BiCGSTAB for general square matrices. 16 | * 17 | * These iterative solvers are associated with some preconditioners: 18 | * - IdentityPreconditioner - not really useful 19 | * - DiagonalPreconditioner - also called JAcobi preconditioner, work very well on diagonal dominant matrices. 20 | * - IncompleteILUT - incomplete LU factorization with dual thresholding 21 | * 22 | * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | */ 28 | 29 | #include "src/misc/Solve.h" 30 | #include "src/misc/SparseSolve.h" 31 | 32 | #include "src/IterativeLinearSolvers/IterativeSolverBase.h" 33 | #include "src/IterativeLinearSolvers/BasicPreconditioners.h" 34 | #include "src/IterativeLinearSolvers/ConjugateGradient.h" 35 | #include "src/IterativeLinearSolvers/BiCGSTAB.h" 36 | #include "src/IterativeLinearSolvers/IncompleteLUT.h" 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 41 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Jacobi: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_JACOBI_MODULE_H 2 | #define EIGEN_JACOBI_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | /** \defgroup Jacobi_Module Jacobi module 9 | * This module provides Jacobi and Givens rotations. 10 | * 11 | * \code 12 | * #include 13 | * \endcode 14 | * 15 | * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: 16 | * - MatrixBase::applyOnTheLeft() 17 | * - MatrixBase::applyOnTheRight(). 18 | */ 19 | 20 | #include "src/Jacobi/Jacobi.h" 21 | 22 | #include "src/Core/util/ReenableStupidWarnings.h" 23 | 24 | #endif // EIGEN_JACOBI_MODULE_H 25 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 26 | 27 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/LU: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_LU_MODULE_H 2 | #define EIGEN_LU_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | /** \defgroup LU_Module LU module 9 | * This module includes %LU decomposition and related notions such as matrix inversion and determinant. 10 | * This module defines the following MatrixBase methods: 11 | * - MatrixBase::inverse() 12 | * - MatrixBase::determinant() 13 | * 14 | * \code 15 | * #include 16 | * \endcode 17 | */ 18 | 19 | #include "src/misc/Solve.h" 20 | #include "src/misc/Kernel.h" 21 | #include "src/misc/Image.h" 22 | #include "src/LU/FullPivLU.h" 23 | #include "src/LU/PartialPivLU.h" 24 | #ifdef EIGEN_USE_LAPACKE 25 | #include "src/LU/PartialPivLU_MKL.h" 26 | #endif 27 | #include "src/LU/Determinant.h" 28 | #include "src/LU/Inverse.h" 29 | 30 | #if defined EIGEN_VECTORIZE_SSE 31 | #include "src/LU/arch/Inverse_SSE.h" 32 | #endif 33 | 34 | #ifdef EIGEN2_SUPPORT 35 | #include "src/Eigen2Support/LU.h" 36 | #endif 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_LU_MODULE_H 41 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 42 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/LeastSquares: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_REGRESSION_MODULE_H 2 | #define EIGEN_REGRESSION_MODULE_H 3 | 4 | #ifndef EIGEN2_SUPPORT 5 | #error LeastSquares is only available in Eigen2 support mode (define EIGEN2_SUPPORT) 6 | #endif 7 | 8 | // exclude from normal eigen3-only documentation 9 | #ifdef EIGEN2_SUPPORT 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Eigenvalues" 16 | #include "Geometry" 17 | 18 | /** \defgroup LeastSquares_Module LeastSquares module 19 | * This module provides linear regression and related features. 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | 26 | #include "src/Eigen2Support/LeastSquares.h" 27 | 28 | #include "src/Core/util/ReenableStupidWarnings.h" 29 | 30 | #endif // EIGEN2_SUPPORT 31 | 32 | #endif // EIGEN_REGRESSION_MODULE_H 33 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/MetisSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_METISSUPPORT_MODULE_H 2 | #define EIGEN_METISSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | extern "C" { 9 | #include 10 | } 11 | 12 | 13 | /** \ingroup Support_modules 14 | * \defgroup MetisSupport_Module MetisSupport module 15 | * 16 | * \code 17 | * #include 18 | * \endcode 19 | * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 20 | * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink 21 | */ 22 | 23 | 24 | #include "src/MetisSupport/MetisSupport.h" 25 | 26 | #include "src/Core/util/ReenableStupidWarnings.h" 27 | 28 | #endif // EIGEN_METISSUPPORT_MODULE_H 29 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/OrderingMethods: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_ORDERINGMETHODS_MODULE_H 2 | #define EIGEN_ORDERINGMETHODS_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | /** 9 | * \defgroup OrderingMethods_Module OrderingMethods module 10 | * 11 | * This module is currently for internal use only 12 | * 13 | * It defines various built-in and external ordering methods for sparse matrices. 14 | * They are typically used to reduce the number of elements during 15 | * the sparse matrix decomposition (LLT, LU, QR). 16 | * Precisely, in a preprocessing step, a permutation matrix P is computed using 17 | * those ordering methods and applied to the columns of the matrix. 18 | * Using for instance the sparse Cholesky decomposition, it is expected that 19 | * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). 20 | * 21 | * 22 | * Usage : 23 | * \code 24 | * #include 25 | * \endcode 26 | * 27 | * A simple usage is as a template parameter in the sparse decomposition classes : 28 | * 29 | * \code 30 | * SparseLU > solver; 31 | * \endcode 32 | * 33 | * \code 34 | * SparseQR > solver; 35 | * \endcode 36 | * 37 | * It is possible as well to call directly a particular ordering method for your own purpose, 38 | * \code 39 | * AMDOrdering ordering; 40 | * PermutationMatrix perm; 41 | * SparseMatrix A; 42 | * //Fill the matrix ... 43 | * 44 | * ordering(A, perm); // Call AMD 45 | * \endcode 46 | * 47 | * \note Some of these methods (like AMD or METIS), need the sparsity pattern 48 | * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 49 | * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. 50 | * If your matrix is already symmetric (at leat in structure), you can avoid that 51 | * by calling the method with a SelfAdjointView type. 52 | * 53 | * \code 54 | * // Call the ordering on the pattern of the lower triangular matrix A 55 | * ordering(A.selfadjointView(), perm); 56 | * \endcode 57 | */ 58 | 59 | #ifndef EIGEN_MPL2_ONLY 60 | #include "src/OrderingMethods/Amd.h" 61 | #endif 62 | 63 | #include "src/OrderingMethods/Ordering.h" 64 | #include "src/Core/util/ReenableStupidWarnings.h" 65 | 66 | #endif // EIGEN_ORDERINGMETHODS_MODULE_H 67 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/PaStiXSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_PASTIXSUPPORT_MODULE_H 2 | #define EIGEN_PASTIXSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include 9 | extern "C" { 10 | #include 11 | #include 12 | } 13 | 14 | #ifdef complex 15 | #undef complex 16 | #endif 17 | 18 | /** \ingroup Support_modules 19 | * \defgroup PaStiXSupport_Module PaStiXSupport module 20 | * 21 | * This module provides an interface to the PaSTiX library. 22 | * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. 23 | * It provides the two following main factorization classes: 24 | * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. 25 | * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. 26 | * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | * 32 | * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies. 33 | * The dependencies depend on how PaSTiX has been compiled. 34 | * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. 35 | * 36 | */ 37 | 38 | #include "src/misc/Solve.h" 39 | #include "src/misc/SparseSolve.h" 40 | 41 | #include "src/PaStiXSupport/PaStiXSupport.h" 42 | 43 | 44 | #include "src/Core/util/ReenableStupidWarnings.h" 45 | 46 | #endif // EIGEN_PASTIXSUPPORT_MODULE_H 47 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/PardisoSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_PARDISOSUPPORT_MODULE_H 2 | #define EIGEN_PARDISOSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include 9 | 10 | #include 11 | 12 | /** \ingroup Support_modules 13 | * \defgroup PardisoSupport_Module PardisoSupport module 14 | * 15 | * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. 16 | * 17 | * \code 18 | * #include 19 | * \endcode 20 | * 21 | * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies. 22 | * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. 23 | * 24 | */ 25 | 26 | #include "src/PardisoSupport/PardisoSupport.h" 27 | 28 | #include "src/Core/util/ReenableStupidWarnings.h" 29 | 30 | #endif // EIGEN_PARDISOSUPPORT_MODULE_H 31 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/QR: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_QR_MODULE_H 2 | #define EIGEN_QR_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include "Cholesky" 9 | #include "Jacobi" 10 | #include "Householder" 11 | 12 | /** \defgroup QR_Module QR module 13 | * 14 | * 15 | * 16 | * This module provides various QR decompositions 17 | * This module also provides some MatrixBase methods, including: 18 | * - MatrixBase::qr(), 19 | * 20 | * \code 21 | * #include 22 | * \endcode 23 | */ 24 | 25 | #include "src/misc/Solve.h" 26 | #include "src/QR/HouseholderQR.h" 27 | #include "src/QR/FullPivHouseholderQR.h" 28 | #include "src/QR/ColPivHouseholderQR.h" 29 | #ifdef EIGEN_USE_LAPACKE 30 | #include "src/QR/HouseholderQR_MKL.h" 31 | #include "src/QR/ColPivHouseholderQR_MKL.h" 32 | #endif 33 | 34 | #ifdef EIGEN2_SUPPORT 35 | #include "src/Eigen2Support/QR.h" 36 | #endif 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #ifdef EIGEN2_SUPPORT 41 | #include "Eigenvalues" 42 | #endif 43 | 44 | #endif // EIGEN_QR_MODULE_H 45 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 46 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/QtAlignedMalloc: -------------------------------------------------------------------------------- 1 | 2 | #ifndef EIGEN_QTMALLOC_MODULE_H 3 | #define EIGEN_QTMALLOC_MODULE_H 4 | 5 | #include "Core" 6 | 7 | #if (!EIGEN_MALLOC_ALREADY_ALIGNED) 8 | 9 | #include "src/Core/util/DisableStupidWarnings.h" 10 | 11 | void *qMalloc(size_t size) 12 | { 13 | return Eigen::internal::aligned_malloc(size); 14 | } 15 | 16 | void qFree(void *ptr) 17 | { 18 | Eigen::internal::aligned_free(ptr); 19 | } 20 | 21 | void *qRealloc(void *ptr, size_t size) 22 | { 23 | void* newPtr = Eigen::internal::aligned_malloc(size); 24 | memcpy(newPtr, ptr, size); 25 | Eigen::internal::aligned_free(ptr); 26 | return newPtr; 27 | } 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif 32 | 33 | #endif // EIGEN_QTMALLOC_MODULE_H 34 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 35 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SPQRSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_SPQRSUPPORT_MODULE_H 2 | #define EIGEN_SPQRSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include "SuiteSparseQR.hpp" 9 | 10 | /** \ingroup Support_modules 11 | * \defgroup SPQRSupport_Module SuiteSparseQR module 12 | * 13 | * This module provides an interface to the SPQR library, which is part of the suitesparse package. 14 | * 15 | * \code 16 | * #include 17 | * \endcode 18 | * 19 | * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). 20 | * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules 21 | * 22 | */ 23 | 24 | #include "src/misc/Solve.h" 25 | #include "src/misc/SparseSolve.h" 26 | #include "src/CholmodSupport/CholmodSupport.h" 27 | #include "src/SPQRSupport/SuiteSparseQRSupport.h" 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SVD: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_SVD_MODULE_H 2 | #define EIGEN_SVD_MODULE_H 3 | 4 | #include "QR" 5 | #include "Householder" 6 | #include "Jacobi" 7 | 8 | #include "src/Core/util/DisableStupidWarnings.h" 9 | 10 | /** \defgroup SVD_Module SVD module 11 | * 12 | * 13 | * 14 | * This module provides SVD decomposition for matrices (both real and complex). 15 | * This decomposition is accessible via the following MatrixBase method: 16 | * - MatrixBase::jacobiSvd() 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | */ 22 | 23 | #include "src/misc/Solve.h" 24 | #include "src/SVD/JacobiSVD.h" 25 | #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) 26 | #include "src/SVD/JacobiSVD_MKL.h" 27 | #endif 28 | #include "src/SVD/UpperBidiagonalization.h" 29 | 30 | #ifdef EIGEN2_SUPPORT 31 | #include "src/Eigen2Support/SVD.h" 32 | #endif 33 | 34 | #include "src/Core/util/ReenableStupidWarnings.h" 35 | 36 | #endif // EIGEN_SVD_MODULE_H 37 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 38 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/Sparse: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_SPARSE_MODULE_H 2 | #define EIGEN_SPARSE_MODULE_H 3 | 4 | /** \defgroup Sparse_Module Sparse meta-module 5 | * 6 | * Meta-module including all related modules: 7 | * - \ref SparseCore_Module 8 | * - \ref OrderingMethods_Module 9 | * - \ref SparseCholesky_Module 10 | * - \ref SparseLU_Module 11 | * - \ref SparseQR_Module 12 | * - \ref IterativeLinearSolvers_Module 13 | * 14 | * \code 15 | * #include 16 | * \endcode 17 | */ 18 | 19 | #include "SparseCore" 20 | #include "OrderingMethods" 21 | #include "SparseCholesky" 22 | #include "SparseLU" 23 | #include "SparseQR" 24 | #include "IterativeLinearSolvers" 25 | 26 | #endif // EIGEN_SPARSE_MODULE_H 27 | 28 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SparseCholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2013 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSECHOLESKY_MODULE_H 11 | #define EIGEN_SPARSECHOLESKY_MODULE_H 12 | 13 | #include "SparseCore" 14 | #include "OrderingMethods" 15 | 16 | #include "src/Core/util/DisableStupidWarnings.h" 17 | 18 | /** 19 | * \defgroup SparseCholesky_Module SparseCholesky module 20 | * 21 | * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. 22 | * Those decompositions are accessible via the following classes: 23 | * - SimplicialLLt, 24 | * - SimplicialLDLt 25 | * 26 | * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module. 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | */ 32 | 33 | #ifdef EIGEN_MPL2_ONLY 34 | #error The SparseCholesky module has nothing to offer in MPL2 only mode 35 | #endif 36 | 37 | #include "src/misc/Solve.h" 38 | #include "src/misc/SparseSolve.h" 39 | #include "src/SparseCholesky/SimplicialCholesky.h" 40 | 41 | #ifndef EIGEN_MPL2_ONLY 42 | #include "src/SparseCholesky/SimplicialCholesky_impl.h" 43 | #endif 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_SPARSECHOLESKY_MODULE_H 48 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SparseCore: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_SPARSECORE_MODULE_H 2 | #define EIGEN_SPARSECORE_MODULE_H 3 | 4 | #include "Core" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | /** 15 | * \defgroup SparseCore_Module SparseCore module 16 | * 17 | * This module provides a sparse matrix representation, and basic associatd matrix manipulations 18 | * and operations. 19 | * 20 | * See the \ref TutorialSparse "Sparse tutorial" 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * This module depends on: Core. 27 | */ 28 | 29 | namespace Eigen { 30 | 31 | /** The type used to identify a general sparse storage. */ 32 | struct Sparse {}; 33 | 34 | } 35 | 36 | #include "src/SparseCore/SparseUtil.h" 37 | #include "src/SparseCore/SparseMatrixBase.h" 38 | #include "src/SparseCore/CompressedStorage.h" 39 | #include "src/SparseCore/AmbiVector.h" 40 | #include "src/SparseCore/SparseMatrix.h" 41 | #include "src/SparseCore/MappedSparseMatrix.h" 42 | #include "src/SparseCore/SparseVector.h" 43 | #include "src/SparseCore/SparseBlock.h" 44 | #include "src/SparseCore/SparseTranspose.h" 45 | #include "src/SparseCore/SparseCwiseUnaryOp.h" 46 | #include "src/SparseCore/SparseCwiseBinaryOp.h" 47 | #include "src/SparseCore/SparseDot.h" 48 | #include "src/SparseCore/SparsePermutation.h" 49 | #include "src/SparseCore/SparseRedux.h" 50 | #include "src/SparseCore/SparseFuzzy.h" 51 | #include "src/SparseCore/ConservativeSparseSparseProduct.h" 52 | #include "src/SparseCore/SparseSparseProductWithPruning.h" 53 | #include "src/SparseCore/SparseProduct.h" 54 | #include "src/SparseCore/SparseDenseProduct.h" 55 | #include "src/SparseCore/SparseDiagonalProduct.h" 56 | #include "src/SparseCore/SparseTriangularView.h" 57 | #include "src/SparseCore/SparseSelfAdjointView.h" 58 | #include "src/SparseCore/TriangularSolver.h" 59 | #include "src/SparseCore/SparseView.h" 60 | 61 | #include "src/Core/util/ReenableStupidWarnings.h" 62 | 63 | #endif // EIGEN_SPARSECORE_MODULE_H 64 | 65 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SparseLU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // Copyright (C) 2012 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_SPARSELU_MODULE_H 12 | #define EIGEN_SPARSELU_MODULE_H 13 | 14 | #include "SparseCore" 15 | 16 | /** 17 | * \defgroup SparseLU_Module SparseLU module 18 | * This module defines a supernodal factorization of general sparse matrices. 19 | * The code is fully optimized for supernode-panel updates with specialized kernels. 20 | * Please, see the documentation of the SparseLU class for more details. 21 | */ 22 | 23 | #include "src/misc/Solve.h" 24 | #include "src/misc/SparseSolve.h" 25 | 26 | // Ordering interface 27 | #include "OrderingMethods" 28 | 29 | #include "src/SparseLU/SparseLU_gemm_kernel.h" 30 | 31 | #include "src/SparseLU/SparseLU_Structs.h" 32 | #include "src/SparseLU/SparseLU_SupernodalMatrix.h" 33 | #include "src/SparseLU/SparseLUImpl.h" 34 | #include "src/SparseCore/SparseColEtree.h" 35 | #include "src/SparseLU/SparseLU_Memory.h" 36 | #include "src/SparseLU/SparseLU_heap_relax_snode.h" 37 | #include "src/SparseLU/SparseLU_relax_snode.h" 38 | #include "src/SparseLU/SparseLU_pivotL.h" 39 | #include "src/SparseLU/SparseLU_panel_dfs.h" 40 | #include "src/SparseLU/SparseLU_kernel_bmod.h" 41 | #include "src/SparseLU/SparseLU_panel_bmod.h" 42 | #include "src/SparseLU/SparseLU_column_dfs.h" 43 | #include "src/SparseLU/SparseLU_column_bmod.h" 44 | #include "src/SparseLU/SparseLU_copy_to_ucol.h" 45 | #include "src/SparseLU/SparseLU_pruneL.h" 46 | #include "src/SparseLU/SparseLU_Utils.h" 47 | #include "src/SparseLU/SparseLU.h" 48 | 49 | #endif // EIGEN_SPARSELU_MODULE_H 50 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SparseQR: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_SPARSEQR_MODULE_H 2 | #define EIGEN_SPARSEQR_MODULE_H 3 | 4 | #include "SparseCore" 5 | #include "OrderingMethods" 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | /** \defgroup SparseQR_Module SparseQR module 9 | * \brief Provides QR decomposition for sparse matrices 10 | * 11 | * This module provides a simplicial version of the left-looking Sparse QR decomposition. 12 | * The columns of the input matrix should be reordered to limit the fill-in during the 13 | * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. 14 | * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 15 | * of built-in and external ordering methods. 16 | * 17 | * \code 18 | * #include 19 | * \endcode 20 | * 21 | * 22 | */ 23 | 24 | #include "src/misc/Solve.h" 25 | #include "src/misc/SparseSolve.h" 26 | 27 | #include "OrderingMethods" 28 | #include "src/SparseCore/SparseColEtree.h" 29 | #include "src/SparseQR/SparseQR.h" 30 | 31 | #include "src/Core/util/ReenableStupidWarnings.h" 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/StdDeque: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDDEQUE_MODULE_H 12 | #define EIGEN_STDDEQUE_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdDeque.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDDEQUE_MODULE_H 28 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/StdList: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Hauke Heibel 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_STDLIST_MODULE_H 11 | #define EIGEN_STDLIST_MODULE_H 12 | 13 | #include "Core" 14 | #include 15 | 16 | #if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */ 17 | 18 | #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) 19 | 20 | #else 21 | 22 | #include "src/StlSupport/StdList.h" 23 | 24 | #endif 25 | 26 | #endif // EIGEN_STDLIST_MODULE_H 27 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/StdVector: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDVECTOR_MODULE_H 12 | #define EIGEN_STDVECTOR_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdVector.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDVECTOR_MODULE_H 28 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/SuperLUSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H 2 | #define EIGEN_SUPERLUSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | #ifdef EMPTY 9 | #define EIGEN_EMPTY_WAS_ALREADY_DEFINED 10 | #endif 11 | 12 | typedef int int_t; 13 | #include 14 | #include 15 | #include 16 | 17 | // slu_util.h defines a preprocessor token named EMPTY which is really polluting, 18 | // so we remove it in favor of a SUPERLU_EMPTY token. 19 | // If EMPTY was already defined then we don't undef it. 20 | 21 | #if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) 22 | # undef EIGEN_EMPTY_WAS_ALREADY_DEFINED 23 | #elif defined(EMPTY) 24 | # undef EMPTY 25 | #endif 26 | 27 | #define SUPERLU_EMPTY (-1) 28 | 29 | namespace Eigen { struct SluMatrix; } 30 | 31 | /** \ingroup Support_modules 32 | * \defgroup SuperLUSupport_Module SuperLUSupport module 33 | * 34 | * This module provides an interface to the SuperLU library. 35 | * It provides the following factorization class: 36 | * - class SuperLU: a supernodal sequential LU factorization. 37 | * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). 38 | * 39 | * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. 40 | * 41 | * \code 42 | * #include 43 | * \endcode 44 | * 45 | * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies. 46 | * The dependencies depend on how superlu has been compiled. 47 | * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. 48 | * 49 | */ 50 | 51 | #include "src/misc/Solve.h" 52 | #include "src/misc/SparseSolve.h" 53 | 54 | #include "src/SuperLUSupport/SuperLUSupport.h" 55 | 56 | 57 | #include "src/Core/util/ReenableStupidWarnings.h" 58 | 59 | #endif // EIGEN_SUPERLUSUPPORT_MODULE_H 60 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/UmfPackSupport: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H 2 | #define EIGEN_UMFPACKSUPPORT_MODULE_H 3 | 4 | #include "SparseCore" 5 | 6 | #include "src/Core/util/DisableStupidWarnings.h" 7 | 8 | extern "C" { 9 | #include 10 | } 11 | 12 | /** \ingroup Support_modules 13 | * \defgroup UmfPackSupport_Module UmfPackSupport module 14 | * 15 | * This module provides an interface to the UmfPack library which is part of the suitesparse package. 16 | * It provides the following factorization class: 17 | * - class UmfPackLU: a multifrontal sequential LU factorization. 18 | * 19 | * \code 20 | * #include 21 | * \endcode 22 | * 23 | * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies. 24 | * The dependencies depend on how umfpack has been compiled. 25 | * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. 26 | * 27 | */ 28 | 29 | #include "src/misc/Solve.h" 30 | #include "src/misc/SparseSolve.h" 31 | 32 | #include "src/UmfPackSupport/UmfPackSupport.h" 33 | 34 | #include "src/Core/util/ReenableStupidWarnings.h" 35 | 36 | #endif // EIGEN_UMFPACKSUPPORT_MODULE_H 37 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/eigen/Eigen/src/.DS_Store -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b012417ab00d06768dc6be6734a8a38226d8a5cc7035d94e24b56674cc22e927 3 | size 308 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Cholesky/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:9c08213b3514c3e3e6407e6cc17bfd7813c335ef6c25f02dd92399162560dc04 3 | size 153 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/CholmodSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:092799c2693794ba318e683c68c79c76f445eaaf39996fd269ad86402e236641 3 | size 172 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:aa698f6dd6d7f1d6c03d4413eaf2acf789003a18f20990f96888a2246d553734 3 | size 215 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/CoreIterators.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_COREITERATORS_H 11 | #define EIGEN_COREITERATORS_H 12 | 13 | namespace Eigen { 14 | 15 | /* This file contains the respective InnerIterator definition of the expressions defined in Eigen/Core 16 | */ 17 | 18 | /** \ingroup SparseCore_Module 19 | * \class InnerIterator 20 | * \brief An InnerIterator allows to loop over the element of a sparse (or dense) matrix or expression 21 | * 22 | * todo 23 | */ 24 | 25 | // generic version for dense matrix and expressions 26 | template class DenseBase::InnerIterator 27 | { 28 | protected: 29 | typedef typename Derived::Scalar Scalar; 30 | typedef typename Derived::Index Index; 31 | 32 | enum { IsRowMajor = (Derived::Flags&RowMajorBit)==RowMajorBit }; 33 | public: 34 | EIGEN_STRONG_INLINE InnerIterator(const Derived& expr, Index outer) 35 | : m_expression(expr), m_inner(0), m_outer(outer), m_end(expr.innerSize()) 36 | {} 37 | 38 | EIGEN_STRONG_INLINE Scalar value() const 39 | { 40 | return (IsRowMajor) ? m_expression.coeff(m_outer, m_inner) 41 | : m_expression.coeff(m_inner, m_outer); 42 | } 43 | 44 | EIGEN_STRONG_INLINE InnerIterator& operator++() { m_inner++; return *this; } 45 | 46 | EIGEN_STRONG_INLINE Index index() const { return m_inner; } 47 | inline Index row() const { return IsRowMajor ? m_outer : index(); } 48 | inline Index col() const { return IsRowMajor ? index() : m_outer; } 49 | 50 | EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; } 51 | 52 | protected: 53 | const Derived& m_expression; 54 | Index m_inner; 55 | const Index m_outer; 56 | const Index m_end; 57 | }; 58 | 59 | } // end namespace Eigen 60 | 61 | #endif // EIGEN_COREITERATORS_H 62 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/arch/AltiVec/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:592b8b5770f960ccaf22807f4c2d66041b3e2a9c53a7c8c74bfcd544854f1bb7 3 | size 178 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/arch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3a433e47e159feb23a69f00b528e707ce00e2149be80959daa05164a343b0e2e 3 | size 97 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/arch/Default/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4358275f55f4a8e0f1a74ac1d0f1e413e1772fb29b9bb40868cdd3448451e880 3 | size 178 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/arch/Default/Settings.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | 12 | /* All the parameters defined in this file can be specialized in the 13 | * architecture specific files, and/or by the user. 14 | * More to come... */ 15 | 16 | #ifndef EIGEN_DEFAULT_SETTINGS_H 17 | #define EIGEN_DEFAULT_SETTINGS_H 18 | 19 | /** Defines the maximal loop size to enable meta unrolling of loops. 20 | * Note that the value here is expressed in Eigen's own notion of "number of FLOPS", 21 | * it does not correspond to the number of iterations or the number of instructions 22 | */ 23 | #ifndef EIGEN_UNROLLING_LIMIT 24 | #define EIGEN_UNROLLING_LIMIT 100 25 | #endif 26 | 27 | /** Defines the threshold between a "small" and a "large" matrix. 28 | * This threshold is mainly used to select the proper product implementation. 29 | */ 30 | #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 31 | #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8 32 | #endif 33 | 34 | /** Defines the maximal width of the blocks used in the triangular product and solver 35 | * for vectors (level 2 blas xTRMV and xTRSV). The default is 8. 36 | */ 37 | #ifndef EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 38 | #define EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 8 39 | #endif 40 | 41 | 42 | /** Defines the default number of registers available for that architecture. 43 | * Currently it must be 8 or 16. Other values will fail. 44 | */ 45 | #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 46 | #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8 47 | #endif 48 | 49 | #endif // EIGEN_DEFAULT_SETTINGS_H 50 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/arch/NEON/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:5f7e9d3dc61745a9866fdb4bea3a0b071eed6e3b28a844f5f6ae009ff91e8764 3 | size 169 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/arch/SSE/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e45d38111e54e01fc8dd72e363a20b443df4203e5d6bb0f8bee3bd9be561fe4d 3 | size 166 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/products/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3ce52d66e3753088e35d259bbbd72ffcf943690d960bba8bea037520e4636162 3 | size 166 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:8f6366c65c3ab620577f269a334ef3e622015306a094d34a5b447028b2b58d55 3 | size 157 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/util/DisableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_WARNINGS_DISABLED 2 | #define EIGEN_WARNINGS_DISABLED 3 | 4 | #ifdef _MSC_VER 5 | // 4100 - unreferenced formal parameter (occurred e.g. in aligned_allocator::destroy(pointer p)) 6 | // 4101 - unreferenced local variable 7 | // 4127 - conditional expression is constant 8 | // 4181 - qualifier applied to reference type ignored 9 | // 4211 - nonstandard extension used : redefined extern to static 10 | // 4244 - 'argument' : conversion from 'type1' to 'type2', possible loss of data 11 | // 4273 - QtAlignedMalloc, inconsistent DLL linkage 12 | // 4324 - structure was padded due to declspec(align()) 13 | // 4512 - assignment operator could not be generated 14 | // 4522 - 'class' : multiple assignment operators specified 15 | // 4700 - uninitialized local variable 'xyz' used 16 | // 4717 - 'function' : recursive on all control paths, function will cause runtime stack overflow 17 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 18 | #pragma warning( push ) 19 | #endif 20 | #pragma warning( disable : 4100 4101 4127 4181 4211 4244 4273 4324 4512 4522 4700 4717 ) 21 | #elif defined __INTEL_COMPILER 22 | // 2196 - routine is both "inline" and "noinline" ("noinline" assumed) 23 | // ICC 12 generates this warning even without any inline keyword, when defining class methods 'inline' i.e. inside of class body 24 | // typedef that may be a reference type. 25 | // 279 - controlling expression is constant 26 | // ICC 12 generates this warning on assert(constant_expression_depending_on_template_params) and frankly this is a legitimate use case. 27 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 28 | #pragma warning push 29 | #endif 30 | #pragma warning disable 2196 279 31 | #elif defined __clang__ 32 | // -Wconstant-logical-operand - warning: use of logical && with constant operand; switch to bitwise & or remove constant 33 | // this is really a stupid warning as it warns on compile-time expressions involving enums 34 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 35 | #pragma clang diagnostic push 36 | #endif 37 | #pragma clang diagnostic ignored "-Wconstant-logical-operand" 38 | #endif 39 | 40 | #endif // not EIGEN_WARNINGS_DISABLED 41 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/util/NonMPL2.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_MPL2_ONLY 2 | #error Including non-MPL2 code in EIGEN_MPL2_ONLY mode 3 | #endif 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Core/util/ReenableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_WARNINGS_DISABLED 2 | #undef EIGEN_WARNINGS_DISABLED 3 | 4 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 5 | #ifdef _MSC_VER 6 | #pragma warning( pop ) 7 | #elif defined __INTEL_COMPILER 8 | #pragma warning pop 9 | #elif defined __clang__ 10 | #pragma clang diagnostic pop 11 | #endif 12 | #endif 13 | 14 | #endif // EIGEN_WARNINGS_DISABLED 15 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:ae52d87dbdfbd9396cfb2fd351a9c36c6fe060b6731dbc22aed837e0a0d460ec 3 | size 195 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/Geometry/All.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN2_GEOMETRY_MODULE_H 2 | #define EIGEN2_GEOMETRY_MODULE_H 3 | 4 | #include 5 | 6 | #ifndef M_PI 7 | #define M_PI 3.14159265358979323846 8 | #endif 9 | 10 | #if EIGEN2_SUPPORT_STAGE < STAGE20_RESOLVE_API_CONFLICTS 11 | #include "RotationBase.h" 12 | #include "Rotation2D.h" 13 | #include "Quaternion.h" 14 | #include "AngleAxis.h" 15 | #include "Transform.h" 16 | #include "Translation.h" 17 | #include "Scaling.h" 18 | #include "AlignedBox.h" 19 | #include "Hyperplane.h" 20 | #include "ParametrizedLine.h" 21 | #endif 22 | 23 | 24 | #define RotationBase eigen2_RotationBase 25 | #define Rotation2D eigen2_Rotation2D 26 | #define Rotation2Df eigen2_Rotation2Df 27 | #define Rotation2Dd eigen2_Rotation2Dd 28 | 29 | #define Quaternion eigen2_Quaternion 30 | #define Quaternionf eigen2_Quaternionf 31 | #define Quaterniond eigen2_Quaterniond 32 | 33 | #define AngleAxis eigen2_AngleAxis 34 | #define AngleAxisf eigen2_AngleAxisf 35 | #define AngleAxisd eigen2_AngleAxisd 36 | 37 | #define Transform eigen2_Transform 38 | #define Transform2f eigen2_Transform2f 39 | #define Transform2d eigen2_Transform2d 40 | #define Transform3f eigen2_Transform3f 41 | #define Transform3d eigen2_Transform3d 42 | 43 | #define Translation eigen2_Translation 44 | #define Translation2f eigen2_Translation2f 45 | #define Translation2d eigen2_Translation2d 46 | #define Translation3f eigen2_Translation3f 47 | #define Translation3d eigen2_Translation3d 48 | 49 | #define Scaling eigen2_Scaling 50 | #define Scaling2f eigen2_Scaling2f 51 | #define Scaling2d eigen2_Scaling2d 52 | #define Scaling3f eigen2_Scaling3f 53 | #define Scaling3d eigen2_Scaling3d 54 | 55 | #define AlignedBox eigen2_AlignedBox 56 | 57 | #define Hyperplane eigen2_Hyperplane 58 | #define ParametrizedLine eigen2_ParametrizedLine 59 | 60 | #define ei_toRotationMatrix eigen2_ei_toRotationMatrix 61 | #define ei_quaternion_assign_impl eigen2_ei_quaternion_assign_impl 62 | #define ei_transform_product_impl eigen2_ei_transform_product_impl 63 | 64 | #include "RotationBase.h" 65 | #include "Rotation2D.h" 66 | #include "Quaternion.h" 67 | #include "AngleAxis.h" 68 | #include "Transform.h" 69 | #include "Translation.h" 70 | #include "Scaling.h" 71 | #include "AlignedBox.h" 72 | #include "Hyperplane.h" 73 | #include "ParametrizedLine.h" 74 | 75 | #undef ei_toRotationMatrix 76 | #undef ei_quaternion_assign_impl 77 | #undef ei_transform_product_impl 78 | 79 | #undef RotationBase 80 | #undef Rotation2D 81 | #undef Rotation2Df 82 | #undef Rotation2Dd 83 | 84 | #undef Quaternion 85 | #undef Quaternionf 86 | #undef Quaterniond 87 | 88 | #undef AngleAxis 89 | #undef AngleAxisf 90 | #undef AngleAxisd 91 | 92 | #undef Transform 93 | #undef Transform2f 94 | #undef Transform2d 95 | #undef Transform3f 96 | #undef Transform3d 97 | 98 | #undef Translation 99 | #undef Translation2f 100 | #undef Translation2d 101 | #undef Translation3f 102 | #undef Translation3d 103 | 104 | #undef Scaling 105 | #undef Scaling2f 106 | #undef Scaling2d 107 | #undef Scaling3f 108 | #undef Scaling3d 109 | 110 | #undef AlignedBox 111 | 112 | #undef Hyperplane 113 | #undef ParametrizedLine 114 | 115 | #endif // EIGEN2_GEOMETRY_MODULE_H 116 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:929c65ffe6f710fb79c06bf40d080baf7d66ef2487a47429f79505b003031e22 3 | size 179 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/Lazy.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_LAZY_H 11 | #define EIGEN_LAZY_H 12 | 13 | namespace Eigen { 14 | 15 | /** \deprecated it is only used by lazy() which is deprecated 16 | * 17 | * \returns an expression of *this with added flags 18 | * 19 | * Example: \include MatrixBase_marked.cpp 20 | * Output: \verbinclude MatrixBase_marked.out 21 | * 22 | * \sa class Flagged, extract(), part() 23 | */ 24 | template 25 | template 26 | inline const Flagged 27 | MatrixBase::marked() const 28 | { 29 | return derived(); 30 | } 31 | 32 | /** \deprecated use MatrixBase::noalias() 33 | * 34 | * \returns an expression of *this with the EvalBeforeAssigningBit flag removed. 35 | * 36 | * Example: \include MatrixBase_lazy.cpp 37 | * Output: \verbinclude MatrixBase_lazy.out 38 | * 39 | * \sa class Flagged, marked() 40 | */ 41 | template 42 | inline const Flagged 43 | MatrixBase::lazy() const 44 | { 45 | return derived(); 46 | } 47 | 48 | 49 | /** \internal 50 | * Overloaded to perform an efficient C += (A*B).lazy() */ 51 | template 52 | template 53 | Derived& MatrixBase::operator+=(const Flagged, 0, 54 | EvalBeforeAssigningBit>& other) 55 | { 56 | other._expression().derived().addTo(derived()); return derived(); 57 | } 58 | 59 | /** \internal 60 | * Overloaded to perform an efficient C -= (A*B).lazy() */ 61 | template 62 | template 63 | Derived& MatrixBase::operator-=(const Flagged, 0, 64 | EvalBeforeAssigningBit>& other) 65 | { 66 | other._expression().derived().subTo(derived()); return derived(); 67 | } 68 | 69 | } // end namespace Eigen 70 | 71 | #endif // EIGEN_LAZY_H 72 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/Macros.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2011 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN2_MACROS_H 11 | #define EIGEN2_MACROS_H 12 | 13 | #define ei_assert eigen_assert 14 | #define ei_internal_assert eigen_internal_assert 15 | 16 | #define EIGEN_ALIGN_128 EIGEN_ALIGN16 17 | 18 | #define EIGEN_ARCH_WANTS_ALIGNMENT EIGEN_ALIGN_STATICALLY 19 | 20 | #endif // EIGEN2_MACROS_H 21 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN2_MATH_FUNCTIONS_H 11 | #define EIGEN2_MATH_FUNCTIONS_H 12 | 13 | namespace Eigen { 14 | 15 | template inline typename NumTraits::Real ei_real(const T& x) { return numext::real(x); } 16 | template inline typename NumTraits::Real ei_imag(const T& x) { return numext::imag(x); } 17 | template inline T ei_conj(const T& x) { return numext::conj(x); } 18 | template inline typename NumTraits::Real ei_abs (const T& x) { using std::abs; return abs(x); } 19 | template inline typename NumTraits::Real ei_abs2(const T& x) { return numext::abs2(x); } 20 | template inline T ei_sqrt(const T& x) { using std::sqrt; return sqrt(x); } 21 | template inline T ei_exp (const T& x) { using std::exp; return exp(x); } 22 | template inline T ei_log (const T& x) { using std::log; return log(x); } 23 | template inline T ei_sin (const T& x) { using std::sin; return sin(x); } 24 | template inline T ei_cos (const T& x) { using std::cos; return cos(x); } 25 | template inline T ei_atan2(const T& x,const T& y) { using std::atan2; return atan2(x,y); } 26 | template inline T ei_pow (const T& x,const T& y) { return numext::pow(x,y); } 27 | template inline T ei_random () { return internal::random(); } 28 | template inline T ei_random (const T& x, const T& y) { return internal::random(x, y); } 29 | 30 | template inline T precision () { return NumTraits::dummy_precision(); } 31 | template inline T machine_epsilon () { return NumTraits::epsilon(); } 32 | 33 | 34 | template 35 | inline bool ei_isMuchSmallerThan(const Scalar& x, const OtherScalar& y, 36 | typename NumTraits::Real precision = NumTraits::dummy_precision()) 37 | { 38 | return internal::isMuchSmallerThan(x, y, precision); 39 | } 40 | 41 | template 42 | inline bool ei_isApprox(const Scalar& x, const Scalar& y, 43 | typename NumTraits::Real precision = NumTraits::dummy_precision()) 44 | { 45 | return internal::isApprox(x, y, precision); 46 | } 47 | 48 | template 49 | inline bool ei_isApproxOrLessThan(const Scalar& x, const Scalar& y, 50 | typename NumTraits::Real precision = NumTraits::dummy_precision()) 51 | { 52 | return internal::isApproxOrLessThan(x, y, precision); 53 | } 54 | 55 | } // end namespace Eigen 56 | 57 | #endif // EIGEN2_MATH_FUNCTIONS_H 58 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/Memory.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2011 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN2_MEMORY_H 11 | #define EIGEN2_MEMORY_H 12 | 13 | namespace Eigen { 14 | 15 | inline void* ei_aligned_malloc(size_t size) { return internal::aligned_malloc(size); } 16 | inline void ei_aligned_free(void *ptr) { internal::aligned_free(ptr); } 17 | inline void* ei_aligned_realloc(void *ptr, size_t new_size, size_t old_size) { return internal::aligned_realloc(ptr, new_size, old_size); } 18 | inline void* ei_handmade_aligned_malloc(size_t size) { return internal::handmade_aligned_malloc(size); } 19 | inline void ei_handmade_aligned_free(void *ptr) { internal::handmade_aligned_free(ptr); } 20 | 21 | template inline void* ei_conditional_aligned_malloc(size_t size) 22 | { 23 | return internal::conditional_aligned_malloc(size); 24 | } 25 | template inline void ei_conditional_aligned_free(void *ptr) 26 | { 27 | internal::conditional_aligned_free(ptr); 28 | } 29 | template inline void* ei_conditional_aligned_realloc(void* ptr, size_t new_size, size_t old_size) 30 | { 31 | return internal::conditional_aligned_realloc(ptr, new_size, old_size); 32 | } 33 | 34 | template inline T* ei_aligned_new(size_t size) 35 | { 36 | return internal::aligned_new(size); 37 | } 38 | template inline void ei_aligned_delete(T *ptr, size_t size) 39 | { 40 | return internal::aligned_delete(ptr, size); 41 | } 42 | 43 | } // end namespace Eigen 44 | 45 | #endif // EIGEN2_MACROS_H 46 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/Meta.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2011 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN2_META_H 11 | #define EIGEN2_META_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | struct ei_traits : internal::traits 17 | {}; 18 | 19 | struct ei_meta_true { enum { ret = 1 }; }; 20 | struct ei_meta_false { enum { ret = 0 }; }; 21 | 22 | template 23 | struct ei_meta_if { typedef Then ret; }; 24 | 25 | template 26 | struct ei_meta_if { typedef Else ret; }; 27 | 28 | template struct ei_is_same_type { enum { ret = 0 }; }; 29 | template struct ei_is_same_type { enum { ret = 1 }; }; 30 | 31 | template struct ei_unref { typedef T type; }; 32 | template struct ei_unref { typedef T type; }; 33 | 34 | template struct ei_unpointer { typedef T type; }; 35 | template struct ei_unpointer { typedef T type; }; 36 | template struct ei_unpointer { typedef T type; }; 37 | 38 | template struct ei_unconst { typedef T type; }; 39 | template struct ei_unconst { typedef T type; }; 40 | template struct ei_unconst { typedef T & type; }; 41 | template struct ei_unconst { typedef T * type; }; 42 | 43 | template struct ei_cleantype { typedef T type; }; 44 | template struct ei_cleantype { typedef typename ei_cleantype::type type; }; 45 | template struct ei_cleantype { typedef typename ei_cleantype::type type; }; 46 | template struct ei_cleantype { typedef typename ei_cleantype::type type; }; 47 | template struct ei_cleantype { typedef typename ei_cleantype::type type; }; 48 | template struct ei_cleantype { typedef typename ei_cleantype::type type; }; 49 | 50 | /** \internal In short, it computes int(sqrt(\a Y)) with \a Y an integer. 51 | * Usage example: \code ei_meta_sqrt<1023>::ret \endcode 52 | */ 53 | template Y))) > 57 | // use ?: instead of || just to shut up a stupid gcc 4.3 warning 58 | class ei_meta_sqrt 59 | { 60 | enum { 61 | MidX = (InfX+SupX)/2, 62 | TakeInf = MidX*MidX > Y ? 1 : 0, 63 | NewInf = int(TakeInf) ? InfX : int(MidX), 64 | NewSup = int(TakeInf) ? int(MidX) : SupX 65 | }; 66 | public: 67 | enum { ret = ei_meta_sqrt::ret }; 68 | }; 69 | 70 | template 71 | class ei_meta_sqrt { public: enum { ret = (SupX*SupX <= Y) ? SupX : InfX }; }; 72 | 73 | } // end namespace Eigen 74 | 75 | #endif // EIGEN2_META_H 76 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/QR.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // Copyright (C) 2011 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN2_QR_H 12 | #define EIGEN2_QR_H 13 | 14 | namespace Eigen { 15 | 16 | template 17 | class QR : public HouseholderQR 18 | { 19 | public: 20 | 21 | typedef HouseholderQR Base; 22 | typedef Block MatrixRBlockType; 23 | 24 | QR() : Base() {} 25 | 26 | template 27 | explicit QR(const T& t) : Base(t) {} 28 | 29 | template 30 | bool solve(const MatrixBase& b, ResultType *result) const 31 | { 32 | *result = static_cast(this)->solve(b); 33 | return true; 34 | } 35 | 36 | MatrixType matrixQ(void) const { 37 | MatrixType ret = MatrixType::Identity(this->rows(), this->cols()); 38 | ret = this->householderQ() * ret; 39 | return ret; 40 | } 41 | 42 | bool isFullRank() const { 43 | return true; 44 | } 45 | 46 | const TriangularView 47 | matrixR(void) const 48 | { 49 | int cols = this->cols(); 50 | return MatrixRBlockType(this->matrixQR(), 0, 0, cols, cols).template triangularView(); 51 | } 52 | }; 53 | 54 | /** \return the QR decomposition of \c *this. 55 | * 56 | * \sa class QR 57 | */ 58 | template 59 | const QR::PlainObject> 60 | MatrixBase::qr() const 61 | { 62 | return QR(eval()); 63 | } 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN2_QR_H 68 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/TriangularSolver.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TRIANGULAR_SOLVER2_H 11 | #define EIGEN_TRIANGULAR_SOLVER2_H 12 | 13 | namespace Eigen { 14 | 15 | const unsigned int UnitDiagBit = UnitDiag; 16 | const unsigned int SelfAdjointBit = SelfAdjoint; 17 | const unsigned int UpperTriangularBit = Upper; 18 | const unsigned int LowerTriangularBit = Lower; 19 | 20 | const unsigned int UpperTriangular = Upper; 21 | const unsigned int LowerTriangular = Lower; 22 | const unsigned int UnitUpperTriangular = UnitUpper; 23 | const unsigned int UnitLowerTriangular = UnitLower; 24 | 25 | template 26 | template 27 | typename ExpressionType::PlainObject 28 | Flagged::solveTriangular(const MatrixBase& other) const 29 | { 30 | return m_matrix.template triangularView().solve(other.derived()); 31 | } 32 | 33 | template 34 | template 35 | void Flagged::solveTriangularInPlace(const MatrixBase& other) const 36 | { 37 | m_matrix.template triangularView().solveInPlace(other.derived()); 38 | } 39 | 40 | } // end namespace Eigen 41 | 42 | #endif // EIGEN_TRIANGULAR_SOLVER2_H 43 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigen2Support/VectorBlock.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN2_VECTORBLOCK_H 12 | #define EIGEN2_VECTORBLOCK_H 13 | 14 | namespace Eigen { 15 | 16 | /** \deprecated use DenseMase::head(Index) */ 17 | template 18 | inline VectorBlock 19 | MatrixBase::start(Index size) 20 | { 21 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 22 | return VectorBlock(derived(), 0, size); 23 | } 24 | 25 | /** \deprecated use DenseMase::head(Index) */ 26 | template 27 | inline const VectorBlock 28 | MatrixBase::start(Index size) const 29 | { 30 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 31 | return VectorBlock(derived(), 0, size); 32 | } 33 | 34 | /** \deprecated use DenseMase::tail(Index) */ 35 | template 36 | inline VectorBlock 37 | MatrixBase::end(Index size) 38 | { 39 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 40 | return VectorBlock(derived(), this->size() - size, size); 41 | } 42 | 43 | /** \deprecated use DenseMase::tail(Index) */ 44 | template 45 | inline const VectorBlock 46 | MatrixBase::end(Index size) const 47 | { 48 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 49 | return VectorBlock(derived(), this->size() - size, size); 50 | } 51 | 52 | /** \deprecated use DenseMase::head() */ 53 | template 54 | template 55 | inline VectorBlock 56 | MatrixBase::start() 57 | { 58 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 59 | return VectorBlock(derived(), 0); 60 | } 61 | 62 | /** \deprecated use DenseMase::head() */ 63 | template 64 | template 65 | inline const VectorBlock 66 | MatrixBase::start() const 67 | { 68 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 69 | return VectorBlock(derived(), 0); 70 | } 71 | 72 | /** \deprecated use DenseMase::tail() */ 73 | template 74 | template 75 | inline VectorBlock 76 | MatrixBase::end() 77 | { 78 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 79 | return VectorBlock(derived(), size() - Size); 80 | } 81 | 82 | /** \deprecated use DenseMase::tail() */ 83 | template 84 | template 85 | inline const VectorBlock 86 | MatrixBase::end() const 87 | { 88 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 89 | return VectorBlock(derived(), size() - Size); 90 | } 91 | 92 | } // end namespace Eigen 93 | 94 | #endif // EIGEN2_VECTORBLOCK_H 95 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Eigenvalues/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4d73adcecd840f04907c247fd139b17c3e414a9a77c5317933fa32f5d056bb84 3 | size 162 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b6f0eebf50d22d05666dccf8ae4d8712a1be67d0fb16ba8f99ad7f59a0295f8e 3 | size 177 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Geometry/arch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:f538681c9973a0cf4aee4af8000bcc49df36469dc3ed4f71286c990d60b830aa 3 | size 168 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Householder/BlockHouseholder.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2010 Vincent Lejeune 5 | // Copyright (C) 2010 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_BLOCK_HOUSEHOLDER_H 12 | #define EIGEN_BLOCK_HOUSEHOLDER_H 13 | 14 | // This file contains some helper function to deal with block householder reflectors 15 | 16 | namespace Eigen { 17 | 18 | namespace internal { 19 | 20 | /** \internal */ 21 | template 22 | void make_block_householder_triangular_factor(TriangularFactorType& triFactor, const VectorsType& vectors, const CoeffsType& hCoeffs) 23 | { 24 | typedef typename TriangularFactorType::Index Index; 25 | typedef typename VectorsType::Scalar Scalar; 26 | const Index nbVecs = vectors.cols(); 27 | eigen_assert(triFactor.rows() == nbVecs && triFactor.cols() == nbVecs && vectors.rows()>=nbVecs); 28 | 29 | for(Index i = 0; i < nbVecs; i++) 30 | { 31 | Index rs = vectors.rows() - i; 32 | Scalar Vii = vectors(i,i); 33 | vectors.const_cast_derived().coeffRef(i,i) = Scalar(1); 34 | triFactor.col(i).head(i).noalias() = -hCoeffs(i) * vectors.block(i, 0, rs, i).adjoint() 35 | * vectors.col(i).tail(rs); 36 | vectors.const_cast_derived().coeffRef(i, i) = Vii; 37 | // FIXME add .noalias() once the triangular product can work inplace 38 | triFactor.col(i).head(i) = triFactor.block(0,0,i,i).template triangularView() 39 | * triFactor.col(i).head(i); 40 | triFactor(i,i) = hCoeffs(i); 41 | } 42 | } 43 | 44 | /** \internal */ 45 | template 46 | void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vectors, const CoeffsType& hCoeffs) 47 | { 48 | typedef typename MatrixType::Index Index; 49 | enum { TFactorSize = MatrixType::ColsAtCompileTime }; 50 | Index nbVecs = vectors.cols(); 51 | Matrix T(nbVecs,nbVecs); 52 | make_block_householder_triangular_factor(T, vectors, hCoeffs); 53 | 54 | const TriangularView& V(vectors); 55 | 56 | // A -= V T V^* A 57 | Matrix tmp = V.adjoint() * mat; 59 | // FIXME add .noalias() once the triangular product can work inplace 60 | tmp = T.template triangularView().adjoint() * tmp; 61 | mat.noalias() -= V * tmp; 62 | } 63 | 64 | } // end namespace internal 65 | 66 | } // end namespace Eigen 67 | 68 | #endif // EIGEN_BLOCK_HOUSEHOLDER_H 69 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Householder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:a0edd4738e9517a1bf15bfedba9500492b2be6539325ff53ab08119e539e20d4 3 | size 162 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/IterativeLinearSolvers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:19e6166aa63c1c0fc4af8999d80b3db1e2ffbde32ed78a6c6d83276a474d05ee 3 | size 195 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/Jacobi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:8279a05913222d45fd32fd8d29b772684746dc4ee7f1549ba19efc273dc87cd2 3 | size 147 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/LU/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:0c245574796daec63408f10d43e4db1c40789dffaa01b350b7daafeee3a99169 3 | size 160 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/LU/arch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e3d4f157b62ad0ef33ca5e5148d26e4cb12377f972d301c5d8bf0783ccd0e887 3 | size 150 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/MetisSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:a3b9a7bdc7e80032dff6ade2607ca606924d4bf776a6039424e9e41821d5b614 3 | size 166 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/OrderingMethods/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e2f4b59f3b9b592bc3428b71c628ea8180e12b70f33ca0dfb260bddce476efb4 3 | size 174 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/PaStiXSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:dfb0f2fdc1fe4985d221748846b2aafa4338dc4a13b6e0c60ed18bb7cf930a66 3 | size 169 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/PardisoSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b17c96be265826256d5b28b03059eaa005d3d6ebee4ec1ef3e3df58ef6f9ae27 3 | size 172 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/QR/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3217f8a3bd8979e08ff1d9217f8f9e6364f5b248228247f1b805167ee1ea8787 3 | size 135 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/QR/HouseholderQR_MKL.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING 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 | ******************************************************************************** 28 | * Content : Eigen bindings to Intel(R) MKL 29 | * Householder QR decomposition of a matrix w/o pivoting based on 30 | * LAPACKE_?geqrf function. 31 | ******************************************************************************** 32 | */ 33 | 34 | #ifndef EIGEN_QR_MKL_H 35 | #define EIGEN_QR_MKL_H 36 | 37 | #include "../Core/util/MKL_support.h" 38 | 39 | namespace Eigen { 40 | 41 | namespace internal { 42 | 43 | /** \internal Specialization for the data types supported by MKL */ 44 | 45 | #define EIGEN_MKL_QR_NOPIV(EIGTYPE, MKLTYPE, MKLPREFIX) \ 46 | template \ 47 | struct householder_qr_inplace_blocked \ 48 | { \ 49 | static void run(MatrixQR& mat, HCoeffs& hCoeffs, \ 50 | typename MatrixQR::Index = 32, \ 51 | typename MatrixQR::Scalar* = 0) \ 52 | { \ 53 | lapack_int m = (lapack_int) mat.rows(); \ 54 | lapack_int n = (lapack_int) mat.cols(); \ 55 | lapack_int lda = (lapack_int) mat.outerStride(); \ 56 | lapack_int matrix_order = (MatrixQR::IsRowMajor) ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ 57 | LAPACKE_##MKLPREFIX##geqrf( matrix_order, m, n, (MKLTYPE*)mat.data(), lda, (MKLTYPE*)hCoeffs.data()); \ 58 | hCoeffs.adjointInPlace(); \ 59 | } \ 60 | }; 61 | 62 | EIGEN_MKL_QR_NOPIV(double, double, d) 63 | EIGEN_MKL_QR_NOPIV(float, float, s) 64 | EIGEN_MKL_QR_NOPIV(dcomplex, MKL_Complex16, z) 65 | EIGEN_MKL_QR_NOPIV(scomplex, MKL_Complex8, c) 66 | 67 | } // end namespace internal 68 | 69 | } // end namespace Eigen 70 | 71 | #endif // EIGEN_QR_MKL_H 72 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SPQRSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:aaffd843ca479e81b3be852520991201728ec91f7e00db6bf85939ab563456d7 3 | size 163 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SVD/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:292757f296dfbb9d390e0e4a9fc8c74bc65834a0507afd3a0791a876ca72540f 3 | size 138 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseCholesky/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3b52e169ef93ca4cc8e63d4995dab47ee39f7bbe864c468c17c652035dafdebe 3 | size 171 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseCore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e8d45cd46e40bff15b06dcdc9007131844745a938e3b7fcf60cbdbeef73114d4 3 | size 160 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseCore/SparseFuzzy.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_FUZZY_H 11 | #define EIGEN_SPARSE_FUZZY_H 12 | 13 | // template 14 | // template 15 | // bool SparseMatrixBase::isApprox( 16 | // const OtherDerived& other, 17 | // typename NumTraits::Real prec 18 | // ) const 19 | // { 20 | // const typename internal::nested::type nested(derived()); 21 | // const typename internal::nested::type otherNested(other.derived()); 22 | // return (nested - otherNested).cwise().abs2().sum() 23 | // <= prec * prec * (std::min)(nested.cwise().abs2().sum(), otherNested.cwise().abs2().sum()); 24 | // } 25 | 26 | #endif // EIGEN_SPARSE_FUZZY_H 27 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseCore/SparseRedux.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSEREDUX_H 11 | #define EIGEN_SPARSEREDUX_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | typename internal::traits::Scalar 17 | SparseMatrixBase::sum() const 18 | { 19 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 20 | Scalar res(0); 21 | for (Index j=0; j 28 | typename internal::traits >::Scalar 29 | SparseMatrix<_Scalar,_Options,_Index>::sum() const 30 | { 31 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 32 | return Matrix::Map(&m_data.value(0), m_data.size()).sum(); 33 | } 34 | 35 | template 36 | typename internal::traits >::Scalar 37 | SparseVector<_Scalar,_Options,_Index>::sum() const 38 | { 39 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 40 | return Matrix::Map(&m_data.value(0), m_data.size()).sum(); 41 | } 42 | 43 | } // end namespace Eigen 44 | 45 | #endif // EIGEN_SPARSEREDUX_H 46 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseCore/SparseTranspose.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSETRANSPOSE_H 11 | #define EIGEN_SPARSETRANSPOSE_H 12 | 13 | namespace Eigen { 14 | 15 | template class TransposeImpl 16 | : public SparseMatrixBase > 17 | { 18 | typedef typename internal::remove_all::type _MatrixTypeNested; 19 | public: 20 | 21 | EIGEN_SPARSE_PUBLIC_INTERFACE(Transpose ) 22 | 23 | class InnerIterator; 24 | class ReverseInnerIterator; 25 | 26 | inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); } 27 | }; 28 | 29 | // NOTE: VC10 and VC11 trigger an ICE if don't put typename TransposeImpl:: in front of Index, 30 | // a typedef typename TransposeImpl::Index Index; 31 | // does not fix the issue. 32 | // An alternative is to define the nested class in the parent class itself. 33 | template class TransposeImpl::InnerIterator 34 | : public _MatrixTypeNested::InnerIterator 35 | { 36 | typedef typename _MatrixTypeNested::InnerIterator Base; 37 | typedef typename TransposeImpl::Index Index; 38 | public: 39 | 40 | EIGEN_STRONG_INLINE InnerIterator(const TransposeImpl& trans, typename TransposeImpl::Index outer) 41 | : Base(trans.derived().nestedExpression(), outer) 42 | {} 43 | typename TransposeImpl::Index row() const { return Base::col(); } 44 | typename TransposeImpl::Index col() const { return Base::row(); } 45 | }; 46 | 47 | template class TransposeImpl::ReverseInnerIterator 48 | : public _MatrixTypeNested::ReverseInnerIterator 49 | { 50 | typedef typename _MatrixTypeNested::ReverseInnerIterator Base; 51 | typedef typename TransposeImpl::Index Index; 52 | public: 53 | 54 | EIGEN_STRONG_INLINE ReverseInnerIterator(const TransposeImpl& xpr, typename TransposeImpl::Index outer) 55 | : Base(xpr.derived().nestedExpression(), outer) 56 | {} 57 | typename TransposeImpl::Index row() const { return Base::col(); } 58 | typename TransposeImpl::Index col() const { return Base::row(); } 59 | }; 60 | 61 | } // end namespace Eigen 62 | 63 | #endif // EIGEN_SPARSETRANSPOSE_H 64 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseCore/SparseView.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2011 Gael Guennebaud 5 | // Copyright (C) 2010 Daniel Lowengrub 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_SPARSEVIEW_H 12 | #define EIGEN_SPARSEVIEW_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template 19 | struct traits > : traits 20 | { 21 | typedef typename MatrixType::Index Index; 22 | typedef Sparse StorageKind; 23 | enum { 24 | Flags = int(traits::Flags) & (RowMajorBit) 25 | }; 26 | }; 27 | 28 | } // end namespace internal 29 | 30 | template 31 | class SparseView : public SparseMatrixBase > 32 | { 33 | typedef typename MatrixType::Nested MatrixTypeNested; 34 | typedef typename internal::remove_all::type _MatrixTypeNested; 35 | public: 36 | EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView) 37 | 38 | SparseView(const MatrixType& mat, const Scalar& m_reference = Scalar(0), 39 | typename NumTraits::Real m_epsilon = NumTraits::dummy_precision()) : 40 | m_matrix(mat), m_reference(m_reference), m_epsilon(m_epsilon) {} 41 | 42 | class InnerIterator; 43 | 44 | inline Index rows() const { return m_matrix.rows(); } 45 | inline Index cols() const { return m_matrix.cols(); } 46 | 47 | inline Index innerSize() const { return m_matrix.innerSize(); } 48 | inline Index outerSize() const { return m_matrix.outerSize(); } 49 | 50 | protected: 51 | MatrixTypeNested m_matrix; 52 | Scalar m_reference; 53 | typename NumTraits::Real m_epsilon; 54 | }; 55 | 56 | template 57 | class SparseView::InnerIterator : public _MatrixTypeNested::InnerIterator 58 | { 59 | typedef typename SparseView::Index Index; 60 | public: 61 | typedef typename _MatrixTypeNested::InnerIterator IterBase; 62 | InnerIterator(const SparseView& view, Index outer) : 63 | IterBase(view.m_matrix, outer), m_view(view) 64 | { 65 | incrementToNonZero(); 66 | } 67 | 68 | EIGEN_STRONG_INLINE InnerIterator& operator++() 69 | { 70 | IterBase::operator++(); 71 | incrementToNonZero(); 72 | return *this; 73 | } 74 | 75 | using IterBase::value; 76 | 77 | protected: 78 | const SparseView& m_view; 79 | 80 | private: 81 | void incrementToNonZero() 82 | { 83 | while((bool(*this)) && internal::isMuchSmallerThan(value(), m_view.m_reference, m_view.m_epsilon)) 84 | { 85 | IterBase::operator++(); 86 | } 87 | } 88 | }; 89 | 90 | template 91 | const SparseView MatrixBase::sparseView(const Scalar& m_reference, 92 | const typename NumTraits::Real& m_epsilon) const 93 | { 94 | return SparseView(derived(), m_reference, m_epsilon); 95 | } 96 | 97 | } // end namespace Eigen 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseLU/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:7bcc0dc0a1c5b90dc19886071782de2222e2ed039d76ccba7787ed0b30984ebc 3 | size 153 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseLU/SparseLU_Utils.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | 11 | #ifndef EIGEN_SPARSELU_UTILS_H 12 | #define EIGEN_SPARSELU_UTILS_H 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | /** 18 | * \brief Count Nonzero elements in the factors 19 | */ 20 | template 21 | void SparseLUImpl::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) 22 | { 23 | nnzL = 0; 24 | nnzU = (glu.xusub)(n); 25 | Index nsuper = (glu.supno)(n); 26 | Index jlen; 27 | Index i, j, fsupc; 28 | if (n <= 0 ) return; 29 | // For each supernode 30 | for (i = 0; i <= nsuper; i++) 31 | { 32 | fsupc = glu.xsup(i); 33 | jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 34 | 35 | for (j = fsupc; j < glu.xsup(i+1); j++) 36 | { 37 | nnzL += jlen; 38 | nnzU += j - fsupc + 1; 39 | jlen--; 40 | } 41 | } 42 | } 43 | 44 | /** 45 | * \brief Fix up the data storage lsub for L-subscripts. 46 | * 47 | * It removes the subscripts sets for structural pruning, 48 | * and applies permutation to the remaining subscripts 49 | * 50 | */ 51 | template 52 | void SparseLUImpl::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) 53 | { 54 | Index fsupc, i, j, k, jstart; 55 | 56 | Index nextl = 0; 57 | Index nsuper = (glu.supno)(n); 58 | 59 | // For each supernode 60 | for (i = 0; i <= nsuper; i++) 61 | { 62 | fsupc = glu.xsup(i); 63 | jstart = glu.xlsub(fsupc); 64 | glu.xlsub(fsupc) = nextl; 65 | for (j = jstart; j < glu.xlsub(fsupc + 1); j++) 66 | { 67 | glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A 68 | nextl++; 69 | } 70 | for (k = fsupc+1; k < glu.xsup(i+1); k++) 71 | glu.xlsub(k) = nextl; // other columns in supernode i 72 | } 73 | 74 | glu.xlsub(n) = nextl; 75 | } 76 | 77 | } // end namespace internal 78 | 79 | } // end namespace Eigen 80 | #endif // EIGEN_SPARSELU_UTILS_H 81 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseLU/SparseLU_relax_snode.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | /* This file is a modified version of heap_relax_snode.c file in SuperLU 11 | * -- SuperLU routine (version 3.0) -- 12 | * Univ. of California Berkeley, Xerox Palo Alto Research Center, 13 | * and Lawrence Berkeley National Lab. 14 | * October 15, 2003 15 | * 16 | * Copyright (c) 1994 by Xerox Corporation. All rights reserved. 17 | * 18 | * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY 19 | * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. 20 | * 21 | * Permission is hereby granted to use or copy this program for any 22 | * purpose, provided the above notices are retained on all copies. 23 | * Permission to modify the code and to distribute modified code is 24 | * granted, provided the above notices are retained, and a notice that 25 | * the code was modified is included with the above copyright notice. 26 | */ 27 | 28 | #ifndef SPARSELU_RELAX_SNODE_H 29 | #define SPARSELU_RELAX_SNODE_H 30 | 31 | namespace Eigen { 32 | 33 | namespace internal { 34 | 35 | /** 36 | * \brief Identify the initial relaxed supernodes 37 | * 38 | * This routine is applied to a column elimination tree. 39 | * It assumes that the matrix has been reordered according to the postorder of the etree 40 | * \param n the number of columns 41 | * \param et elimination tree 42 | * \param relax_columns Maximum number of columns allowed in a relaxed snode 43 | * \param descendants Number of descendants of each node in the etree 44 | * \param relax_end last column in a supernode 45 | */ 46 | template 47 | void SparseLUImpl::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end) 48 | { 49 | 50 | // compute the number of descendants of each node in the etree 51 | Index j, parent; 52 | relax_end.setConstant(emptyIdxLU); 53 | descendants.setZero(); 54 | for (j = 0; j < n; j++) 55 | { 56 | parent = et(j); 57 | if (parent != n) // not the dummy root 58 | descendants(parent) += descendants(j) + 1; 59 | } 60 | // Identify the relaxed supernodes by postorder traversal of the etree 61 | Index snode_start; // beginning of a snode 62 | for (j = 0; j < n; ) 63 | { 64 | parent = et(j); 65 | snode_start = j; 66 | while ( parent != n && descendants(parent) < relax_columns ) 67 | { 68 | j = parent; 69 | parent = et(j); 70 | } 71 | // Found a supernode in postordered etree, j is the last column 72 | relax_end(snode_start) = j; // Record last column 73 | j++; 74 | // Search for a new leaf 75 | while (descendants(j) != 0 && j < n) j++; 76 | } // End postorder traversal of the etree 77 | 78 | } 79 | 80 | } // end namespace internal 81 | 82 | } // end namespace Eigen 83 | #endif 84 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SparseQR/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:296f16383a95d97fdca76ef137943ee46740646502aaa4e0a86405467bc7948a 3 | size 154 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/StlSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2067753ca954689f81375d919b98615049f94411e9f9c817702e2fe1992ea212 3 | size 159 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/StlSupport/details.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STL_DETAILS_H 12 | #define EIGEN_STL_DETAILS_H 13 | 14 | #ifndef EIGEN_ALIGNED_ALLOCATOR 15 | #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator 16 | #endif 17 | 18 | namespace Eigen { 19 | 20 | // This one is needed to prevent reimplementing the whole std::vector. 21 | template 22 | class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR 23 | { 24 | public: 25 | typedef size_t size_type; 26 | typedef ptrdiff_t difference_type; 27 | typedef T* pointer; 28 | typedef const T* const_pointer; 29 | typedef T& reference; 30 | typedef const T& const_reference; 31 | typedef T value_type; 32 | 33 | template 34 | struct rebind 35 | { 36 | typedef aligned_allocator_indirection other; 37 | }; 38 | 39 | aligned_allocator_indirection() {} 40 | aligned_allocator_indirection(const aligned_allocator_indirection& ) : EIGEN_ALIGNED_ALLOCATOR() {} 41 | aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR& ) {} 42 | template 43 | aligned_allocator_indirection(const aligned_allocator_indirection& ) {} 44 | template 45 | aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR& ) {} 46 | ~aligned_allocator_indirection() {} 47 | }; 48 | 49 | #ifdef _MSC_VER 50 | 51 | // sometimes, MSVC detects, at compile time, that the argument x 52 | // in std::vector::resize(size_t s,T x) won't be aligned and generate an error 53 | // even if this function is never called. Whence this little wrapper. 54 | #define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \ 55 | typename Eigen::internal::conditional< \ 56 | Eigen::internal::is_arithmetic::value, \ 57 | T, \ 58 | Eigen::internal::workaround_msvc_stl_support \ 59 | >::type 60 | 61 | namespace internal { 62 | template struct workaround_msvc_stl_support : public T 63 | { 64 | inline workaround_msvc_stl_support() : T() {} 65 | inline workaround_msvc_stl_support(const T& other) : T(other) {} 66 | inline operator T& () { return *static_cast(this); } 67 | inline operator const T& () const { return *static_cast(this); } 68 | template 69 | inline T& operator=(const OtherT& other) 70 | { T::operator=(other); return *this; } 71 | inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other) 72 | { T::operator=(other); return *this; } 73 | }; 74 | } 75 | 76 | #else 77 | 78 | #define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T 79 | 80 | #endif 81 | 82 | } 83 | 84 | #endif // EIGEN_STL_DETAILS_H 85 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/SuperLUSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:7923cfae9b44cebe64fb3ec9da736785ff5fc50de97e4bd4f1cf801033d067f5 3 | size 172 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/UmfPackSupport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:ecb71a0e12ff6a0aaa4130d7ba0d666fea664e64310aa1f64ecb96a458e2b42b 3 | size 172 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/misc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:73e7ee97b8d8e0697906ed70456cc94bfdf73cceb8f8344afc82759ebbb7ef9d 3 | size 141 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/misc/Image.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MISC_IMAGE_H 11 | #define EIGEN_MISC_IMAGE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | /** \class image_retval_base 18 | * 19 | */ 20 | template 21 | struct traits > 22 | { 23 | typedef typename DecompositionType::MatrixType MatrixType; 24 | typedef Matrix< 25 | typename MatrixType::Scalar, 26 | MatrixType::RowsAtCompileTime, // the image is a subspace of the destination space, whose 27 | // dimension is the number of rows of the original matrix 28 | Dynamic, // we don't know at compile time the dimension of the image (the rank) 29 | MatrixType::Options, 30 | MatrixType::MaxRowsAtCompileTime, // the image matrix will consist of columns from the original matrix, 31 | MatrixType::MaxColsAtCompileTime // so it has the same number of rows and at most as many columns. 32 | > ReturnType; 33 | }; 34 | 35 | template struct image_retval_base 36 | : public ReturnByValue > 37 | { 38 | typedef _DecompositionType DecompositionType; 39 | typedef typename DecompositionType::MatrixType MatrixType; 40 | typedef ReturnByValue Base; 41 | typedef typename Base::Index Index; 42 | 43 | image_retval_base(const DecompositionType& dec, const MatrixType& originalMatrix) 44 | : m_dec(dec), m_rank(dec.rank()), 45 | m_cols(m_rank == 0 ? 1 : m_rank), 46 | m_originalMatrix(originalMatrix) 47 | {} 48 | 49 | inline Index rows() const { return m_dec.rows(); } 50 | inline Index cols() const { return m_cols; } 51 | inline Index rank() const { return m_rank; } 52 | inline const DecompositionType& dec() const { return m_dec; } 53 | inline const MatrixType& originalMatrix() const { return m_originalMatrix; } 54 | 55 | template inline void evalTo(Dest& dst) const 56 | { 57 | static_cast*>(this)->evalTo(dst); 58 | } 59 | 60 | protected: 61 | const DecompositionType& m_dec; 62 | Index m_rank, m_cols; 63 | const MatrixType& m_originalMatrix; 64 | }; 65 | 66 | } // end namespace internal 67 | 68 | #define EIGEN_MAKE_IMAGE_HELPERS(DecompositionType) \ 69 | typedef typename DecompositionType::MatrixType MatrixType; \ 70 | typedef typename MatrixType::Scalar Scalar; \ 71 | typedef typename MatrixType::RealScalar RealScalar; \ 72 | typedef typename MatrixType::Index Index; \ 73 | typedef Eigen::internal::image_retval_base Base; \ 74 | using Base::dec; \ 75 | using Base::originalMatrix; \ 76 | using Base::rank; \ 77 | using Base::rows; \ 78 | using Base::cols; \ 79 | image_retval(const DecompositionType& dec, const MatrixType& originalMatrix) \ 80 | : Base(dec, originalMatrix) {} 81 | 82 | } // end namespace Eigen 83 | 84 | #endif // EIGEN_MISC_IMAGE_H 85 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/misc/Kernel.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MISC_KERNEL_H 11 | #define EIGEN_MISC_KERNEL_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | /** \class kernel_retval_base 18 | * 19 | */ 20 | template 21 | struct traits > 22 | { 23 | typedef typename DecompositionType::MatrixType MatrixType; 24 | typedef Matrix< 25 | typename MatrixType::Scalar, 26 | MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix" 27 | // is the number of cols of the original matrix 28 | // so that the product "matrix * kernel = zero" makes sense 29 | Dynamic, // we don't know at compile-time the dimension of the kernel 30 | MatrixType::Options, 31 | MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter 32 | MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space, 33 | // whose dimension is the number of columns of the original matrix 34 | > ReturnType; 35 | }; 36 | 37 | template struct kernel_retval_base 38 | : public ReturnByValue > 39 | { 40 | typedef _DecompositionType DecompositionType; 41 | typedef ReturnByValue Base; 42 | typedef typename Base::Index Index; 43 | 44 | kernel_retval_base(const DecompositionType& dec) 45 | : m_dec(dec), 46 | m_rank(dec.rank()), 47 | m_cols(m_rank==dec.cols() ? 1 : dec.cols() - m_rank) 48 | {} 49 | 50 | inline Index rows() const { return m_dec.cols(); } 51 | inline Index cols() const { return m_cols; } 52 | inline Index rank() const { return m_rank; } 53 | inline const DecompositionType& dec() const { return m_dec; } 54 | 55 | template inline void evalTo(Dest& dst) const 56 | { 57 | static_cast*>(this)->evalTo(dst); 58 | } 59 | 60 | protected: 61 | const DecompositionType& m_dec; 62 | Index m_rank, m_cols; 63 | }; 64 | 65 | } // end namespace internal 66 | 67 | #define EIGEN_MAKE_KERNEL_HELPERS(DecompositionType) \ 68 | typedef typename DecompositionType::MatrixType MatrixType; \ 69 | typedef typename MatrixType::Scalar Scalar; \ 70 | typedef typename MatrixType::RealScalar RealScalar; \ 71 | typedef typename MatrixType::Index Index; \ 72 | typedef Eigen::internal::kernel_retval_base Base; \ 73 | using Base::dec; \ 74 | using Base::rank; \ 75 | using Base::rows; \ 76 | using Base::cols; \ 77 | kernel_retval(const DecompositionType& dec) : Base(dec) {} 78 | 79 | } // end namespace Eigen 80 | 81 | #endif // EIGEN_MISC_KERNEL_H 82 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/misc/Solve.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MISC_SOLVE_H 11 | #define EIGEN_MISC_SOLVE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | /** \class solve_retval_base 18 | * 19 | */ 20 | template 21 | struct traits > 22 | { 23 | typedef typename DecompositionType::MatrixType MatrixType; 24 | typedef Matrix ReturnType; 30 | }; 31 | 32 | template struct solve_retval_base 33 | : public ReturnByValue > 34 | { 35 | typedef typename remove_all::type RhsNestedCleaned; 36 | typedef _DecompositionType DecompositionType; 37 | typedef ReturnByValue Base; 38 | typedef typename Base::Index Index; 39 | 40 | solve_retval_base(const DecompositionType& dec, const Rhs& rhs) 41 | : m_dec(dec), m_rhs(rhs) 42 | {} 43 | 44 | inline Index rows() const { return m_dec.cols(); } 45 | inline Index cols() const { return m_rhs.cols(); } 46 | inline const DecompositionType& dec() const { return m_dec; } 47 | inline const RhsNestedCleaned& rhs() const { return m_rhs; } 48 | 49 | template inline void evalTo(Dest& dst) const 50 | { 51 | static_cast*>(this)->evalTo(dst); 52 | } 53 | 54 | protected: 55 | const DecompositionType& m_dec; 56 | typename Rhs::Nested m_rhs; 57 | }; 58 | 59 | } // end namespace internal 60 | 61 | #define EIGEN_MAKE_SOLVE_HELPERS(DecompositionType,Rhs) \ 62 | typedef typename DecompositionType::MatrixType MatrixType; \ 63 | typedef typename MatrixType::Scalar Scalar; \ 64 | typedef typename MatrixType::RealScalar RealScalar; \ 65 | typedef typename MatrixType::Index Index; \ 66 | typedef Eigen::internal::solve_retval_base Base; \ 67 | using Base::dec; \ 68 | using Base::rhs; \ 69 | using Base::rows; \ 70 | using Base::cols; \ 71 | solve_retval(const DecompositionType& dec, const Rhs& rhs) \ 72 | : Base(dec, rhs) {} 73 | 74 | } // end namespace Eigen 75 | 76 | #endif // EIGEN_MISC_SOLVE_H 77 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:12d1faa5d66e9696b5ddb7488920926eadf60d470b2c143ad3f0b12fe16027e3 3 | size 150 4 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/plugins/CommonCwiseBinaryOps.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | // This file is a base class plugin containing common coefficient wise functions. 12 | 13 | /** \returns an expression of the difference of \c *this and \a other 14 | * 15 | * \note If you want to substract a given scalar from all coefficients, see Cwise::operator-(). 16 | * 17 | * \sa class CwiseBinaryOp, operator-=() 18 | */ 19 | EIGEN_MAKE_CWISE_BINARY_OP(operator-,internal::scalar_difference_op) 20 | 21 | /** \returns an expression of the sum of \c *this and \a other 22 | * 23 | * \note If you want to add a given scalar to all coefficients, see Cwise::operator+(). 24 | * 25 | * \sa class CwiseBinaryOp, operator+=() 26 | */ 27 | EIGEN_MAKE_CWISE_BINARY_OP(operator+,internal::scalar_sum_op) 28 | 29 | /** \returns an expression of a custom coefficient-wise operator \a func of *this and \a other 30 | * 31 | * The template parameter \a CustomBinaryOp is the type of the functor 32 | * of the custom operator (see class CwiseBinaryOp for an example) 33 | * 34 | * Here is an example illustrating the use of custom functors: 35 | * \include class_CwiseBinaryOp.cpp 36 | * Output: \verbinclude class_CwiseBinaryOp.out 37 | * 38 | * \sa class CwiseBinaryOp, operator+(), operator-(), cwiseProduct() 39 | */ 40 | template 41 | EIGEN_STRONG_INLINE const CwiseBinaryOp 42 | binaryExpr(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other, const CustomBinaryOp& func = CustomBinaryOp()) const 43 | { 44 | return CwiseBinaryOp(derived(), other.derived(), func); 45 | } 46 | 47 | -------------------------------------------------------------------------------- /orbslamios/eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | // This file is a base class plugin containing matrix specifics coefficient wise functions. 12 | 13 | /** \returns an expression of the coefficient-wise absolute value of \c *this 14 | * 15 | * Example: \include MatrixBase_cwiseAbs.cpp 16 | * Output: \verbinclude MatrixBase_cwiseAbs.out 17 | * 18 | * \sa cwiseAbs2() 19 | */ 20 | EIGEN_STRONG_INLINE const CwiseUnaryOp, const Derived> 21 | cwiseAbs() const { return derived(); } 22 | 23 | /** \returns an expression of the coefficient-wise squared absolute value of \c *this 24 | * 25 | * Example: \include MatrixBase_cwiseAbs2.cpp 26 | * Output: \verbinclude MatrixBase_cwiseAbs2.out 27 | * 28 | * \sa cwiseAbs() 29 | */ 30 | EIGEN_STRONG_INLINE const CwiseUnaryOp, const Derived> 31 | cwiseAbs2() const { return derived(); } 32 | 33 | /** \returns an expression of the coefficient-wise square root of *this. 34 | * 35 | * Example: \include MatrixBase_cwiseSqrt.cpp 36 | * Output: \verbinclude MatrixBase_cwiseSqrt.out 37 | * 38 | * \sa cwisePow(), cwiseSquare() 39 | */ 40 | inline const CwiseUnaryOp, const Derived> 41 | cwiseSqrt() const { return derived(); } 42 | 43 | /** \returns an expression of the coefficient-wise inverse of *this. 44 | * 45 | * Example: \include MatrixBase_cwiseInverse.cpp 46 | * Output: \verbinclude MatrixBase_cwiseInverse.out 47 | * 48 | * \sa cwiseProduct() 49 | */ 50 | inline const CwiseUnaryOp, const Derived> 51 | cwiseInverse() const { return derived(); } 52 | 53 | -------------------------------------------------------------------------------- /orbslamios/icon.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/orbslamios/icon.jpg -------------------------------------------------------------------------------- /orbslamios/main.m: -------------------------------------------------------------------------------- 1 | // 2 | // main.m 3 | // orbslamios 4 | // 5 | // Created by Ying Gaoxuan on 16/11/1. 6 | // Copyright © 2016年 Ying Gaoxuan. All rights reserved. 7 | // 8 | 9 | #import 10 | #import "AppDelegate.h" 11 | 12 | int main(int argc, char * argv[]) { 13 | @autoreleasepool { 14 | return UIApplicationMain(argc, argv, nil, NSStringFromClass([AppDelegate class])); 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /orbslamiosTests/Info.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | CFBundleDevelopmentRegion 6 | en 7 | CFBundleExecutable 8 | $(EXECUTABLE_NAME) 9 | CFBundleIdentifier 10 | $(PRODUCT_BUNDLE_IDENTIFIER) 11 | CFBundleInfoDictionaryVersion 12 | 6.0 13 | CFBundleName 14 | $(PRODUCT_NAME) 15 | CFBundlePackageType 16 | BNDL 17 | CFBundleShortVersionString 18 | 1.0 19 | CFBundleVersion 20 | 1 21 | 22 | 23 | -------------------------------------------------------------------------------- /orbslamiosTests/orbslamiosTests.m: -------------------------------------------------------------------------------- 1 | // 2 | // orbslamiosTests.m 3 | // orbslamiosTests 4 | // 5 | // Created by Ying Gaoxuan on 16/11/1. 6 | // Copyright © 2016年 Ying Gaoxuan. All rights reserved. 7 | // 8 | 9 | #import 10 | 11 | @interface orbslamiosTests : XCTestCase 12 | 13 | @end 14 | 15 | @implementation orbslamiosTests 16 | 17 | - (void)setUp { 18 | [super setUp]; 19 | // Put setup code here. This method is called before the invocation of each test method in the class. 20 | } 21 | 22 | - (void)tearDown { 23 | // Put teardown code here. This method is called after the invocation of each test method in the class. 24 | [super tearDown]; 25 | } 26 | 27 | - (void)testExample { 28 | // This is an example of a functional test case. 29 | // Use XCTAssert and related functions to verify your tests produce the correct results. 30 | } 31 | 32 | - (void)testPerformanceExample { 33 | // This is an example of a performance test case. 34 | [self measureBlock:^{ 35 | // Put the code you want to measure the time of here. 36 | }]; 37 | } 38 | 39 | @end 40 | -------------------------------------------------------------------------------- /orbslamiosUITests/Info.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | CFBundleDevelopmentRegion 6 | en 7 | CFBundleExecutable 8 | $(EXECUTABLE_NAME) 9 | CFBundleIdentifier 10 | $(PRODUCT_BUNDLE_IDENTIFIER) 11 | CFBundleInfoDictionaryVersion 12 | 6.0 13 | CFBundleName 14 | $(PRODUCT_NAME) 15 | CFBundlePackageType 16 | BNDL 17 | CFBundleShortVersionString 18 | 1.0 19 | CFBundleVersion 20 | 1 21 | 22 | 23 | -------------------------------------------------------------------------------- /orbslamiosUITests/orbslamiosUITests.m: -------------------------------------------------------------------------------- 1 | // 2 | // orbslamiosUITests.m 3 | // orbslamiosUITests 4 | // 5 | // Created by Ying Gaoxuan on 16/11/1. 6 | // Copyright © 2016年 Ying Gaoxuan. All rights reserved. 7 | // 8 | 9 | #import 10 | 11 | @interface orbslamiosUITests : XCTestCase 12 | 13 | @end 14 | 15 | @implementation orbslamiosUITests 16 | 17 | - (void)setUp { 18 | [super setUp]; 19 | 20 | // Put setup code here. This method is called before the invocation of each test method in the class. 21 | 22 | // In UI tests it is usually best to stop immediately when a failure occurs. 23 | self.continueAfterFailure = NO; 24 | // UI tests must launch the application that they test. Doing this in setup will make sure it happens for each test method. 25 | [[[XCUIApplication alloc] init] launch]; 26 | 27 | // In UI tests it’s important to set the initial state - such as interface orientation - required for your tests before they run. The setUp method is a good place to do this. 28 | } 29 | 30 | - (void)tearDown { 31 | // Put teardown code here. This method is called after the invocation of each test method in the class. 32 | [super tearDown]; 33 | } 34 | 35 | - (void)testExample { 36 | // Use recording to get started writing UI tests. 37 | // Use XCTAssert and related functions to verify your tests produce the correct results. 38 | } 39 | 40 | @end 41 | -------------------------------------------------------------------------------- /ygxslam/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/ygxslam/.DS_Store -------------------------------------------------------------------------------- /ygxslam/ORBvoc.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ygx2011/ORB_SLAM2-IOS/31b6ef6e13424e3a0df984b17ea379c4249ef41a/ygxslam/ORBvoc.bin -------------------------------------------------------------------------------- /ygxslam/Settings.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 549.991 9 | Camera.fy: 551.34 10 | Camera.cx: 318.32 11 | Camera.cy: 237.415 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 | # stereo baseline times fx 25 | Camera.bf: 2150 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: 100 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 800 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 | --------------------------------------------------------------------------------